From bc3ac6ad72ff7da7ca4182df2943d589f6b8c1b9 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 25 Sep 2026 14:53:19 -0500 Subject: [PATCH] Lateral Integrate torque feedback, lane-change smoothing and low-speed Turn Assist through dedicated lateral owners. Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com> Co-authored-by: pharmacomaniac --- .../car/gm/tests/test_ordinary_camera.py | 196 + .../gm/tests/test_ordinary_camera_removed.py | 117 + .../car/gm/tests/test_ordinary_sdgm.py | 94 + .../controls/lib/latcontrol_torque.py | 14 +- openpilot/selfdrive/locationd/lagd.py | 4 +- openpilot/selfdrive/locationd/paramsd.py | 4 +- openpilot/starpilot/lateral/LICENSE | 8 + openpilot/starpilot/lateral/README.md | 79 + openpilot/starpilot/lateral/TORQUE.md | 69 + openpilot/starpilot/lateral/__init__.py | 1 + .../lateral/adjacent_lane_evidence.py | 57 + openpilot/starpilot/lateral/ascm_policy.py | 24 + .../starpilot/lateral/auto_lane_change.py | 117 + openpilot/starpilot/lateral/bolt_policy.py | 175 + openpilot/starpilot/lateral/bolt_shaping.py | 368 ++ openpilot/starpilot/lateral/camera_policy.py | 51 + .../starpilot/lateral/controller_selection.py | 252 ++ .../starpilot/lateral/corolla_tss2_policy.py | 194 + openpilot/starpilot/lateral/gain_runtime.py | 68 + .../starpilot/lateral/genesis_g70_policy.py | 436 +++ .../starpilot/lateral/genesis_gv70_policy.py | 159 + .../starpilot/lateral/gm_geometry_runtime.py | 83 + .../starpilot/lateral/gm_ordinary_policy.py | 105 + openpilot/starpilot/lateral/gv70_shaping.py | 272 ++ .../starpilot/lateral/hkg_torque_policy.py | 129 + .../starpilot/lateral/hkg_vehicle_policy.py | 178 + .../starpilot/lateral/hkg_vehicle_shaping.py | 1354 +++++++ openpilot/starpilot/lateral/ioniq6_policy.py | 897 +++++ openpilot/starpilot/lateral/lane_centering.py | 275 ++ .../lateral/lane_change_preferences.py | 204 ++ .../lateral/lane_change_smoothing.py | 65 + .../lateral/lane_change_status_wire.py | 132 + openpilot/starpilot/lateral/lane_feedback.py | 50 + openpilot/starpilot/lateral/lane_runtime.py | 85 + .../starpilot/lateral/learning_preferences.py | 88 + .../starpilot/lateral/low_speed_advisory.py | 29 + .../starpilot/lateral/model_turn_assist.py | 259 ++ .../starpilot/lateral/ordinary_cc_policy.py | 36 + .../starpilot/lateral/palisade_policy.py | 26 + .../starpilot/lateral/palisade_shaping.py | 154 + openpilot/starpilot/lateral/pause.py | 90 + openpilot/starpilot/lateral/sdgm_policy.py | 34 + .../starpilot/lateral/sonata_hybrid_policy.py | 24 + .../lateral/sonata_hybrid_shaping.py | 142 + .../starpilot/lateral/suburban_policy.py | 24 + openpilot/starpilot/lateral/tests/__init__.py | 0 .../fixtures/adjacent_lane_geometry.json | 39 + .../tests/fixtures/corolla_tss2_ba901b5f.json | 1 + .../tests/fixtures/genesis_g70_ba901b5f.json | 1 + .../lateral/tests/fixtures/gv70_385428f6.json | 3198 +++++++++++++++++ .../lateral/tests/fixtures/oct2_turns.json | 1 + .../lateral/tests/test_auto_lane_change.py | 214 ++ .../tests/test_bolt_torque_admission.py | 95 + .../lateral/tests/test_bolt_torque_policy.py | 116 + .../tests/test_controller_selection.py | 171 + .../lateral/tests/test_corolla_tss2_policy.py | 74 + .../lateral/tests/test_ev6_default.py | 134 + .../lateral/tests/test_genesis_g70_policy.py | 211 ++ .../lateral/tests/test_genesis_gv70_policy.py | 146 + .../tests/test_genesis_torque_context.py | 74 + .../lateral/tests/test_hkg_centre_context.py | 111 + .../tests/test_hkg_highway_friction.py | 156 + .../lateral/tests/test_hkg_torque_context.py | 119 + .../lateral/tests/test_hkg_vehicle_policy.py | 138 + .../tests/test_ioniq6_center_assist.py | 44 + .../tests/test_ioniq6_firmware_buffers.py | 70 + .../lateral/tests/test_ioniq6_policy.py | 159 + .../lateral/tests/test_lane_centering.py | 322 ++ .../tests/test_lane_change_defaults.py | 22 + .../tests/test_lane_change_smoothing.py | 78 + .../lateral/tests/test_lane_feedback.py | 83 + .../lateral/tests/test_lane_runtime.py | 408 +++ .../lateral/tests/test_palisade_policy.py | 87 + .../tests/test_sonata_hybrid_policy.py | 48 + .../lateral/tests/test_torque_application.py | 103 + .../lateral/tests/test_torque_family.py | 169 + .../lateral/tests/test_torque_learning.py | 275 ++ .../lateral/tests/test_torque_runtime.py | 291 ++ .../lateral/tests/test_torque_settings.py | 86 + .../lateral/tests/test_torque_tuning.py | 128 + .../tests/test_turn_lane_change_smoothing.py | 89 + .../lateral/tests/test_turn_model_runtime.py | 88 + .../lateral/tests/testdata/README.md | 8 + .../tests/testdata/ioniq6_frozen_2023.json | 208 ++ .../tests/testdata/ioniq6_frozen_2025.json | 208 ++ .../tests/testdata/non_ioniq_parent.json | 198 + .../starpilot/lateral/torque_extension.py | 102 + openpilot/starpilot/lateral/torque_runtime.py | 234 ++ .../starpilot/lateral/torque_settings.py | 285 ++ openpilot/starpilot/lateral/torque_shaping.py | 50 + .../starpilot/lateral/torque_supported.py | 63 + openpilot/starpilot/lateral/torque_tuning.py | 82 + .../starpilot/lateral/torque_turn_assist.py | 21 + openpilot/starpilot/lateral/volt_policy.py | 53 + 94 files changed, 16280 insertions(+), 3 deletions(-) create mode 100644 opendbc_repo/opendbc/car/gm/tests/test_ordinary_camera.py create mode 100644 opendbc_repo/opendbc/car/gm/tests/test_ordinary_camera_removed.py create mode 100644 opendbc_repo/opendbc/car/gm/tests/test_ordinary_sdgm.py create mode 100644 openpilot/starpilot/lateral/LICENSE create mode 100644 openpilot/starpilot/lateral/README.md create mode 100644 openpilot/starpilot/lateral/TORQUE.md create mode 100644 openpilot/starpilot/lateral/__init__.py create mode 100644 openpilot/starpilot/lateral/adjacent_lane_evidence.py create mode 100644 openpilot/starpilot/lateral/ascm_policy.py create mode 100644 openpilot/starpilot/lateral/auto_lane_change.py create mode 100644 openpilot/starpilot/lateral/bolt_policy.py create mode 100644 openpilot/starpilot/lateral/bolt_shaping.py create mode 100644 openpilot/starpilot/lateral/camera_policy.py create mode 100644 openpilot/starpilot/lateral/controller_selection.py create mode 100644 openpilot/starpilot/lateral/corolla_tss2_policy.py create mode 100644 openpilot/starpilot/lateral/gain_runtime.py create mode 100644 openpilot/starpilot/lateral/genesis_g70_policy.py create mode 100644 openpilot/starpilot/lateral/genesis_gv70_policy.py create mode 100644 openpilot/starpilot/lateral/gm_geometry_runtime.py create mode 100644 openpilot/starpilot/lateral/gm_ordinary_policy.py create mode 100644 openpilot/starpilot/lateral/gv70_shaping.py create mode 100644 openpilot/starpilot/lateral/hkg_torque_policy.py create mode 100644 openpilot/starpilot/lateral/hkg_vehicle_policy.py create mode 100644 openpilot/starpilot/lateral/hkg_vehicle_shaping.py create mode 100644 openpilot/starpilot/lateral/ioniq6_policy.py create mode 100644 openpilot/starpilot/lateral/lane_centering.py create mode 100644 openpilot/starpilot/lateral/lane_change_preferences.py create mode 100644 openpilot/starpilot/lateral/lane_change_smoothing.py create mode 100644 openpilot/starpilot/lateral/lane_change_status_wire.py create mode 100644 openpilot/starpilot/lateral/lane_feedback.py create mode 100644 openpilot/starpilot/lateral/lane_runtime.py create mode 100644 openpilot/starpilot/lateral/learning_preferences.py create mode 100644 openpilot/starpilot/lateral/low_speed_advisory.py create mode 100644 openpilot/starpilot/lateral/model_turn_assist.py create mode 100644 openpilot/starpilot/lateral/ordinary_cc_policy.py create mode 100644 openpilot/starpilot/lateral/palisade_policy.py create mode 100644 openpilot/starpilot/lateral/palisade_shaping.py create mode 100644 openpilot/starpilot/lateral/pause.py create mode 100644 openpilot/starpilot/lateral/sdgm_policy.py create mode 100644 openpilot/starpilot/lateral/sonata_hybrid_policy.py create mode 100644 openpilot/starpilot/lateral/sonata_hybrid_shaping.py create mode 100644 openpilot/starpilot/lateral/suburban_policy.py create mode 100644 openpilot/starpilot/lateral/tests/__init__.py create mode 100644 openpilot/starpilot/lateral/tests/fixtures/adjacent_lane_geometry.json create mode 100644 openpilot/starpilot/lateral/tests/fixtures/corolla_tss2_ba901b5f.json create mode 100644 openpilot/starpilot/lateral/tests/fixtures/genesis_g70_ba901b5f.json create mode 100644 openpilot/starpilot/lateral/tests/fixtures/gv70_385428f6.json create mode 100644 openpilot/starpilot/lateral/tests/fixtures/oct2_turns.json create mode 100644 openpilot/starpilot/lateral/tests/test_auto_lane_change.py create mode 100644 openpilot/starpilot/lateral/tests/test_bolt_torque_admission.py create mode 100644 openpilot/starpilot/lateral/tests/test_bolt_torque_policy.py create mode 100644 openpilot/starpilot/lateral/tests/test_controller_selection.py create mode 100644 openpilot/starpilot/lateral/tests/test_corolla_tss2_policy.py create mode 100644 openpilot/starpilot/lateral/tests/test_ev6_default.py create mode 100644 openpilot/starpilot/lateral/tests/test_genesis_g70_policy.py create mode 100644 openpilot/starpilot/lateral/tests/test_genesis_gv70_policy.py create mode 100644 openpilot/starpilot/lateral/tests/test_genesis_torque_context.py create mode 100644 openpilot/starpilot/lateral/tests/test_hkg_centre_context.py create mode 100644 openpilot/starpilot/lateral/tests/test_hkg_highway_friction.py create mode 100644 openpilot/starpilot/lateral/tests/test_hkg_torque_context.py create mode 100644 openpilot/starpilot/lateral/tests/test_hkg_vehicle_policy.py create mode 100644 openpilot/starpilot/lateral/tests/test_ioniq6_center_assist.py create mode 100644 openpilot/starpilot/lateral/tests/test_ioniq6_firmware_buffers.py create mode 100644 openpilot/starpilot/lateral/tests/test_ioniq6_policy.py create mode 100644 openpilot/starpilot/lateral/tests/test_lane_centering.py create mode 100644 openpilot/starpilot/lateral/tests/test_lane_change_defaults.py create mode 100644 openpilot/starpilot/lateral/tests/test_lane_change_smoothing.py create mode 100644 openpilot/starpilot/lateral/tests/test_lane_feedback.py create mode 100644 openpilot/starpilot/lateral/tests/test_lane_runtime.py create mode 100644 openpilot/starpilot/lateral/tests/test_palisade_policy.py create mode 100644 openpilot/starpilot/lateral/tests/test_sonata_hybrid_policy.py create mode 100644 openpilot/starpilot/lateral/tests/test_torque_application.py create mode 100644 openpilot/starpilot/lateral/tests/test_torque_family.py create mode 100644 openpilot/starpilot/lateral/tests/test_torque_learning.py create mode 100644 openpilot/starpilot/lateral/tests/test_torque_runtime.py create mode 100644 openpilot/starpilot/lateral/tests/test_torque_settings.py create mode 100644 openpilot/starpilot/lateral/tests/test_torque_tuning.py create mode 100644 openpilot/starpilot/lateral/tests/test_turn_lane_change_smoothing.py create mode 100644 openpilot/starpilot/lateral/tests/test_turn_model_runtime.py create mode 100644 openpilot/starpilot/lateral/tests/testdata/README.md create mode 100644 openpilot/starpilot/lateral/tests/testdata/ioniq6_frozen_2023.json create mode 100644 openpilot/starpilot/lateral/tests/testdata/ioniq6_frozen_2025.json create mode 100644 openpilot/starpilot/lateral/tests/testdata/non_ioniq_parent.json create mode 100644 openpilot/starpilot/lateral/torque_extension.py create mode 100644 openpilot/starpilot/lateral/torque_runtime.py create mode 100644 openpilot/starpilot/lateral/torque_settings.py create mode 100644 openpilot/starpilot/lateral/torque_shaping.py create mode 100644 openpilot/starpilot/lateral/torque_supported.py create mode 100644 openpilot/starpilot/lateral/torque_tuning.py create mode 100644 openpilot/starpilot/lateral/torque_turn_assist.py create mode 100644 openpilot/starpilot/lateral/volt_policy.py diff --git a/opendbc_repo/opendbc/car/gm/tests/test_ordinary_camera.py b/opendbc_repo/opendbc/car/gm/tests/test_ordinary_camera.py new file mode 100644 index 0000000000..a36eac3260 --- /dev/null +++ b/opendbc_repo/opendbc/car/gm/tests/test_ordinary_camera.py @@ -0,0 +1,196 @@ +import unittest + +from opendbc.can import CANPacker +from opendbc.car import Bus, gen_empty_fingerprint, structs +from opendbc.car.gm.interface import CarInterface +from opendbc.car.gm.values import DBC, ORDINARY_CAMERA_CAR, ORDINARY_CAMERA_ALPHA_CAR, is_ordinary_camera_profile + + +def params(identity, *, alpha=False, release=False, camera=True, be=True): + fp = gen_empty_fingerprint() + fp[0].update({0xF1: 6, 0xC9: 8, 0x1C4: 8, 0x184: 8, 0x34A: 5}) + if be: + fp[0][0xBE] = 6 + if not camera: + fp[0][0x1E1] = 7 + if camera: + fp[2].update({0x320: 6, 0x180: 4, 0x370: 6}) + return CarInterface.get_params(identity, fp, [], alpha, release, False) + + +def feed(ci, packer, now, *, cruise=True, gas=False, brake=False, fcw=0, counter=0): + from opendbc.car.gm.tests.test_cc_gateway_stock import pt_frames + frames = pt_frames(packer, gas=gas, counter=counter, acc_cruise=2 if cruise else 0) + frames = [frame for frame in frames if frame[0] != 0xC9] + frames.append(packer.make_can_msg("EBCMBrakePedalPosition", 0, {"BrakePedalPosition": 40 if brake else 0})) + frames.append(packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': 1, 'BrakePressed': int(brake)})) + frames += [packer.make_can_msg('ASCMLKASteeringCmd', 2, {'RollingCounter': counter}), + packer.make_can_msg('AEBCmd', 2, {}), + packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, + {'ACCCruiseState': 2, 'ACCSpeedSetpoint': 80, 'FCWAlert': fcw})] + return ci.update([(now, frames)]), frames + + +class TestOrdinaryCamera(unittest.TestCase): + def test_final_cp_and_default_policy_modes(self): + from opendbc.car.gm.camera import policy_for + from opendbc.car.gm.feature_capabilities import longitudinal_supported, display_supported + from opendbc.car.gm.lateral import lane_centering_supported + from opendbc.car.gm.aol import qualified_gm + from openpilot.starpilot.lateral.controller_selection import policy_for as lateral_policy_for + self.assertEqual(len(ORDINARY_CAMERA_CAR), 6) + for identity in ORDINARY_CAMERA_CAR: + for release in (False, True): + for alpha in (False, True): + cp = params(identity, alpha=alpha, release=release) + long = alpha and not release and identity in ORDINARY_CAMERA_ALPHA_CAR + self.assertEqual(cp.openpilotLongitudinalControl, long) + self.assertEqual(cp.pcmCruise, not long) + self.assertEqual(cp.safetyConfigs[0].safetyParam, 0xC170 if long else 0xC171) + self.assertFalse(cp.dashcamOnly) + self.assertTrue(is_ordinary_camera_profile(cp, longitudinal=long)) + self.assertEqual(policy_for(cp) is not None, long) + self.assertEqual(longitudinal_supported(cp), long) + self.assertTrue(display_supported(cp)) + self.assertTrue(lane_centering_supported(cp)) + self.assertTrue(qualified_gm(cp)) + self.assertEqual(lateral_policy_for(cp), 'ordinary_camera') + absent = params(identity, alpha=alpha, release=release, camera=False) + self.assertFalse(absent.dashcamOnly) + self.assertTrue(is_ordinary_camera_profile(absent, longitudinal=long)) + self.assertEqual(absent.safetyConfigs[0].safetyParam, 0xC173 if long else 0xC172) + + def test_actual_required_sources_and_alt_status_ownership(self): + for identity in ORDINARY_CAMERA_CAR: + cp = params(identity) + ci = CarInterface(cp) + packer = CANPacker(DBC[identity][Bus.pt]) + for tick in range(30): + out, _ = feed(ci, packer, 1_000_000_000 + tick * 10_000_000, fcw=2, counter=tick % 4) + self.assertTrue(out.canValid) + self.assertTrue(out.cruiseState.enabled) + self.assertFalse(out.cruiseState.nonAdaptive) + self.assertAlmostEqual(out.cruiseState.speed, 80 / 3.6, places=5) + self.assertEqual(ci.CS.stock_fcw_alert, 2) + + def test_packed_commands_gas_lateral_and_stock_cancel_route(self): + for identity in ORDINARY_CAMERA_CAR: + for alpha in (False, True): + cp = params(identity, alpha=alpha) + ci = CarInterface(cp) + packer = CANPacker(DBC[identity][Bus.pt]) + emitted_cancel = False + for tick in range(30): + now = 1_000_000_000 + tick * 40_000_000 + out, _ = feed(ci, packer, now, gas=tick >= 12, counter=tick % 4) + self.assertTrue(out.canValid) + cc = structs.CarControl(enabled=True, latActive=True, longActive=cp.openpilotLongitudinalControl and tick < 12) + cc.actuators.torque = .1 + cc.actuators.accel = 1. + cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.pid + cc.cruiseControl.cancel = not cp.openpilotLongitudinalControl + ci.CC.frame = tick * 4 + _, messages = ci.apply(cc.as_reader(), now) + self.assertFalse(any(message[0] in (0x200, 0xBD, 0x1F5, 0x3D1, 0xA1, 0x306, 0x308, 0x310) for message in messages)) + if cp.openpilotLongitudinalControl: + self.assertEqual([message[0] for message in messages if message[0] in (0x2CD, 0x2CB, 0x315, 0x370)], + [0x2CD, 0x2CB, 0x315, 0x370]) + self.assertTrue(all(message[2] == 0 for message in messages if message[0] in (0x2CD, 0x2CB, 0x315, 0x370))) + if tick >= 12: + self.assertEqual((ci.CC.apply_gas, ci.CC.apply_brake), (-500., 0)) + else: + self.assertFalse(any(message[0] in (0x2CD, 0x2CB, 0x315, 0x370) for message in messages)) + for message in messages: + if message[0] == 0x1E1: + emitted_cancel = True + self.assertEqual(message[2], 2) + if tick >= 12: + self.assertNotEqual(ci.CC.apply_torque_last, 0) + if not cp.openpilotLongitudinalControl: + self.assertTrue(emitted_cancel) + + def test_expired_camera_and_main_sources_zero_actual_steering(self): + for alpha in (False, True): + identity = next(iter(ORDINARY_CAMERA_ALPHA_CAR)) + for omitted in ('camera', 'main'): + cp = params(identity, alpha=alpha) + ci = CarInterface(cp) + packer = CANPacker(DBC[identity][Bus.pt]) + for tick in range(30): + now = 1_000_000_000 + tick * 10_000_000 + out, messages = feed(ci, packer, now, counter=tick % 4) + self.assertTrue(out.canValid) + cc = structs.CarControl(enabled=True, latActive=True) + cc.actuators.torque = .1 + ci.CC.frame = 120 + ci.apply(cc.as_reader(), now) + self.assertNotEqual(ci.CC.apply_torque_last, 0) + retained = [message for message in messages if message[2] == 0] if omitted == 'camera' else [ + message for message in messages if message[0] != 0xC9] + for tick in range(1, 80): + stamp = now + tick * 20_000_000 + ci.update([(stamp, retained)]) + ci.CC.frame += 10 + _, emitted = ci.apply(cc.as_reader(), stamp) + self.assertEqual(ci.CC.apply_torque_last, 0) + self.assertTrue(any(message[0] == 0x180 for message in emitted)) + + def test_f1_analog_source_with_and_without_be_keeps_c9_brake_owner(self): + from opendbc.car.gm.values import GMFlags + for identity in ORDINARY_CAMERA_CAR: + for be in (False, True): + for alpha in (False, True): + cp = params(identity, alpha=alpha, be=be) + self.assertEqual(bool(cp.flags & GMFlags.NO_ACCELERATOR_POS_MSG), not be) + ci = CarInterface(cp) + packer = CANPacker(DBC[identity][Bus.pt]) + for tick in range(30): + now = 1_000_000_000 + tick * 10_000_000 + _, messages = feed(ci, packer, now, counter=tick % 4) + messages = [message for message in messages if message[0] not in (0xC9, 0xF1) and (be or message[0] != 0xBE)] + messages += [packer.make_can_msg('EBCMBrakePedalPosition', 0, {'BrakePedalPosition': 80}), + packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': 1, 'BrakePressed': 0})] + out = ci.update([(now + 1, messages)]) + self.assertTrue(out.canValid) + self.assertFalse(out.brakePressed) + messages = [message for message in messages if message[0] not in (0xC9, 0xF1)] + messages += [packer.make_can_msg('EBCMBrakePedalPosition', 0, {'BrakePedalPosition': 0}), + packer.make_can_msg('ECMEngineStatus', 0, {'CruiseMainOn': 1, 'BrakePressed': 1})] + out = ci.update([(now + 10_000_000, messages)]) + self.assertTrue(out.brakePressed) + + def test_actual_card_preserves_final_camera_owner(self): + import os + from unittest.mock import patch + from openpilot.common.params import Params + from openpilot.common.prefix import OpenpilotPrefix + from openpilot.selfdrive.car.card import Car + from opendbc.car.gm.radar_interface import RadarInterface + for identity in ORDINARY_CAMERA_CAR: + with OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '1'}): + saved = Params() + saved.put_bool('OpenpilotEnabledToggle', True, block=True) + saved.put_bool('AlwaysOnLateral', True, block=True) + cp = params(identity, alpha=True, be=False) + card = Car(CI=CarInterface(cp), RI=RadarInterface(cp)) + long = identity in ORDINARY_CAMERA_ALPHA_CAR + self.assertEqual(card.CP.openpilotLongitudinalControl, long) + self.assertEqual(card.CP.pcmCruise, not long) + self.assertEqual(card.CP.safetyConfigs[0].safetyParam, 0xC170 if long else 0xC171) + self.assertTrue(is_ordinary_camera_profile(card.CP, longitudinal=long)) + + def test_declared_f1_source_recipe_matches_selected_analog_owner(self): + from unittest.mock import patch + from opendbc.car.gm import carstate + real_parser = carstate.CANParser + for be in (False, True): + recipes = {} + def make_parser(dbc, messages, bus, recipes=recipes): + recipes[bus] = list(messages) + return real_parser(dbc, messages, bus) + cp = params(next(iter(ORDINARY_CAMERA_ALPHA_CAR)), alpha=True, be=be) + with patch.object(carstate, 'CANParser', side_effect=make_parser): + CarInterface(cp) + self.assertEqual([frequency for name, frequency in recipes[0] if name == 'EBCMBrakePedalPosition'], + [10 if be else 100]) + self.assertEqual(any(name == 'ECMAcceleratorPos' for name, _ in recipes[0]), be) diff --git a/opendbc_repo/opendbc/car/gm/tests/test_ordinary_camera_removed.py b/opendbc_repo/opendbc/car/gm/tests/test_ordinary_camera_removed.py new file mode 100644 index 0000000000..3342b5fe42 --- /dev/null +++ b/opendbc_repo/opendbc/car/gm/tests/test_ordinary_camera_removed.py @@ -0,0 +1,117 @@ +"""Observed PT source contract for ordinary GM camera-absent layouts.""" +import unittest +import os +from unittest.mock import patch + +from opendbc.can import CANPacker +from opendbc.car import Bus, gen_empty_fingerprint, structs +from opendbc.car.gm.interface import CarInterface +from opendbc.car.gm.values import DBC, GMFlags, ORDINARY_CAMERA_CAR, ORDINARY_CAMERA_ALPHA_CAR, is_ordinary_camera_profile + + +def params(identity, *, alpha=False, release=False, analog=True): + fp = gen_empty_fingerprint() + fp[0].update({0xF1: 6, 0xC9: 8, 0x1C4: 8, 0x184: 8, 0x34A: 5, 0x1E1: 7}) + if analog: + fp[0][0xBE] = 6 + return CarInterface.get_params(identity, fp, [], alpha, release, False) + + +def feed(ci, packer, now, *, cruise=True, main=True, gas=False, brake=False, counter=0): + from opendbc.car.gm.tests.test_cc_gateway_stock import pt_frames + frames = pt_frames(packer, main=main, gas=gas, brake=brake, counter=counter, acc_cruise=2 if cruise else 0) + if ci.CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG: + frames = [message for message in frames if message[0] != 0xBE] + frames.append(packer.make_can_msg('EBCMBrakePedalPosition', 0, {'BrakePedalPosition': 40 if brake else 0})) + frames.append(packer.make_can_msg('ASCMLKASteeringCmd', 128, {'RollingCounter': counter})) + return ci.update([(now, frames)]), frames + + +class TestOrdinaryCameraRemoved(unittest.TestCase): + def test_final_profile_shared_owners_and_absent_camera_state(self): + from opendbc.car.gm.aol import qualified_gm + from opendbc.car.gm.lateral import lane_centering_supported + from opendbc.car.gm.feature_capabilities import longitudinal_supported + from openpilot.starpilot.lateral.controller_selection import policy_for + for identity in ORDINARY_CAMERA_CAR: + for analog in (False, True): + for release in (False, True): + for alpha in (False, True): + cp = params(identity, alpha=alpha, release=release, analog=analog) + long = alpha and not release and identity in ORDINARY_CAMERA_ALPHA_CAR + self.assertTrue(is_ordinary_camera_profile(cp, longitudinal=long)) + self.assertTrue(cp.flags & GMFlags.NO_CAMERA) + self.assertEqual(cp.safetyConfigs[0].safetyParam, 0xC173 if long else 0xC172) + self.assertEqual(cp.openpilotLongitudinalControl, long) + self.assertTrue(qualified_gm(cp)) + self.assertTrue(lane_centering_supported(cp)) + self.assertEqual(longitudinal_supported(cp), long) + self.assertIsNotNone(policy_for(cp)) + ci = CarInterface(cp) + packer = CANPacker(DBC[identity][Bus.pt]) + for tick in range(12): + out, _ = feed(ci, packer, 1_000_000_000 + tick * 10_000_000, counter=tick % 4) + self.assertTrue(out.canValid) + self.assertTrue(out.cruiseState.enabled) + self.assertEqual(out.cruiseState.speed, 0.) + self.assertFalse(out.stockAeb or out.stockFcw or out.cruiseState.nonAdaptive) + + def test_actual_card_and_malformed_or_missing_startup_sources(self): + from openpilot.common.params import Params + from openpilot.common.prefix import OpenpilotPrefix + from openpilot.selfdrive.car.card import Car + from opendbc.car.gm.radar_interface import RadarInterface + from opendbc.car.gm.values import is_ordinary_camera_removed + self.assertFalse(is_ordinary_camera_removed(None)) + for identity in ORDINARY_CAMERA_CAR: + with OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '1'}): + saved = Params() + saved.put_bool('OpenpilotEnabledToggle', True, block=True) + cp = params(identity, alpha=True) + card = Car(CI=CarInterface(cp), RI=RadarInterface(cp)) + self.assertTrue(is_ordinary_camera_removed(card.CP)) + self.assertEqual(card.CP.safetyConfigs[0].safetyParam, cp.safetyConfigs[0].safetyParam) + for missing in (0xF1, 0xC9, 0x1C4, 0x184, 0x34A, 0x1E1): + fp = gen_empty_fingerprint() + fp[0].update({0xF1: 6, 0xC9: 8, 0x1C4: 8, 0x184: 8, 0x34A: 5, 0x1E1: 7}) + fp[0].pop(missing) + denied = CarInterface.get_params(identity, fp, [], True, False, False) + self.assertTrue(denied.dashcamOnly) + fp[0][missing] = 7 + fp[2][0x320] = 5 + self.assertTrue(CarInterface.get_params(identity, fp, [], True, False, False).dashcamOnly) + + def test_actual_controller_routes_and_source_withdrawal(self): + for identity in ORDINARY_CAMERA_CAR: + for alpha in (False, True): + cp = params(identity, alpha=alpha) + ci = CarInterface(cp) + packer = CANPacker(DBC[identity][Bus.pt]) + seen_cancel = False + for tick in range(112): + now = 1_000_000_000 + tick * 10_000_000 + out, _ = feed(ci, packer, now, gas=tick >= 50, counter=tick % 4) + self.assertTrue(out.canValid) + cc = structs.CarControl(enabled=True, latActive=True, longActive=cp.openpilotLongitudinalControl) + cc.actuators.torque = .1 + cc.actuators.accel = 1. + cc.cruiseControl.cancel = True + _, emitted = ci.apply(cc.as_reader(), now) + ids = [m[0] for m in emitted] + self.assertNotIn(0x2CD, ids) + self.assertFalse(any(i in ids for i in (0x200, 0xBD, 0x1F5, 0x3D1))) + self.assertEqual(any(i in ids for i in (0x409, 0x40A)), cp.openpilotLongitudinalControl and tick % 100 == 0) + if 0x409 in ids: + self.assertLess(ids.index(0x409), ids.index(0x184)) + for address, payload, bus in emitted: + if address == 0x1E1: + seen_cancel = True + self.assertEqual(bus, 2) + self.assertEqual(payload[4] & 3, tick % 4) + if not cp.openpilotLongitudinalControl: + self.assertFalse(any(i in ids for i in (0x2CB, 0x315, 0x370, 0x409, 0x40A))) + self.assertEqual(seen_cancel, not cp.openpilotLongitudinalControl) + ci.CS.ordinary_removed_sources = (0,) * 6 + ci.CC.frame = 112 + applied, _ = ci.apply(cc.as_reader(), now + 10_000_000) + self.assertEqual(applied.torqueOutputCan, 0) diff --git a/opendbc_repo/opendbc/car/gm/tests/test_ordinary_sdgm.py b/opendbc_repo/opendbc/car/gm/tests/test_ordinary_sdgm.py new file mode 100644 index 0000000000..9f21a569fe --- /dev/null +++ b/opendbc_repo/opendbc/car/gm/tests/test_ordinary_sdgm.py @@ -0,0 +1,94 @@ +import unittest + +from opendbc.car.gm.tests.test_ascm_intercept import params +from opendbc.car.gm.values import CAR, ORDINARY_SDGM_CAR, SDGM_CANCEL_PT_CAR, CarControllerParams, is_ordinary_sdgm_profile +from opendbc.car.gm.longitudinal import sdgm_policy_for +from opendbc.car.gm.ordinary import demands + + +class TestOrdinarySdgm(unittest.TestCase): + def test_exact_final_configuration(self): + self.assertEqual(len(ORDINARY_SDGM_CAR), 7) + for candidate in ORDINARY_SDGM_CAR: + for release in (False, True): + for alpha in (False, True): + for sascm in (False, True): + for c9 in (False, True): + cp = params(candidate, sascm=sascm, accelerator=not c9, alpha=alpha, release=release) + long = alpha and sascm and not release + expected = 0x1001 | (2 if long else 0) | (0x400 if c9 else 0) + if not long and candidate in SDGM_CANCEL_PT_CAR: + expected |= 0x2000 + self.assertEqual(cp.safetyConfigs[0].safetyParam, expected) + self.assertTrue(is_ordinary_sdgm_profile(cp, longitudinal=long)) + self.assertEqual(sdgm_policy_for(cp) is not None, long) + if long: + limits = CarControllerParams(cp) + self.assertEqual((limits.MAX_GAS, limits.MAX_ACC_REGEN, limits.INACTIVE_REGEN), (2698, -540, -500)) + + def test_blazer_and_xt4_final_overrides(self): + cp = params(CAR.CHEVROLET_BLAZER, sascm=True, alpha=True) + self.assertAlmostEqual(cp.longitudinalActuatorDelay, .7, places=6) + self.assertAlmostEqual(cp.stopAccel, -.30, places=6) + for actual, expected in zip(sdgm_policy_for(cp).kp[1], (.09, .075, .055, .040), strict=True): + self.assertAlmostEqual(actual, expected, places=6) + xt4 = params(CAR.CADILLAC_XT4) + self.assertAlmostEqual(xt4.minSteerSpeed, 30 * .44704, places=6) + self.assertEqual(xt4.safetyConfigs[0].safetyParam, 0x3001) + + def test_camera_endpoints_and_braking_inactive(self): + cp = params(CAR.CHEVROLET_TRAVERSE, sascm=True, alpha=True) + self.assertEqual(demands(-4, 0, (), cp, min_gas=-540, max_gas=2698, inactive_gas=-500, brake_threshold=0), (-500, 400)) + self.assertEqual(demands(2, 100, (), cp, min_gas=-540, max_gas=2698, inactive_gas=-500, brake_threshold=0), (2698, 0)) + + def test_actual_packed_required_sources_and_selected_brake(self): + from opendbc.can import CANPacker + from opendbc.car import Bus + from opendbc.car.gm.carstate import CarState + from opendbc.car.gm.values import DBC + for candidate in ORDINARY_SDGM_CAR: + for c9 in (False, True): + cp = params(candidate, sascm=True, accelerator=not c9, alpha=True) + state = CarState(cp) + parsers = state.get_can_parsers(cp) + packer = CANPacker(DBC[candidate][Bus.pt]) + required = ('PSCMStatus', 'ESPStatus', 'EBCMWheelSpdFront', 'EBCMWheelSpdRear', + 'EBCMFrictionBrakeStatus', 'PSCMSteeringAngle', 'ECMPRDNL2', 'AcceleratorPedal2', + 'ECMEngineStatus', 'BCMTurnSignals', 'BCMDoorBeltStatus', 'BCMGeneralPlatformStatus', 'ASCMSteeringButton') + if not c9: + required += ('ECMAcceleratorPos',) + for index in range(30): + now = 1_000_000_000 + index * 10_000_000 + frames = [packer.make_can_msg(name, 0, {'BrakePressed': 1} if name == 'ECMEngineStatus' and c9 else + {'BrakePedalPos': 10} if name == 'ECMAcceleratorPos' else {}) for name in required] + parsers[Bus.pt].update([(now, frames)]) + parsers[Bus.cam].update([(now, [packer.make_can_msg('ASCMLKASteeringCmd', 2, {}), + packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'FCWAlert': 2})])]) + result = state.update(parsers) + self.assertTrue(parsers[Bus.pt].can_valid) + self.assertTrue(parsers[Bus.cam].can_valid) + self.assertTrue(result.brakePressed) + self.assertEqual(state.stock_fcw_alert, 2) + + def test_shared_feature_lane_and_aol_finalized_owners(self): + from opendbc.car.gm.feature_capabilities import longitudinal_supported + from opendbc.car.gm.lateral import lane_centering_supported + from opendbc.car.gm.aol import qualified_gm + from openpilot.starpilot.lateral.controller_selection import policy_for + for candidate in ORDINARY_SDGM_CAR: + for release in (False, True): + for alpha in (False, True): + cp = params(candidate, sascm=True, alpha=alpha, release=release) + self.assertEqual(longitudinal_supported(cp), alpha and not release) + self.assertTrue(lane_centering_supported(cp)) + self.assertTrue(qualified_gm(cp)) + self.assertEqual(policy_for(cp), 'ordinary_sdgm') + cp.safetyConfigs[0].safetyParam |= 4 + self.assertFalse(longitudinal_supported(cp)) + self.assertFalse(qualified_gm(cp)) + + def test_scoped_geometry_preserves_cc_neighbor(self): + from opendbc.car.gm.values import CAR + for candidate in (CAR.CADILLAC_XT5, CAR.CHEVROLET_BLAZER, CAR.BUICK_BABYENCLAVE): + self.assertEqual(candidate.config.specs.tireStiffnessFactor, 1.0) + self.assertEqual(CAR.CADILLAC_XT5_CC.config.specs.tireStiffnessFactor, 1.0) diff --git a/openpilot/selfdrive/controls/lib/latcontrol_torque.py b/openpilot/selfdrive/controls/lib/latcontrol_torque.py index 267c5b5967..668d2c7d67 100644 --- a/openpilot/selfdrive/controls/lib/latcontrol_torque.py +++ b/openpilot/selfdrive/controls/lib/latcontrol_torque.py @@ -8,6 +8,8 @@ from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY from openpilot.common.filter_simple import FirstOrderFilter from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.pid import PIDController +from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection, policy_for +from openpilot.starpilot.lateral.torque_extension import create_extension # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to @@ -33,8 +35,12 @@ LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 VERSION = 1 class LatControlTorque(LatControl): - def __init__(self, CP, CI, dt): + def __init__(self, CP, CI, dt, *, controller_mode: ControllerMode | None = None, turn_assist: bool = False): super().__init__(CP, CI, dt) + self.controller_mode = default_selection(CP).mode if controller_mode is None else ControllerMode(controller_mode) + self.controller_policy = policy_for(CP) + if self.controller_mode == ControllerMode.STARPILOT and self.controller_policy is None: + raise ValueError('No StarPilot torque policy for CarParams') self.torque_params = CP.lateralTuning.torque.as_builder() self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() @@ -45,8 +51,11 @@ class LatControlTorque(LatControl): self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len) self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt) self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) + self.starpilot_extension = create_extension(self, CP, self.controller_mode, self.controller_policy, turn_assist=turn_assist) def update_torque_parameters(self, latAccelFactor, latAccelOffset, friction): + if self.starpilot_extension is not None: + latAccelFactor, latAccelOffset, friction = self.starpilot_extension.transform_torque_parameters(latAccelFactor, latAccelOffset, friction) self.torque_params.latAccelFactor = latAccelFactor self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction @@ -57,6 +66,9 @@ class LatControlTorque(LatControl): self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + if self.starpilot_extension is not None: + return self.starpilot_extension.update(active, CS, VM, params, steer_limited_by_safety, + desired_curvature, curvature_limited, lat_delay) pid_log = log.ControlsState.LateralTorqueState.new_message() pid_log.version = VERSION measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) diff --git a/openpilot/selfdrive/locationd/lagd.py b/openpilot/selfdrive/locationd/lagd.py index b0d527005f..b81860fe8a 100755 --- a/openpilot/selfdrive/locationd/lagd.py +++ b/openpilot/selfdrive/locationd/lagd.py @@ -398,6 +398,8 @@ def main(): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['deviceMotion'].frequency) + from openpilot.starpilot.lateral.gm_geometry_runtime import GeometryPublicationOwner + geometry_owner = GeometryPublicationOwner(params, CP) if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None: lag, valid_blocks = initial_lag_params lag_learner.reset(lag, valid_blocks) @@ -415,7 +417,7 @@ def main(): if sm.frame % 5 == 0: lag_learner.update_estimate() lag_msg = lag_learner.get_msg(sm.all_checks(), DEBUG) - lag_msg_dat = lag_msg.to_bytes() + lag_msg_dat = geometry_owner.delay(lag_msg).to_bytes() pm.send('lateralDelay', lag_msg_dat) if sm.frame % 1200 == 0: # cache every 60 seconds diff --git a/openpilot/selfdrive/locationd/paramsd.py b/openpilot/selfdrive/locationd/paramsd.py index f4e5a53eb3..81fa68996c 100755 --- a/openpilot/selfdrive/locationd/paramsd.py +++ b/openpilot/selfdrive/locationd/paramsd.py @@ -258,6 +258,8 @@ def main(): steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params, CP, REPLAY, DEBUG) learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial) + from openpilot.starpilot.lateral.gm_geometry_runtime import GeometryPublicationOwner + geometry_owner = GeometryPublicationOwner(params, CP) while True: sm.update() @@ -270,7 +272,7 @@ def main(): if sm.updated['deviceMotion']: msg = learner.get_msg(sm.all_checks(), debug=DEBUG) - msg_dat = msg.to_bytes() + msg_dat = geometry_owner.parameters(msg).to_bytes() if sm.frame % 1200 == 0: # once a minute put_cache(params, "LiveParametersV2", msg) diff --git a/openpilot/starpilot/lateral/LICENSE b/openpilot/starpilot/lateral/LICENSE new file mode 100644 index 0000000000..a7daf5d1b6 --- /dev/null +++ b/openpilot/starpilot/lateral/LICENSE @@ -0,0 +1,8 @@ +Copyright (c) 2018, Comma.ai, Inc. +Copyright (c) 2026, firestar5683 and StarPilot contributors + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/openpilot/starpilot/lateral/README.md b/openpilot/starpilot/lateral/README.md new file mode 100644 index 0000000000..8b39b9f758 --- /dev/null +++ b/openpilot/starpilot/lateral/README.md @@ -0,0 +1,79 @@ +# Lateral control + +The lane-centering contribution, torque controller and torque parameter source +have separate owners. None grants steering permission: controlsd, the vehicle +controller and Panda retain their existing authority and limits. See +[TORQUE.md](TORQUE.md) for controller selection, learned parameters and manual +factor/friction preferences. + +## Lane centering + +`LaneCenteringController.update` returns a bounded curvature contribution and +an unclipped candidate. The host applies it after selecting the model or +maneuver curvature and before the native curvature limiter. Longitudinal-only +operation cannot authorize a lateral contribution. + +Both inner lane lines must have probability at least 0.6 and standard deviation +at most 0.3 m, with lane width between 2.6 and 4.8 m. Offset is limited to +0.3 m and 1.1 m center-to-line clearance. Lookahead follows speed within 8–35 m; +the center-error deadband is 0.08 m. End-to-end authority controls how strongly +a confident model path reduces the contribution between 0.15 and 0.50 m of +center error. Raw curvature is capped at 0.004 and scaled by 0.30. + +Acquisition uses a 0.4 s exponential filter. Signal pause and confidence loss +release the contribution over 0.2 s; driver override resets it immediately. +Signal pause takes precedence over the lane-change reset when both apply. +Malformed inputs and nonfinite settings are rejected. The host supplies elapsed +time, model freshness and explicit clock discontinuities. The filter advances +on each valid control tick even when the same model sample is held. + +`LaneCenteringHost` reads saved preferences at 1 Hz, including when a drive +starts with Lane Centering off, so Favorites can enable it during the drive. +Normal startup currently admits the tagged Ioniq 6 longitudinal configuration; +other vehicles are not yet admitted. `LANE_CENTERING_REPLAY_RUNTIME=1` provides +the separate offline replay path. Runtime checks require valid CAN, effective +lateral authority, enabled preferences, and fresh carState, selfdriveState, modelV2 and +vehicleParameters within 2.5 service periods. Missing evidence removes the +contribution without changing the saved preferences. + +Tests cover geometry, filtering, overrides, curvature-source priority, native +limiting and unchanged longitudinal commands. They do not establish vehicle +qualification. This module derives from the lane-centering work introduced by +jc01rho in `9f1066ce8380a2602beaf5d43fdb38ee6ec2dcbe`; the applicable MIT notice +is retained in the repository LICENSE. + +## Automatic lane changes + +The ordinary lane-change path requires a driver nudge. The saved Auto option +adds a delay of 0–5 s and minimum adjacent-lane width of 0–15 ft (stored in +meters), defaulting to 1 s and 0 ft. The saved option takes effect at modeld +startup on vehicles with configured steering control. Active lateral authority +is required; cruise control and Always On Lateral preferences do not enable it. + +DesireHelper owns the transition, blindspot check and signal latch. Auto uses +same-frame model evidence, current calibration, fresh carState/carControl, +engaged lateral control and a recognized vehicle. Camera EOF age uses +CLOCK_BOOTTIME; message and calibration timestamps use CLOCK_MONOTONIC. + +Adjacent width is the median distance between the inner and outer lane lines. +A nearer road edge returns zero width. A saved minimum of zero skips the width +requirement. Malformed geometry is rejected. + +The typed status in Event Data @126 supplies pre-change alert wording only when +its model frame, camera EOF, direction and lifetime match. Missing or stale +status produces neutral text and cannot affect engagement checks. + +## Vehicle-specific torque policies + +The Ioniq 6 policy retains separate 2023 and observed-firmware 2025 shaping for +curvature history, low-speed control, asymmetric feedforward, friction and +stop/pull-away behavior. The 2025 selection requires both firmware markers; +missing evidence selects the standard variant. The policy applies its factor +multiplier once per parameter update. Vehicle initialization uses factor 3.0 +and friction 0.09, with the original startup PID limits; subsequent learned or +manual updates use the parameter-update contract described in TORQUE.md. + +Reference fixtures cover both Ioniq variants through stops, pull-away, highway, +reversal and driver override, with independent non-Ioniq regression cases. +Other explicit policies and the standard controller remain separate; tuning +one platform must not change another platform's controller or parameter source. diff --git a/openpilot/starpilot/lateral/TORQUE.md b/openpilot/starpilot/lateral/TORQUE.md new file mode 100644 index 0000000000..1cc8775183 --- /dev/null +++ b/openpilot/starpilot/lateral/TORQUE.md @@ -0,0 +1,69 @@ +# Torque controllers and parameter sources + +Controller selection and torque parameter selection are separate. `LatControlTorque` uses the standard controller unless the vehicle has an explicit policy, currently Ioniq 6, Genesis G70 2020, Corolla TSS2, the five manual Bolt identities or the five Volt identities. The `AdvancedLateralTune` preference does **not** select a different controller; it enables manual factor/friction adjustments through `TorqueHost`. + +## Controller choice + +Supported vehicles can save either the standard torque controller or their StarPilot vehicle policy. `LateralControllerSelection` is a versioned JSON document with separate choices by vehicle identity. Missing or invalid settings preserve the existing controller: the vehicle policy for the registered supported platforms, and the standard controller elsewhere. An unsupported vehicle cannot select another vehicle's policy. + +`controlsd` resolves the choice once from CarParams before constructing the controller. Later preference changes take effect on the next drive; they cannot change the running controller or carry its integrator into another algorithm. Selecting standard skips vehicle-policy construction, including policy-specific gains and factor multipliers. It retains the vehicle's CarParams, delay estimation and separately configured manual torque values. Automatic learning is a separate preference. It therefore selects a control algorithm, not OEM steering or a complete upstream vehicle tune. + +Galaxy and native settings share the preference owner. The owner checks the current vehicle and displayed document before saving, preserves other vehicles' choices, and reports a saved selection rather than claiming that it is already active. + +**Automatic Steering Learning** saves an On or Off choice for the exact vehicle brand and fingerprint in `SteeringLearningPreferences`. Controller changes preserve this choice. Vehicles without a saved entry use the valid legacy `ForceAutoTuneOff` preference, or default to On when it is absent; no migration rewrites that legacy value. The UI shows the saved choice separately from controller compatibility. On does not enable incompatible scalar learning or claim an automatic result is applied. Prep My Vehicle for Tuning deliberately saves Off for the prepared vehicle and journals the prior choice for exact restoration. + +The runtime uses the same exact-platform learning policy. The running StarPilot controller, including a default or invalid-selection fallback to an established policy, cannot consume learned parameters through either the direct controls path or `TorqueHost`. The `torqued` process samples this policy at startup and, when disabled, skips old caches, sample collection, fitting and cache writes; existing cached data is retained. Standard with a saved learning-off preference also disables learning, even when manual adjustments are off. Unknown platforms keep their established learning behavior. Saved controller and learning changes are intent for the next process startup, not evidence of the currently running controller. Offline `TorqueEstimator` analysis retains estimation by default; the running process explicitly supplies its learning policy. + +## Runtime admission + +### Preparing a support log + +**Prep My Vehicle for Tuning** is a parked On/Off toggle for registered steering-policy vehicles. On saves a durable, vehicle-bound snapshot before selecting StarPilot steering, turning learning/manual adjustments/Turn Assist Off and selecting default lateral lane preferences. Off restores the exact saved bytes, including absent keys, after a restart or interrupted write. Later user edits are preserved and require review rather than being overwritten. Manual torque documents, calibration and longitudinal preferences remain saved. Record a route, then open a tuning request on the StarPilot Discord: https://firestar.link/discord. + +The journal is written before preference changes, and every replacement rechecks the vehicle and parked authority. Interrupted preparation is shown as incomplete; switching Off recovers the original snapshot. Invalid controller/lane documents or later user edits block changes without overwriting them. + +Start a new drive after preparation and use the ordinary route log. The controls startup event records its actual selected controller/policy, learning decision, initial vehicle source, torque values and PID limits. The usual route data contains vehicle/model/calibration and control behavior; this setup does not claim that a saved request proves a running baseline or vehicle qualification. The preparation journal records saved preference state, not proof of a qualified running tune. + +`torque_runtime.py` admits normal Ioniq 6 sessions when manual adjustments are enabled and valid, independently of longitudinal ownership. `TORQUE_REPLAY_RUNTIME=1` additionally permits the exact Toyota/Lexus torque platforms in `torque_supported.py` for offline replay. Admission requires the expected brand, torque steering and lateral tuning, finite valid CarParams values, and a non-dashcam configuration. The five manual Bolt identities also support an explicit, vehicle-bound friction profile; their requirements are described below. Other vehicles keep the standard parameter path and cannot inherit these manual settings. + +The source selector does not grant steering, longitudinal or independent-axis permission. Existing controls and vehicle safety checks continue to own actuation. + +## Parameter precedence + +The source starts with `CarParams.lateralTuning.torque`. When the startup learning policy permits it, fresh, valid version-1 `lateralTorqueParameters` may replace those values within the car-relative limits in `TorqueHost._learned`. Messages must pass the service checks and be no older than two publication periods. `ForceAutoTuneOff` ignores learned values without disabling valid manual overrides. + +Manual factor and friction are independent. A manual factor uses the vehicle's original offset; an unchanged field may still use its learned value when learning is allowed, or its vehicle value otherwise. Missing settings do not create overrides. Malformed values or a saved profile requiring review fall back to the vehicle values while preserving the saved data. + +`torque_settings.py` stores manual choices by vehicle identity and the factor/offset/friction basis. A changed vehicle basis requires review before reusing its custom values. For the existing Ioniq/Toyota parameter hosts, legacy `SteerLatAccel` and `SteerFriction` are used only when no profile document exists. Bolt does not import these device-wide values. Matching `SteerLatAccelStock` and `SteerFrictionStock` markers identify stock tracking rather than a manual override; these optional keys remain persistent. Legacy comparisons retain two-decimal rounding and are resolved before checking custom-value bounds. + +## Application and reset + +`TorqueTuning` is the typed boundary between source selection and the controller. `TorqueHost` refreshes saved settings at 1 Hz, checks source freshness every control frame, and rate-limits changes while active. A missing learner falls back to valid manual values or the vehicle tune. Clock discontinuities, invalid settings or loss of lateral permission reset the optional tune to vehicle values. Invalid CAN suppresses lateral output and clears the optional controller's integrator. + +In admitted sessions, the torque learner skips old cached CarParams and learned values without deleting them. Other vehicles retain the standard cache policy. No learned cache is automatically translated into a manual profile. + +A vehicle policy may convert a selected raw factor before application. The Ioniq 6 policy applies its factor multiplier once; callers must not pre-apply it. `TorqueHost.apply` also avoids redundant initial updates because a vehicle policy's startup PID limits can differ from its later parameter-update limits. Tests under `tests/` cover source selection, vehicle isolation, transitions and driver-override behavior. + +## Manual Bolt controller + +The manual Bolt ACC 2022–2023, ACC Pedal 2022–2023, CC 2022–2023, CC 2018–2021 and CC 2017 identities default to the StarPilot controller. Their generation policy owns asymmetric torque conversion, feedforward, friction, center shaping and request history. It adjusts the live VehicleModel steering ratio for CC 2017 and CC 2018–2021. Saved STANDARD selection preserves the upstream controller and ratio. Ordinary Bolt EUV keeps its standard controller. + +The default Bolt policy uses its established constant proportional gain and generation calibration. Manual friction is available with either controller, independently of automatic learning. The current torque estimator does not authorize learned parameters for GM; this feature preserves that producer policy. Starting the optional parameter host requires manual adjustments On and a valid custom-friction profile for the exact vehicle and current calibration. Enabling it takes effect on the next drive. An absent profile leaves the existing parameter path unchanged; old device-wide tuning values are retained but never activated automatically. + +Once started, the host follows the refresh and transition rules above. Returning friction to its source, or turning manual adjustments Off, removes the manual override and uses the admitted learned or vehicle values according to the running learning policy. Invalid settings or inactive lateral control restore vehicle values. The Bolt editor uses the existing calibration-relative friction bounds. It does not expose lateral-acceleration factor editing: the StarPilot Bolt conversion uses a fixed nonlinear curve, and historical factor selections also affected offset selection. Profiles containing that unsupported choice require an explicit reset for this vehicle; other vehicle profiles are preserved. + +Manual steering ratio/delay and FLM trial overrides are separate from these ordinary manual preferences. Steering and longitudinal admission remain with the current control and safety owners. + +Vehicle-specific torque behavior is selected once in `torque_extension.py`. The upstream controller calls its generic update and parameter hooks; Controls calls its generic vehicle-model ratio hook. Standard selection constructs no extension and retains the upstream numerical path. + +## Manual Bolt steering response + +The same five manual Bolt identities support a typed custom proportional gain with either selected controller. The editor calls this Steering response: higher values respond more strongly and lower values more gently. Custom values must be finite and within 0.3–0.9. This is independent of friction and automatic learning. A gain-only profile does not start `TorqueHost`, alter torque-parameter sources, or change the estimator cache policy. No raw `SteerKP` value is imported. + +A custom gain is bound to the selected controller, its initialized proportional-gain table and the vehicle's current torque calibration. The StarPilot source gain is 0.6; Standard retains its upstream speed table. A changed binding pauses only the gain until explicitly reviewed, preserving friction and other vehicle profiles. Version-1 documents remain readable with no gain override; an explicit gain save adds the version-2 fields. + +Starting the gain owner requires manual adjustments On and a qualified custom gain at startup, so enabling it takes effect on the next drive. Once started, saved values refresh at 1 Hz. Returning to Selected controller, turning manual adjustments Off, or invalidating the saved gain restores the captured source table. Changing gain never resets the integrator or request history. This typed refresh contract rejects out-of-range values rather than reproducing the old device-wide knob's clamping and cached reload timing. Steering permission and output limits remain with the existing controllers and safety owners. + +## Volt controller + +Admitted Volt, Volt ASCM, Volt Camera, Volt CC and Volt 2019 configurations use their StarPilot torque policy by default. It uses the common Volt torque calibration, asymmetric fixed conversion, proportional gain 0.6 and integral gain 0.35. A saved Standard selection preserves the standard torque algorithm and its parameter policy, using the restored vehicle geometry and calibration. The vehicle model uses the selected configuration’s geometry without a policy ratio multiplier. This controller selection does not enable manual torque settings or expand automatic learning. diff --git a/openpilot/starpilot/lateral/__init__.py b/openpilot/starpilot/lateral/__init__.py new file mode 100644 index 0000000000..64dbeaf13a --- /dev/null +++ b/openpilot/starpilot/lateral/__init__.py @@ -0,0 +1 @@ +"""Standalone lateral feature contracts. Nothing in this package actuates a car.""" diff --git a/openpilot/starpilot/lateral/adjacent_lane_evidence.py b/openpilot/starpilot/lateral/adjacent_lane_evidence.py new file mode 100644 index 0000000000..bbe74a7951 --- /dev/null +++ b/openpilot/starpilot/lateral/adjacent_lane_evidence.py @@ -0,0 +1,57 @@ +"""Same-frame adjacent width for automatic lane changes.""" + +import bisect +import math +from statistics import median + +from openpilot.selfdrive.modeld.constants import ModelConstants + + +def _points(line): + xs, ys = list(line.x), list(line.y) + if (len(xs) != ModelConstants.IDX_N or len(ys) != ModelConstants.IDX_N or + not all(math.isfinite(value) for value in xs + ys) or + any(b <= a for a, b in zip(xs, xs[1:], strict=False))): + return None + return xs, ys + + +def _at(points, x): + xs, ys = points + if x <= xs[0]: + return ys[0] + if x >= xs[-1]: + return ys[-1] + index = bisect.bisect_left(xs, x) + ratio = (x - xs[index - 1]) / (xs[index] - xs[index - 1]) + return ys[index - 1] + ratio * (ys[index] - ys[index - 1]) + + +def adjacent_lane_available(model, direction: int, minimum_width_m: float) -> bool: + """Compare median adjacent width with the saved minimum.""" + try: + if not math.isfinite(minimum_width_m) or not 0 <= minimum_width_m <= 4.572 or direction not in (-1, 1): + return False + inner, outer, edge = ((1, 0, 0) if direction == -1 else (2, 3, 1)) + if (len(model.laneLines) != 4 or len(model.roadEdges) != 2 or len(model.laneLineProbs) != 4 or + len(model.laneLineStds) != 4 or len(model.roadEdgeStds) != 2): + return False + if (not all(math.isfinite(value) and 0 <= value <= 1 for value in model.laneLineProbs) or + not all(math.isfinite(value) and value >= 0 for value in list(model.laneLineStds) + list(model.roadEdgeStds))): + return False + inner_points = _points(model.laneLines[inner]) + outer_points = _points(model.laneLines[outer]) + edge_points = _points(model.roadEdges[edge]) + if inner_points is None or outer_points is None or edge_points is None: + return False + xs, ys = inner_points + lane_distances = [abs(y - _at(outer_points, x)) for x, y in zip(xs, ys, strict=True)] + edge_distances = [abs(y - _at(edge_points, x)) for x, y in zip(xs, ys, strict=True)] + if not all(math.isfinite(value) for value in lane_distances + edge_distances): + return False + width = median(lane_distances) + if median(edge_distances) < width: + width = 0.0 + return width >= minimum_width_m + except (AttributeError, IndexError, TypeError, ValueError, OverflowError): + return False diff --git a/openpilot/starpilot/lateral/ascm_policy.py b/openpilot/starpilot/lateral/ascm_policy.py new file mode 100644 index 0000000000..728d21dbad --- /dev/null +++ b/openpilot/starpilot/lateral/ascm_policy.py @@ -0,0 +1,24 @@ +"""Ordinary ASCM torque selection; factory PID identity stays on PID.""" + +import math + +from opendbc.car.structs import CarParams +from opendbc.car.gm.values import is_ordinary_ascm_profile +from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy + + +def supported_cp(cp): + if not (is_ordinary_ascm_profile(cp, longitudinal=False) or is_ordinary_ascm_profile(cp, longitudinal=True)): + return False + if cp.steerControlType != CarParams.SteerControlType.torque or cp.lateralTuning.which() != "torque": + return False + tune = cp.lateralTuning.torque + return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and + math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0) + + +class AscmTorquePolicy(GMOrdinaryTorquePolicy): + def __init__(self, parent, cp): + if not supported_cp(cp): + raise ValueError("Unsupported ordinary ASCM torque profile") + super().__init__(parent, cp) diff --git a/openpilot/starpilot/lateral/auto_lane_change.py b/openpilot/starpilot/lateral/auto_lane_change.py new file mode 100644 index 0000000000..7602fc1aa8 --- /dev/null +++ b/openpilot/starpilot/lateral/auto_lane_change.py @@ -0,0 +1,117 @@ +"""Auto Lane Change qualification from one model frame.""" + +import math +import time +from dataclasses import replace + +from opendbc.car import structs +from openpilot.cereal import log +from openpilot.starpilot.lateral.adjacent_lane_evidence import adjacent_lane_available +from openpilot.starpilot.lateral.lane_change_preferences import LaneChangePolicy + + +MODEL_MAX_AGE_NS = 150_000_000 # camera EOF uses CLOCK_BOOTTIME +CAR_MAX_AGE_NS = 100_000_000 +CONTROL_MAX_AGE_NS = 100_000_000 +CALIBRATION_MAX_AGE_NS = 750_000_000 +CLOCK_PAIR_MAX_SKEW_NS = 1_000_000 + + +class ClockEpochGuard: + """After a suspend offset jump, require new inputs in the resumed epoch.""" + + def __init__(self, now_mono_ns: int, now_boot_ns: int, sample_skew_ns: int = 0): + self.offset_ns = now_boot_ns - now_mono_ns + self.sample_skew_ns = sample_skew_ns + self.barrier_mono_ns = now_mono_ns if sample_skew_ns > CLOCK_PAIR_MAX_SKEW_NS else 0 + + def ready(self, sm, now_mono_ns: int, now_boot_ns: int, sample_skew_ns: int = 0) -> bool: + if sample_skew_ns < 0 or sample_skew_ns > CLOCK_PAIR_MAX_SKEW_NS: + return False + offset = now_boot_ns - now_mono_ns + if abs(offset - self.offset_ns) > max(CLOCK_PAIR_MAX_SKEW_NS, sample_skew_ns + self.sample_skew_ns): + self.offset_ns = offset + self.sample_skew_ns = sample_skew_ns + self.barrier_mono_ns = now_mono_ns + return False + self.sample_skew_ns = sample_skew_ns + if self.barrier_mono_ns: + try: + services = ("carState", "carControl", "extrinsicsCalibration") + if any(int(sm.logMonoTime[name]) <= self.barrier_mono_ns or + int(sm.recv_time[name] * 1e9) <= self.barrier_mono_ns for name in services): + return False + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return False + self.barrier_mono_ns = 0 + return True + + +def steering_capable(cp) -> bool: + try: + return bool(cp is not None and cp.carFingerprint and not (cp.notCar or cp.passive or cp.dashcamOnly) and + cp.steerControlType in (structs.CarParams.SteerControlType.torque, structs.CarParams.SteerControlType.angle, + structs.CarParams.SteerControlType.curvature) and + cp.safetyConfigs and not any(config.safetyModel == structs.CarParams.SafetyModel.allOutput for config in cp.safetyConfigs) and + any(config.safetyModel not in + (structs.CarParams.SafetyModel.silent, structs.CarParams.SafetyModel.noOutput, + structs.CarParams.SafetyModel.allOutput) for config in cp.safetyConfigs)) + except (AttributeError, TypeError, ValueError): + return False + + +def session_policy(saved: LaneChangePolicy, vehicle_capable: bool) -> LaneChangePolicy: + """Latch the saved request only for a configured steering owner.""" + return saved if vehicle_capable else replace(saved, auto_lane_change=False) + + +def boottime_ns() -> int: + return time.clock_gettime_ns(getattr(time, "CLOCK_BOOTTIME", time.CLOCK_MONOTONIC)) + + +def paired_clocks_ns() -> tuple[int, int, int]: + mono_before = time.monotonic_ns() + boot = boottime_ns() + mono_after = time.monotonic_ns() + return (mono_before + mono_after) // 2, boot, mono_after - mono_before + + +def _fresh_submaster(sm, service: str, now_mono_ns: int, limit_ns: int) -> bool: + try: + observed = int(sm.logMonoTime[service]) + return (sm.seen[service] and sm.alive[service] and sm.valid[service] and + 0 < observed <= now_mono_ns and now_mono_ns - observed <= limit_ns - CLOCK_PAIR_MAX_SKEW_NS) + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return False + + +def current_calibration(sm, now_mono_ns: int) -> bool: + if not _fresh_submaster(sm, "extrinsicsCalibration", now_mono_ns, CALIBRATION_MAX_AGE_NS): + return False + try: + calibration = sm["extrinsicsCalibration"] + return (calibration.calStatus == log.ExtrinsicsCalibration.Status.calibrated and + len(calibration.rpyCalib) == 3 and all(math.isfinite(float(v)) for v in calibration.rpyCalib)) + except (AttributeError, TypeError, ValueError, OverflowError): + return False + + +def auto_evidence(sm, model, direction: int, minimum_width_m: float, *, now_mono_ns: int, + now_boot_ns: int, model_valid: bool, vehicle_capable: bool) -> bool: + """Qualify only the just-filled model, with each clock in its own domain.""" + try: + if not model_valid or not vehicle_capable or int(model.frameAge) > 1: + return False + eof = int(model.timestampEof) + if not 0 < eof <= now_boot_ns or now_boot_ns - eof > MODEL_MAX_AGE_NS: + return False + if not (_fresh_submaster(sm, "carState", now_mono_ns, CAR_MAX_AGE_NS) and + _fresh_submaster(sm, "carControl", now_mono_ns, CONTROL_MAX_AGE_NS) and + current_calibration(sm, now_mono_ns)): + return False + control = sm["carControl"] + car_state = sm["carState"] + return (bool(control.latActive and car_state.canValid and not car_state.canTimeout) and + adjacent_lane_available(model, direction, minimum_width_m)) + except (AttributeError, TypeError, ValueError, OverflowError): + return False diff --git a/openpilot/starpilot/lateral/bolt_policy.py b/openpilot/starpilot/lateral/bolt_policy.py new file mode 100644 index 0000000000..550e2124d9 --- /dev/null +++ b/openpilot/starpilot/lateral/bolt_policy.py @@ -0,0 +1,175 @@ +"""Vehicle-owned default torque controller for the five manual Bolt identities.""" + +from collections import deque +import math + +import numpy as np + +from opendbc.car import structs +from opendbc.car.gm.values import CAR +from opendbc.car.lateral import get_friction +from openpilot.cereal import log +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.pid import PIDController +from openpilot.starpilot.lateral import bolt_shaping as shaping +from openpilot.starpilot.lateral.torque_shaping import ( + FF_ROLL_OFFSET_FADE_BP, + FF_ROLL_OFFSET_FADE_V, + JERK_GAIN, + KI, + LAT_ACCEL_REQUEST_BUFFER_SECONDS, + LOW_SPEED_X, + LOW_SPEED_Y, + LP_FILTER_CUTOFF_HZ, + MAX_LAT_JERK_UP, + MIN_SPEED, + UNWIND_D_DES_THRESHOLD, + UNWIND_LAT_ACCEL_NEAR_ZERO, + center_chatter_friction_jerk_deadzone, +) + +BOLT_GENERATIONS = { + CAR.CHEVROLET_BOLT_CC_2017: 2017, + CAR.CHEVROLET_BOLT_CC_2018_2021: 2018, + CAR.CHEVROLET_BOLT_CC_2022_2023: 2022, + CAR.CHEVROLET_BOLT_ACC_2022_2023: 2022, + CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL: 2022, +} + + +def supported_cp(cp): + return ( + cp.brand == 'gm' + and cp.carFingerprint in BOLT_GENERATIONS + and not cp.notCar + and not cp.passive + and not cp.dashcamOnly + and cp.steerControlType == structs.CarParams.SteerControlType.torque + and cp.lateralTuning.which() == 'torque' + and math.isfinite(cp.lateralTuning.torque.latAccelFactor) + and cp.lateralTuning.torque.latAccelFactor > 0 + and math.isfinite(cp.lateralTuning.torque.latAccelOffset) + and math.isfinite(cp.lateralTuning.torque.friction) + and cp.lateralTuning.torque.friction >= 0 + ) + + +class BoltTorquePolicy: + def __init__(self, parent, cp): + self.parent = parent + if not supported_cp(cp): + raise ValueError('Unsupported Bolt torque profile') + self.generation = BOLT_GENERATIONS[cp.carFingerprint] + self.dt = parent.dt + # Preserve the source calibration's Float32 parameter representation. + self.ff_positive = float(np.float32(1.03)) + self.ff_negative = float(np.float32(float(np.float32(1.07)) * (0.9 if self.generation == 2017 else 1.07))) + self.ki_multiplier = float(np.float32(float(np.float32(0.93)) * (0.9 if self.generation == 2017 else 0.93))) + self.center_boost = float(np.float32(0.026)) if self.generation == 2017 else float(np.float32(float(np.float32(0.02)) * 0.75)) + coefficients = { + 2017: ((2.15, 1.0, 0.129, 0.0), (2.15, 1.0, 0.145, 0.0)), + 2018: ((1.8, 1.1, 0.27, 0.0), (2.0, 1.0, 0.205, 0.0)), + 2022: ((2.6531724862969748, 1.1, 0.1919764879840985, 0.0), (2.7031724862969748, 1.0, 0.1469764879840985, 0.0)), + }[self.generation] + lateral_values = np.arange(-5.0, 5.0, 0.01) + torque_values = [] + for value in lateral_values: + a, b, c, d = coefficients[0 if value >= 0 else 1] + sig_input = a * value + sigmoid = np.sign(sig_input) * (1 / (1 + math.exp(-abs(sig_input))) - 0.5) + torque_values.append(float(sigmoid * b + value * c + d)) + parent.torque_from_lateral_accel = lambda value, tune: np.interp(value, lateral_values, torque_values) + parent.lateral_accel_from_torque = lambda value, tune: np.interp(value, torque_values, lateral_values) + parent.pid = PIDController(0.6, KI * self.ki_multiplier, rate=1 / self.dt) + parent.update_limits() + self.buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) + self.curvature_buffer = deque([0.0] * self.buffer_len, maxlen=self.buffer_len) + self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) + self.measurement_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) + self.low_speed_reset_threshold = max(cp.minSteerSpeed, 0.3) + self.previous_measurement = 0.0 + self.previous_setpoint = 0.0 + self.previous_output = 0.0 + self.previous_pressed = False + + def steer_ratio_scale(self, speed): + if self.generation == 2017: + return 1.0 + (1.045 - 1.0) * shaping._bolt_2017_high_speed_factor(speed) + return 1.01 if self.generation == 2018 else 1.0 + + def update(self, active, cs, vm, params, safety_limited, curvature, curvature_limited, delay): + parent = self.parent + pid_log = log.ControlsState.LateralTorqueState.new_message() + pid_log.version = 2 + measurement = -vm.calc_curvature(math.radians(cs.steeringAngleDeg - params.angleOffsetDeg), cs.vEgo, params.roll) * cs.vEgo**2 + future = curvature * cs.vEgo**2 + if not active: + parent.pid.reset() + self.previous_output = 0.0 + self.curvature_buffer.append(curvature) + self.previous_measurement = measurement + self.measurement_filter.x = self.jerk_filter.x = 0.0 + self.previous_setpoint = future + self.previous_pressed = cs.steeringPressed + pid_log.active = False + return 0.0, 0.0, pid_log + if self.previous_pressed and not cs.steeringPressed: + parent.pid.i *= 0.8 + fade = np.interp(cs.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V) + roll = params.roll * ACCELERATION_DUE_TO_GRAVITY * fade + deadzone = abs(vm.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), cs.vEgo, 0.0)) * cs.vEgo**2 + delay_frames = int(np.clip(delay / self.dt, 1, self.buffer_len)) + expected = self.curvature_buffer[-delay_frames] * cs.vEgo**2 + self.curvature_buffer.append(curvature) + raw_jerk = np.clip((future - expected) / max(delay, self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) + jerk = np.clip(self.jerk_filter.update(raw_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) + setpoint = expected + jerk * delay + unwind = (setpoint - self.previous_setpoint) / self.dt < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO + self.previous_setpoint = setpoint + rate = np.clip(self.measurement_filter.update((measurement - self.previous_measurement) / self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) + self.previous_measurement = measurement + lsf = (np.interp(cs.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(cs.vEgo, MIN_SPEED)) ** 2 + kp = np.interp(cs.vEgo, parent.pid._k_p[0], parent.pid._k_p[1]) + error = (setpoint - measurement) * (1 + lsf / max(kp, 1e-3)) + gravity_adjusted = future - roll + ff = gravity_adjusted - parent.torque_params.latAccelOffset * fade + ff *= np.interp(ff, [-0.05, 0.0, 0.05], [self.ff_negative, 1.0, self.ff_positive]) + threshold = shaping.get_gm_base_friction_threshold(cs.vEgo) + friction_scale = 1.0 + if self.generation == 2022: + ff *= shaping.get_bolt_2022_2023_ff_scale(setpoint, jerk, cs.vEgo) + threshold = shaping.get_bolt_2022_2023_friction_threshold(cs.vEgo, setpoint, jerk) + friction_scale = shaping.get_bolt_2022_2023_friction_scale(cs.vEgo, setpoint, jerk) + elif self.generation == 2018: + threshold = shaping.get_bolt_2018_2021_friction_threshold(cs.vEgo, setpoint, jerk) + friction_scale = shaping.get_bolt_2018_2021_friction_scale(cs.vEgo, setpoint, jerk) + jerk_deadzone = center_chatter_friction_jerk_deadzone(cs.vEgo, setpoint, 0.0) + friction_jerk = math.copysign(max(abs(jerk) - jerk_deadzone, 0.0), jerk) + ff += friction_scale * get_friction(error + JERK_GAIN * friction_jerk, deadzone, threshold, parent.torque_params) + if abs(gravity_adjusted) < 0.15: + ff += np.sign(gravity_adjusted) * self.center_boost * np.interp(abs(gravity_adjusted), [0.0, 0.15], [1.0, 0.0]) + if cs.vEgo < self.low_speed_reset_threshold: + parent.pid.reset() + freeze = safety_limited or cs.steeringPressed or cs.vEgo < self.low_speed_reset_threshold or unwind + pid_log.error = float(error) + output = parent.torque_from_lateral_accel( + parent.pid.update(pid_log.error, error_rate=-rate, speed=cs.vEgo, feedforward=ff, freeze_integrator=freeze), parent.torque_params + ) + if self.generation == 2022: + output *= shaping.get_bolt_2022_2023_center_output_scale(setpoint, cs.vEgo) + limit = shaping.get_bolt_2022_2023_low_speed_center_output_limit(setpoint, cs.vEgo) + output = float(np.clip(output, -limit, limit)) + output = shaping.get_bolt_2022_2023_low_speed_center_output(output, self.previous_output, setpoint, cs.vEgo) + elif self.generation == 2017: + output *= shaping.get_bolt_2017_torque_scale(setpoint, jerk, cs.vEgo) + else: + output *= shaping.get_bolt_2018_2021_dynamic_torque_scale(setpoint, jerk, cs.vEgo) + self.previous_output = float(output) + self.previous_pressed = cs.steeringPressed + pid_log.active = True + pid_log.p, pid_log.i, pid_log.d, pid_log.f = map(float, (parent.pid.p, parent.pid.i, parent.pid.d, parent.pid.f)) + pid_log.output = float(-output) + pid_log.actualLateralAccel, pid_log.desiredLateralAccel, pid_log.desiredLateralJerk = map(float, (measurement, setpoint, jerk)) + pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output) < 1e-3, cs, safety_limited, curvature_limited)) + return -float(output), 0.0, pid_log diff --git a/openpilot/starpilot/lateral/bolt_shaping.py b/openpilot/starpilot/lateral/bolt_shaping.py new file mode 100644 index 0000000000..4728dfb13f --- /dev/null +++ b/openpilot/starpilot/lateral/bolt_shaping.py @@ -0,0 +1,368 @@ +'Default generation-specific Bolt torque shaping.' + +import math + +import numpy as np + + +from opendbc.car.common.conversions import Conversions as CV +from openpilot.starpilot.lateral.torque_shaping import sigmoid + +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.1 +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.06, 1.055, 1.045] +BOLT_2017_TORQUE_SCALE_RIGHT = [1.0, 1.0, 1.035, 1.02, 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_TORQUE_GAIN_LEFT = 0.09 +BOLT_2018_2021_TORQUE_GAIN_RIGHT = 0.05 +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.1 +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.8 +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_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.4 +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_LOW_SPEED_CENTER_TAPER_MAX = 0.12 +BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT = 0.14 +BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.04 +BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED = 4.0 +BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.5 +BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR = 2.0 +BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR_WIDTH = 0.7 +BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 16.5 +BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX_WIDTH = 2.0 +BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LIMIT = 0.38 +BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT = 0.17 +BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT_WIDTH = 0.04 +BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED = 2.5 +BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH = 0.7 +BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX = 8.2 +BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH = 0.6 +BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SCALE_MIN = 0.62 +BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_ALPHA_MIN = 0.28 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.08 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT = 0.18 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.06 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED = 6.7 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 1.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.1 +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 + + +def get_gm_base_friction_threshold(v_ego: float) -> float: + 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 _bolt_2017_high_speed_factor(v_ego: float) -> float: + return sigmoid((max(v_ego, 0.0) - BOLT_2017_STEER_RATIO_ONSET_SPEED) / BOLT_2017_STEER_RATIO_ONSET_WIDTH) + + +def get_bolt_2017_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + center_window = 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_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 = sigmoid((abs_lateral_accel - BOLT_2018_2021_TORQUE_ONSET) / BOLT_2018_2021_TORQUE_ONSET_WIDTH) + cutoff = 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 = 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_gm_base_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.1) + + +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 = sigmoid((abs_lateral_accel - BOLT_2022_2023_FF_ONSET) / BOLT_2022_2023_FF_ONSET_WIDTH) + cutoff = sigmoid((BOLT_2022_2023_FF_CUTOFF - abs_lateral_accel) / BOLT_2022_2023_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + speed_weight = sigmoid((v_ego - BOLT_2022_2023_CENTER_TAPER_SPEED) / BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH) + center_weight = 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_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + highway_speed_weight = sigmoid((v_ego - BOLT_2022_2023_CENTER_TAPER_SPEED) / BOLT_2022_2023_CENTER_TAPER_SPEED_WIDTH) + highway_center_weight = sigmoid((BOLT_2022_2023_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2022_2023_CENTER_TAPER_LAT_WIDTH) + low_speed_onset = sigmoid((v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED) / BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH) + low_speed_cutoff = sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX_WIDTH) + low_speed_center_weight = sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_LAT_WIDTH) + low_speed_floor = sigmoid((v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR) / BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_FLOOR_WIDTH) + highway_reduction = BOLT_2022_2023_CENTER_TAPER_MAX * highway_speed_weight * highway_center_weight + low_speed_reduction = BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_MAX * low_speed_onset * low_speed_cutoff * low_speed_center_weight * low_speed_floor + return 1.0 - min(highway_reduction + low_speed_reduction, 0.95) + + +def get_bolt_2022_2023_low_speed_center_output_limit(desired_lateral_accel: float, v_ego: float) -> float: + """Limit small-signal output while the Bolt is in its low-speed chatter band.""" + speed_onset = sigmoid((v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH) + speed_cutoff = sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX - v_ego) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH) + center_weight = sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT - abs(desired_lateral_accel)) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT_WIDTH) + speed_weight = speed_onset * speed_cutoff + reduction = (1.0 - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LIMIT) * speed_weight * center_weight + return 1.0 - reduction + + +def get_bolt_2022_2023_low_speed_center_output(output_torque: float, prev_output_torque: float, desired_lateral_accel: float, v_ego: float) -> float: + """Damp low-speed center reversals without reducing real turn authority.""" + speed_weight = sigmoid((v_ego - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH) * sigmoid( + (BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX - v_ego) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH + ) + center_weight = sigmoid((BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT - abs(desired_lateral_accel)) / BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_LAT_WIDTH) + envelope = speed_weight * center_weight + output_scale = 1.0 - (1.0 - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SCALE_MIN) * envelope + output_alpha = 1.0 - (1.0 - BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_ALPHA_MIN) * envelope + limited_output = output_torque * output_scale + return float(prev_output_torque + output_alpha * (limited_output - prev_output_torque)) + + +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_gm_base_friction_threshold(v_ego) + center_weight = sigmoid((BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) / BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH) + low_speed_weight = sigmoid((BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED - v_ego) / BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH) + base_threshold += BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP * center_weight * low_speed_weight + 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) diff --git a/openpilot/starpilot/lateral/camera_policy.py b/openpilot/starpilot/lateral/camera_policy.py new file mode 100644 index 0000000000..d2c0985b5a --- /dev/null +++ b/openpilot/starpilot/lateral/camera_policy.py @@ -0,0 +1,51 @@ +"""Default lateral law for exact GM camera and Silverado interceptor configurations.""" +import math +import numpy as np + +from opendbc.car.gm.values import CAR, is_ordinary_camera_profile, is_silverado_cc_pedal_profile +from opendbc.car.structs import CarParams +from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy + + +def supported_cp(cp): + if not (is_ordinary_camera_profile(cp, longitudinal=cp.openpilotLongitudinalControl) or is_silverado_cc_pedal_profile(cp)): + return False + if cp.steerControlType != CarParams.SteerControlType.torque or cp.lateralTuning.which() != 'torque': + return False + tune = cp.lateralTuning.torque + return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and + math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0) + + +def sigmoid(value): + z = math.exp(-value) if value >= 0 else math.exp(value) + return 1 / (1 + z) if value >= 0 else z / (1 + z) + + +class CameraTorquePolicy(GMOrdinaryTorquePolicy): + def __init__(self, parent, cp): + if not supported_cp(cp): + raise ValueError('Unsupported GM camera or Silverado interceptor torque profile') + self.silverado = cp.carFingerprint in (CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_SILVERADO_CC) + coefficients = ((3.8, .81, .24, .0465122) if self.silverado else + (3.8060, .8282, .1702, 0.) if cp.carFingerprint == CAR.CHEVROLET_TRAX else None) + if coefficients is not None: + a, b, c, d = coefficients + lateral = np.arange(-5., 5., .01) + torque = [] + for value in lateral: + x = a * value + curve = np.sign(x) * (1 / (1 + math.exp(-abs(x))) - .5) + torque.append(float(curve * b + value * c + d)) + parent.torque_from_lateral_accel = lambda value, tune: np.interp(value, lateral, torque) + parent.lateral_accel_from_torque = lambda value, tune: np.interp(value, torque, lateral) + super().__init__(parent, cp) + + def taper(self, setpoint, speed): + return 1 - .22 * sigmoid((speed - 12.) / 2.5) * sigmoid((.18 - abs(setpoint)) / .05) if self.silverado else 1. + + def feedforward(self, value, setpoint, jerk, speed): + return value * self.taper(setpoint, speed) + + def output(self, value, setpoint, speed): + return value * self.taper(setpoint, speed) diff --git a/openpilot/starpilot/lateral/controller_selection.py b/openpilot/starpilot/lateral/controller_selection.py new file mode 100644 index 0000000000..458256ed03 --- /dev/null +++ b/openpilot/starpilot/lateral/controller_selection.py @@ -0,0 +1,252 @@ +"""Startup-only selection between the standard torque controller and exact vehicle policies.""" + +from __future__ import annotations + +from dataclasses import dataclass +from enum import StrEnum +import json +from pathlib import Path + +from opendbc.car import structs + + +DOCUMENT_KEY = 'LateralControllerSelection' +DOCUMENT_VERSION = 1 +LEARNING_OFF_KEY = 'ForceAutoTuneOff' +MAX_DOCUMENT_BYTES = 4096 +KNOWN_POLICIES = {'HYUNDAI_IONIQ_6': 'hyundai', 'GENESIS_G70_2020': 'hyundai', 'GENESIS_GV70_ELECTRIFIED_1ST_GEN': 'hyundai', 'TOYOTA_COROLLA_TSS2': 'toyota'} +KNOWN_POLICIES.update({'HYUNDAI_SONATA_HYBRID': 'hyundai', 'TOYOTA_RAV4_TSS2': 'toyota'}) +KNOWN_POLICIES.update(dict.fromkeys(('HYUNDAI_PALISADE', 'HYUNDAI_PALISADE_2023'), 'hyundai')) +KNOWN_POLICIES.update(dict.fromkeys(( + 'CHEVROLET_BOLT_CC_2017', 'CHEVROLET_BOLT_CC_2018_2021', 'CHEVROLET_BOLT_CC_2022_2023', + 'CHEVROLET_BOLT_ACC_2022_2023', 'CHEVROLET_BOLT_ACC_2022_2023_PEDAL', +), 'gm')) + +KNOWN_POLICIES.update(dict.fromkeys(( + 'CHEVROLET_VOLT', 'CHEVROLET_VOLT_ASCM', 'CHEVROLET_VOLT_CAMERA', 'CHEVROLET_VOLT_CC', 'CHEVROLET_VOLT_2019', +), 'gm')) + +KNOWN_POLICIES.update(dict.fromkeys(( + 'CHEVROLET_MALIBU_ASCM', 'BUICK_LACROSSE_ASCM', 'BUICK_LACROSSE_ASCM_19US', + 'CADILLAC_ESCALADE_ASCM', 'CADILLAC_ESCALADE_ESV_2019_ASCM', 'CHEVROLET_SUBURBAN_ASCM', +), 'gm')) + +KNOWN_POLICIES.update(dict.fromkeys(( + 'CADILLAC_XT4', 'CHEVROLET_TRAVERSE', 'CADILLAC_XT5', 'CADILLAC_XT6', + 'BUICK_BABYENCLAVE', 'CHEVROLET_BLAZER', 'CHEVROLET_MALIBU_SDGM', +), 'gm')) + +KNOWN_POLICIES.update(dict.fromkeys(( + 'CADILLAC_CT6_CC', 'CADILLAC_XT4_CC', 'CADILLAC_XT5_CC', 'CHEVROLET_EQUINOX_CC', + 'CHEVROLET_MALIBU_CC', 'CHEVROLET_MALIBU_HYBRID_CC', 'CHEVROLET_SUBURBAN_CC', 'CHEVROLET_TRAILBLAZER_CC', 'GMC_YUKON_CC', +), 'gm')) + +KNOWN_POLICIES.update(dict.fromkeys(( + 'CHEVROLET_SILVERADO', 'CHEVROLET_EQUINOX', 'CHEVROLET_TRAILBLAZER', 'CHEVROLET_TRAX', + 'GMC_YUKON', 'CHEVROLET_SUBURBAN_CAMERA', 'CHEVROLET_SILVERADO_CC', 'CHEVROLET_SUBURBAN', +), 'gm')) + +KNOWN_POLICIES.update(dict.fromkeys(( + 'HYUNDAI_IONIQ_5', + 'HYUNDAI_IONIQ_EV_LTD', 'HYUNDAI_IONIQ_EV_2020', + 'HYUNDAI_SONATA', + 'HYUNDAI_ELANTRA_2022_NON_SCC', 'HYUNDAI_ELANTRA_HEV_2022_NON_SCC', + 'KIA_XCEED_PHEV', + 'KIA_NIRO_EV', 'KIA_NIRO_PHEV_2022', + 'KIA_STINGER_2022', + 'KIA_FORTE', 'KIA_FORTE_2019_NON_SCC', 'KIA_FORTE_2021_NON_SCC', + 'KIA_EV6', + 'KIA_CARNIVAL_2025', 'KIA_CARNIVAL_HEV_4TH_GEN', + 'HYUNDAI_TUCSON_4TH_GEN', + 'HYUNDAI_KONA_NON_SCC', + 'HYUNDAI_KONA_EV_2022', +), 'hyundai')) + +class ControllerMode(StrEnum): + STANDARD = 'standard' + STARPILOT = 'starpilot' + + +@dataclass(frozen=True) +class ControllerSelection: + mode: ControllerMode + policy: str | None + source: str + + +def policy_for(CP) -> str | None: + """Return only an already-implemented, exact torque policy identity.""" + if CP.notCar: + return None + from openpilot.starpilot.lateral.hkg_vehicle_policy import policy_for as hkg_vehicle_policy + profile = hkg_vehicle_policy(CP) + if profile is not None: + return profile + from openpilot.starpilot.lateral.corolla_tss2_policy import supported_cp as corolla_supported + from openpilot.starpilot.lateral.genesis_g70_policy import supported_cp as g70_supported + from openpilot.starpilot.lateral.genesis_gv70_policy import supported_cp as gv70_supported + from openpilot.starpilot.lateral.bolt_policy import supported_cp as bolt_supported + from openpilot.starpilot.lateral.volt_policy import supported_cp as volt_supported + + from openpilot.starpilot.lateral.ascm_policy import supported_cp as ascm_supported + + from openpilot.starpilot.lateral.sdgm_policy import supported_cp as sdgm_supported + + from openpilot.starpilot.lateral.ordinary_cc_policy import supported_cp as cc_supported + + from openpilot.starpilot.lateral.camera_policy import supported_cp as camera_supported + + from openpilot.starpilot.lateral.suburban_policy import supported_cp as suburban_supported + + from opendbc.car.gm.values import is_silverado_cc_pedal_profile + if suburban_supported(CP): + return 'suburban' + if camera_supported(CP): + return 'silverado_cc' if is_silverado_cc_pedal_profile(CP) else 'ordinary_camera' + if cc_supported(CP): + return 'ordinary_cc' + if sdgm_supported(CP): + return 'ordinary_sdgm' + if ascm_supported(CP): + return 'ordinary_ascm' + if volt_supported(CP): + return 'volt' + if bolt_supported(CP): + return 'bolt' + if corolla_supported(CP): + return 'corolla_tss2' + from openpilot.starpilot.lateral.rav4_tss2_policy import supported_cp as rav4_supported + if rav4_supported(CP): + return 'rav4_tss2' + from openpilot.starpilot.lateral.palisade_policy import supported_cp as palisade_supported + from openpilot.starpilot.lateral.sonata_hybrid_policy import supported_cp as sonata_hybrid_supported + if sonata_hybrid_supported(CP): + return 'sonata_hybrid' + if palisade_supported(CP): + return 'palisade' + if gv70_supported(CP): + return 'genesis_gv70_electrified' + if g70_supported(CP): + return 'genesis_g70_2020' + if (str(CP.carFingerprint) == 'HYUNDAI_IONIQ_6' and str(CP.brand) == 'hyundai' and + CP.steerControlType == structs.CarParams.SteerControlType.torque and + CP.lateralTuning.which() == 'torque' and not CP.dashcamOnly and not CP.passive): + return 'ioniq6' + return None + + +def turn_assist_supported(CP) -> bool: + return bool(CP.steerControlType == structs.CarParams.SteerControlType.torque and + CP.lateralTuning.which() == 'torque' and not CP.notCar and not CP.passive and not CP.dashcamOnly and + any(config.safetyModel not in (structs.CarParams.SafetyModel.noOutput, + structs.CarParams.SafetyModel.silent, structs.CarParams.SafetyModel.allOutput) + for config in CP.safetyConfigs)) + + +def model_turn_assist_supported(CP) -> bool: + return turn_assist_supported(CP) and policy_for(CP) == 'ioniq6' + + +def default_selection(CP) -> ControllerSelection: + policy = policy_for(CP) + from openpilot.starpilot.lateral.hkg_vehicle_policy import PROFILES + forte_non_scc = policy == 'kia_forte' and str(CP.carFingerprint) in ('KIA_FORTE_2019_NON_SCC', 'KIA_FORTE_2021_NON_SCC') + mode = ControllerMode.STARPILOT if policy and (policy not in PROFILES or forte_non_scc) else ControllerMode.STANDARD + return ControllerSelection(mode, policy, 'default') + + +def _unique_pairs(pairs): + result = {} + for key, value in pairs: + if key in result: + raise ValueError('Duplicate controller selection field') + result[key] = value + return result + + +def parse_document(raw: bytes) -> dict: + """Reject unknown, ambiguous or partial documents before any saved edit.""" + if len(raw) > MAX_DOCUMENT_BYTES: + raise ValueError('Oversize controller selection') + document = json.loads(raw.decode('utf-8'), object_pairs_hook=_unique_pairs) + if (not isinstance(document, dict) or set(document) != {'version', 'vehicles'} or + type(document['version']) is not int or document['version'] != DOCUMENT_VERSION): + raise ValueError('Invalid controller selection version') + vehicles = document['vehicles'] + if not isinstance(vehicles, dict) or len(vehicles) > len(KNOWN_POLICIES): + raise ValueError('Invalid controller selection vehicles') + for fingerprint, choice in vehicles.items(): + if (fingerprint not in KNOWN_POLICIES or not isinstance(choice, dict) or set(choice) != {'brand', 'mode'} or + choice['brand'] != KNOWN_POLICIES[fingerprint] or type(choice['mode']) is not str or + choice['mode'] not in (ControllerMode.STANDARD, ControllerMode.STARPILOT)): + raise ValueError('Invalid controller selection choice') + return document + + +def replace_mode(raw: bytes | None, CP, mode: ControllerMode) -> bytes: + """Change only one exact platform's saved choice, preserving other entries.""" + if policy_for(CP) is None: + raise ValueError('No exact torque policy for CarParams') + mode = ControllerMode(mode) + document = {'version': DOCUMENT_VERSION, 'vehicles': {}} if raw is None else parse_document(raw) + vehicles = dict(document['vehicles']) + vehicles[str(CP.carFingerprint)] = {'brand': str(CP.brand), 'mode': mode.value} + encoded = json.dumps({'version': DOCUMENT_VERSION, 'vehicles': vehicles}, + sort_keys=True, separators=(',', ':'), allow_nan=False).encode() + if len(encoded) > MAX_DOCUMENT_BYTES: + raise ValueError('Oversize controller selection') + return encoded + + +def selection_from_bytes(CP, raw: bytes | None) -> ControllerSelection: + default = default_selection(CP) + if raw is None or default.policy is None: + return default + try: + choice = parse_document(raw)['vehicles'].get(str(CP.carFingerprint)) + if choice is None or choice['brand'] != str(CP.brand): + return default + return ControllerSelection(ControllerMode(choice['mode']), default.policy, 'saved') + except (UnicodeError, ValueError, TypeError, OverflowError): + return ControllerSelection(default.mode, default.policy, 'invalid') + + +def read_selection(params, CP) -> ControllerSelection: + """Read once at controls startup; a later settings write cannot change this controller.""" + if policy_for(CP) is None: + return default_selection(CP) + try: + with Path(params.get_param_path(DOCUMENT_KEY)).open('rb') as source: + return selection_from_bytes(CP, source.read(MAX_DOCUMENT_BYTES + 1)) + except FileNotFoundError: + return default_selection(CP) + except OSError: + default = default_selection(CP) + return ControllerSelection(default.mode, default.policy, 'invalid') + + +def controller_uses_native_learning(selection: ControllerSelection) -> bool: + # RAV4 adds only two shaping stages to the modern native controller. + return selection.mode == ControllerMode.STANDARD or selection.policy == "rav4_tss2" + + +def learning_allowed(params, CP, *, selection: ControllerSelection | None = None) -> bool: + """Latch the exact controller's learning policy at each process startup. + + An unrelated vehicle must not inherit another platform's learning preference. + Callers that already chose their controller supply that same startup selection. + """ + from openpilot.starpilot.lateral.learning_preferences import read_preference + preference = read_preference(params, CP) + if not preference.valid or not preference.enabled: + return False + if policy_for(CP) is None: + return True + selected = read_selection(params, CP) if selection is None else selection + from openpilot.starpilot.lateral.gm_geometry_runtime import read_geometry + geometry = read_geometry(params, CP) + if geometry.force_off: + return False + if geometry.force_auto: + return selected.source != 'invalid' + return controller_uses_native_learning(selected) and (selected.policy != "rav4_tss2" or selected.source != "invalid") diff --git a/openpilot/starpilot/lateral/corolla_tss2_policy.py b/openpilot/starpilot/lateral/corolla_tss2_policy.py new file mode 100644 index 0000000000..f05ebcfe06 --- /dev/null +++ b/openpilot/starpilot/lateral/corolla_tss2_policy.py @@ -0,0 +1,194 @@ +"""Default Corolla TSS2 torque shaping from StarPilot ba901b5f (no FLM overrides).""" + +import math +from collections import deque + +import numpy as np + +from openpilot.cereal import log +from opendbc.car import structs +from opendbc.car.lateral import get_friction +from opendbc.car.toyota.values import CAR +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.pid import PIDController +from openpilot.starpilot.lateral.torque_shaping import ( + INTERP_SPEEDS, + KP_INTERP, + KI, + LAT_ACCEL_REQUEST_BUFFER_SECONDS, + LP_FILTER_CUTOFF_HZ, + MAX_LAT_JERK_UP, + LOW_SPEED_X, + LOW_SPEED_Y, + MIN_SPEED, + JERK_GAIN, + FF_ROLL_OFFSET_FADE_BP, + FF_ROLL_OFFSET_FADE_V, + UNWIND_D_DES_THRESHOLD, + UNWIND_LAT_ACCEL_NEAR_ZERO, + center_chatter_friction_jerk_deadzone, + sigmoid as _sigmoid, + get_standard_friction_threshold, +) + + +def supported_cp(CP): + return ( + CP.brand == "toyota" + and CP.carFingerprint == CAR.TOYOTA_COROLLA_TSS2 + and CP.steerControlType == structs.CarParams.SteerControlType.torque + and CP.lateralTuning.which() == "torque" + and not CP.dashcamOnly + and not CP.passive + ) + + +TOYOTA_COROLLA_TSS2_PHASE_SCALE = 0.12 +TOYOTA_COROLLA_TSS2_TURN_IN_FF_BOOST = 0.035 +TOYOTA_COROLLA_TSS2_UNWIND_FF_REDUCTION = 0.06 +TOYOTA_COROLLA_TSS2_CURVE_LAT_ONSET = 0.24 +TOYOTA_COROLLA_TSS2_CURVE_LAT_WIDTH = 0.1 +TOYOTA_COROLLA_TSS2_SPEED_ONSET = 4.0 +TOYOTA_COROLLA_TSS2_SPEED_ONSET_WIDTH = 1.5 +TOYOTA_COROLLA_TSS2_SPEED_CUTOFF = 24.0 +TOYOTA_COROLLA_TSS2_SPEED_CUTOFF_WIDTH = 3.0 +TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_MAX = 0.3 +TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT = 0.18 +TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08 +TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED = 4.5 +TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 1.5 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_GAIN = 0.12 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_ONSET = 12.0 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.0 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF = 25.0 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF_WIDTH = 3.0 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT = 0.24 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.1 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK = 0.25 +TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK_WIDTH = 0.1 + + +def get_toyota_corolla_tss2_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + """Add a small, transition-only turn-in correction for Corolla TSS2 torque EPS.""" + if desired_lateral_accel == 0.0: + return 1.0 + phase = math.tanh(desired_lateral_accel * desired_lateral_jerk / TOYOTA_COROLLA_TSS2_PHASE_SCALE) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + curve_weight = _sigmoid((abs(desired_lateral_accel) - TOYOTA_COROLLA_TSS2_CURVE_LAT_ONSET) / TOYOTA_COROLLA_TSS2_CURVE_LAT_WIDTH) + speed_weight = _sigmoid((v_ego - TOYOTA_COROLLA_TSS2_SPEED_ONSET) / TOYOTA_COROLLA_TSS2_SPEED_ONSET_WIDTH) * _sigmoid( + (TOYOTA_COROLLA_TSS2_SPEED_CUTOFF - v_ego) / TOYOTA_COROLLA_TSS2_SPEED_CUTOFF_WIDTH + ) + boost = TOYOTA_COROLLA_TSS2_TURN_IN_FF_BOOST + unwind_reduction = TOYOTA_COROLLA_TSS2_UNWIND_FF_REDUCTION + return 1.0 + curve_weight * speed_weight * (boost * turn_in_weight - unwind_reduction * unwind_weight) + + +def get_toyota_corolla_tss2_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + """Reduce center-only friction chasing on highway-sized model corrections.""" + speed_weight = _sigmoid( + (v_ego - TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_ONSET) / TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH + ) * _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF - v_ego) / TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_SPEED_CUTOFF_WIDTH) + center_weight = _sigmoid( + (TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) / TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_LAT_WIDTH + ) + calm_weight = _sigmoid( + (TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK - abs(desired_lateral_jerk)) / TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_JERK_WIDTH + ) + gain = TOYOTA_COROLLA_TSS2_CENTER_FRICTION_THRESHOLD_GAIN + return get_standard_friction_threshold(v_ego) * (1.0 + gain * speed_weight * center_weight * calm_weight) + + +def get_toyota_corolla_tss2_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + """Taper only near-center crawl-speed torque during manual handoff.""" + center_weight = _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH) + low_speed_weight = _sigmoid((TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED - v_ego) / TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH) + reduction = TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_MAX * center_weight * low_speed_weight + return max(1.0 - reduction, 0.65) + + +class CorollaTSS2TorquePolicy: + def __init__(self, parent, CP): + if not supported_cp(CP): + raise ValueError("Corolla TSS2 policy does not match CarParams") + self.parent = parent + self.dt = parent.dt + parent.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1 / self.dt) + parent.update_limits() + self.request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) + self.curvature_request_buffer = deque([0.0] * self.request_buffer_len, maxlen=self.request_buffer_len) + self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) + self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) + self.previous_measurement = 0.0 + self.prev_desired_lateral_accel = 0.0 + self.prev_steering_pressed = False + self.low_speed_reset_threshold = max(CP.minSteerSpeed, 0.3) + + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + parent = self.parent + pid_log = log.ControlsState.LateralTorqueState.new_message() + pid_log.version = 2 + measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + measurement = measured_curvature * CS.vEgo**2 + future_desired_lateral_accel = desired_curvature * CS.vEgo**2 + if not active: + parent.pid.reset() + self.curvature_request_buffer.append(desired_curvature) + self.previous_measurement = measurement + self.measurement_rate_filter.x = 0.0 + self.jerk_filter.x = 0.0 + self.prev_desired_lateral_accel = future_desired_lateral_accel + self.prev_steering_pressed = CS.steeringPressed + pid_log.active = False + return 0.0, 0.0, pid_log + + if self.prev_steering_pressed and not CS.steeringPressed: + parent.pid.i *= 0.8 + roll_offset_fade = float(np.interp(CS.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V)) + roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY * roll_offset_fade + curvature_deadzone = abs(VM.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), CS.vEgo, 0.0)) + lateral_accel_deadzone = curvature_deadzone * CS.vEgo**2 + delay_frames = int(np.clip(lat_delay / self.dt, 1, self.request_buffer_len)) + expected_lateral_accel = self.curvature_request_buffer[-delay_frames] * CS.vEgo**2 + self.curvature_request_buffer.append(desired_curvature) + raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt) + raw_lateral_jerk = float(np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + desired_lateral_jerk = float(np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation + setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay + desired_lateral_accel_rate = (setpoint - self.prev_desired_lateral_accel) / self.dt + unwind_detected = desired_lateral_accel_rate < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO + self.prev_desired_lateral_accel = setpoint + measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt) + measurement_rate = float(np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + self.previous_measurement = measurement + low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2 + current_kp = np.interp(CS.vEgo, parent.pid._k_p[0], parent.pid._k_p[1]) + error = setpoint - measurement + error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3)) + pid_log.error = float(error_with_lsf) + ff = gravity_adjusted_future_lateral_accel - parent.torque_params.latAccelOffset * roll_offset_fade + ff *= get_toyota_corolla_tss2_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) + friction_threshold = get_toyota_corolla_tss2_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + friction_jerk_deadzone = center_chatter_friction_jerk_deadzone(CS.vEgo, setpoint, 0.0) + friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - friction_jerk_deadzone, 0.0), desired_lateral_jerk) + ff += get_friction(error_with_lsf + JERK_GAIN * friction_jerk, lateral_accel_deadzone, friction_threshold, parent.torque_params) + if CS.vEgo < self.low_speed_reset_threshold: + parent.pid.reset() + freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < self.low_speed_reset_threshold or unwind_detected + output_lataccel = parent.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator) + output_torque = parent.torque_from_lateral_accel(output_lataccel, parent.torque_params) + output_torque *= get_toyota_corolla_tss2_center_output_scale(setpoint, CS.vEgo) + pid_log.active = True + pid_log.p = float(parent.pid.p) + pid_log.i = float(parent.pid.i) + pid_log.d = float(parent.pid.d) + pid_log.f = float(parent.pid.f) + pid_log.output = float(-output_torque) + pid_log.actualLateralAccel = float(measurement) + pid_log.desiredLateralAccel = float(setpoint) + pid_log.desiredLateralJerk = float(desired_lateral_jerk) + pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) + self.prev_steering_pressed = CS.steeringPressed + return -output_torque, 0.0, pid_log diff --git a/openpilot/starpilot/lateral/gain_runtime.py b/openpilot/starpilot/lateral/gain_runtime.py new file mode 100644 index 0000000000..9758a12bb7 --- /dev/null +++ b/openpilot/starpilot/lateral/gain_runtime.py @@ -0,0 +1,68 @@ +"""Startup-selected manual gain ownership; torque parameter learning stays independent.""" + +from __future__ import annotations + +import time + +from openpilot.starpilot.lateral.torque_runtime import gm_manual_supported_cp as supported_cp +from openpilot.starpilot.lateral.torque_supported import BOLT_VEHICLES +from openpilot.starpilot.lateral.torque_settings import DOCUMENT_KEY, MAX_DOCUMENT_BYTES, GainBasis, parse_document +from openpilot.starpilot.saved_source import read_saved + +REFRESH_NS = 1_000_000_000 + + +def gain_basis(cp, controller) -> GainBasis: + tune = cp.lateralTuning.torque + return GainBasis(str(controller.controller_mode), + (tuple(controller.pid._k_p[0]), tuple(controller.pid._k_p[1])), + (tune.latAccelFactor, tune.latAccelOffset, tune.friction)) + + +def _selected(params, cp, basis: GainBasis) -> float | None: + try: + if str(cp.carFingerprint) in BOLT_VEHICLES: + advanced, readable = read_saved(params, "AdvancedLateralTune", 64) + if not readable or advanced != b"1": + return None + raw, readable = read_saved(params, DOCUMENT_KEY, MAX_DOCUMENT_BYTES) + if not readable or raw is None: + return None + profile = parse_document(raw).get(str(cp.carFingerprint)) + if profile is None or profile.gain_basis != basis or profile.proportional_gain.mode != "custom": + return None + return profile.proportional_gain.custom_value + except (OSError, ValueError, TypeError, UnicodeError, OverflowError): + return None + + +class GenericGainOwner: + def __init__(self, params, cp, controller, basis: GainBasis, value: float): + self.params = params + self.cp = cp + self.pid = controller.pid + self.basis = basis + self.last_refresh_ns: int | None = None + self.current_table = basis.source_table + self._apply(value) + + def _apply(self, value: float | None) -> None: + table = self.basis.source_table if value is None else ((0.0,), (value,)) + if table != self.current_table: + self.pid._k_p = [list(row) for row in table] + self.current_table = table + + def refresh(self, *, now_ns: int | None = None) -> None: + now_ns = time.monotonic_ns() if now_ns is None else now_ns + if self.last_refresh_ns is not None and 0 <= now_ns - self.last_refresh_ns < REFRESH_NS: + return + self.last_refresh_ns = now_ns + self._apply(_selected(self.params, self.cp, self.basis)) + + +def create_gain_owner(params, cp, controller) -> GenericGainOwner | None: + if not supported_cp(cp): + return None + basis = gain_basis(cp, controller) + value = _selected(params, cp, basis) + return None if value is None else GenericGainOwner(params, cp, controller, basis, value) diff --git a/openpilot/starpilot/lateral/genesis_g70_policy.py b/openpilot/starpilot/lateral/genesis_g70_policy.py new file mode 100644 index 0000000000..26f0b823e9 --- /dev/null +++ b/openpilot/starpilot/lateral/genesis_g70_policy.py @@ -0,0 +1,436 @@ +"""Default Genesis G70 2020 torque shaping from StarPilot ba901b5f (no FLM overrides).""" + +import math +from collections import deque + +import numpy as np + +from openpilot.starpilot.lateral.torque_extension import apply_turn_assist +from openpilot.cereal import log +from opendbc.car import structs +from opendbc.car.lateral import get_friction +from opendbc.car.hyundai.values import CAR +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY, CV +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.pid import PIDController +from openpilot.starpilot.lateral.torque_shaping import ( + INTERP_SPEEDS, + KP_INTERP, + KI, + LAT_ACCEL_REQUEST_BUFFER_SECONDS, + LP_FILTER_CUTOFF_HZ, + MAX_LAT_JERK_UP, + LOW_SPEED_X, + LOW_SPEED_Y, + MIN_SPEED, + JERK_GAIN, + FF_ROLL_OFFSET_FADE_BP, + FF_ROLL_OFFSET_FADE_V, + UNWIND_D_DES_THRESHOLD, + UNWIND_LAT_ACCEL_NEAR_ZERO, + center_chatter_friction_jerk_deadzone, + sigmoid as _sigmoid, + get_standard_friction_threshold, +) + + +def supported_cp(CP): + return ( + CP.brand == "hyundai" + and CP.carFingerprint == CAR.GENESIS_G70_2020 + and CP.steerControlType == structs.CarParams.SteerControlType.torque + and CP.lateralTuning.which() == "torque" + and not CP.dashcamOnly + and not CP.passive + ) + +GENESIS_G70_FRICTION_THRESHOLD_GAIN = 0.10 +GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION = 0.50 +GENESIS_G70_CURVE_TURN_IN_SPEED_BP = [20.0, 25.0] +GENESIS_G70_CURVE_TURN_IN_LAT_BP = [0.35, 0.70] +GENESIS_G70_FRICTION_THRESHOLD_SPEED_BP = [10.0, 20.0] +GENESIS_G70_FRICTION_THRESHOLD_SPEED_V = [1.0, 2.0] +GENESIS_G70_FRICTION_SPEED_ONSET = 10.0 +GENESIS_G70_FRICTION_SPEED_ONSET_WIDTH = 3.0 +GENESIS_G70_FRICTION_SPEED_CUTOFF = 35.0 +GENESIS_G70_FRICTION_SPEED_CUTOFF_WIDTH = 6.0 +GENESIS_G70_FRICTION_CENTER_LAT = 0.28 +GENESIS_G70_FRICTION_CENTER_LAT_WIDTH = 0.10 +GENESIS_G70_FRICTION_CALM_JERK = 0.35 +GENESIS_G70_FRICTION_CALM_JERK_WIDTH = 0.10 +GENESIS_G70_FRICTION_JERK_DEADZONE_MAX = 0.39 +GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30 +GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08 +GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0 +GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5 +GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_MAX = 0.26 +GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED = 35.0 * CV.MPH_TO_MS +GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS +GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT = 0.35 +GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.15 +GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF = 1.75 +GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF_WIDTH = 0.30 +GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK = 0.20 +GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH = 0.12 +GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.30 +GENESIS_G70_CENTER_OUTPUT_TAPER_LAT = 0.30 +GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10 +GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED = 18.0 +GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 3.0 +GENESIS_G70_LOW_SPEED_CENTER_TAPER_MAX = 0.06 +GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT = 0.14 +GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.05 +GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 7.5 +GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.2 +GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_MAX = 0.24 +GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED = 6.0 +GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED_WIDTH = 1.5 +GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR = 7.0 +GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR_WIDTH = 3.0 +GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL = 8.0 +GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL_WIDTH = 4.0 +GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_BLEND = 0.50 +GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_REDUCTION = 0.85 +GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT = 0.14 +GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH = 0.05 +GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.0 +GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5 +GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.34 +GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.13 +GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.17 +GENESIS_G70_UNWIND_FF_JERK = 0.08 +GENESIS_G70_UNWIND_FF_JERK_WIDTH = 0.11 +GENESIS_G70_UNWIND_FF_SPEED = 18.0 +GENESIS_G70_UNWIND_FF_SPEED_WIDTH = 3.0 +GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.22 +GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED = 50.0 * CV.MPH_TO_MS +GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS +GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18 +GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH = 0.15 +GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK = 0.15 +GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH = 0.10 +GENESIS_G70_OUTPUT_SMOOTHING_SPEED = 40.0 * CV.MPH_TO_MS +GENESIS_G70_OUTPUT_SMOOTHING_SPEED_WIDTH = 6.0 * CV.MPH_TO_MS +GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT = 0.42 +GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH = 0.14 +GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC = 0.18 +GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC = 0.10 +GENESIS_G70_OUTPUT_SMOOTHING_RELEASE_RC = 0.03 +GENESIS_G70_OUTPUT_SMOOTHING_OVERSHOOT = 0.08 +GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45 +GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0 +GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0 + +def get_genesis_g70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, + desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_standard_friction_threshold(v_ego) + base_threshold *= np.interp(v_ego, GENESIS_G70_FRICTION_THRESHOLD_SPEED_BP, GENESIS_G70_FRICTION_THRESHOLD_SPEED_V) + speed_onset = _sigmoid((v_ego - GENESIS_G70_FRICTION_SPEED_ONSET) / GENESIS_G70_FRICTION_SPEED_ONSET_WIDTH) + speed_cutoff = _sigmoid((GENESIS_G70_FRICTION_SPEED_CUTOFF - v_ego) / GENESIS_G70_FRICTION_SPEED_CUTOFF_WIDTH) + center_weight = _sigmoid((GENESIS_G70_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) / + GENESIS_G70_FRICTION_CENTER_LAT_WIDTH) + calm_jerk_weight = _sigmoid((GENESIS_G70_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) / + GENESIS_G70_FRICTION_CALM_JERK_WIDTH) + gain = (GENESIS_G70_FRICTION_THRESHOLD_GAIN * speed_onset * speed_cutoff * + center_weight * calm_jerk_weight) + return base_threshold * (1.0 + gain) + +def get_genesis_g70_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float, + desired_lateral_jerk: float = 0.0, + measured_lateral_accel: float = 0.0) -> float: + speed_weight = _sigmoid((v_ego - GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED) / + GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH) + center_weight = _sigmoid((GENESIS_G70_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) / + GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH) + deadzone = GENESIS_G70_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight + + if desired_lateral_accel * desired_lateral_jerk > 0.0: + turn_in_weight = (np.interp(v_ego, GENESIS_G70_CURVE_TURN_IN_SPEED_BP, [0.0, 1.0]) * + np.interp(abs(desired_lateral_accel), GENESIS_G70_CURVE_TURN_IN_LAT_BP, [0.0, 1.0])) + deadzone += (GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION * turn_in_weight * + max(abs(desired_lateral_jerk) - deadzone, 0.0)) + + overshoot = max(abs(measured_lateral_accel) - abs(desired_lateral_accel), 0.0) + if (desired_lateral_accel * desired_lateral_jerk < 0.0 and + desired_lateral_accel * measured_lateral_accel > 0.0 and overshoot > 0.0): + curve_speed_weight = _sigmoid( + (max(v_ego, 0.0) - GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED) / + GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH + ) + curve_onset_weight = _sigmoid( + (abs(desired_lateral_accel) - GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT) / + GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_WIDTH + ) + curve_cutoff_weight = _sigmoid( + (GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF - abs(desired_lateral_accel)) / + GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_LAT_CUTOFF_WIDTH + ) + jerk_weight = _sigmoid( + (abs(desired_lateral_jerk) - GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK) / + GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH + ) + overshoot_weight = _sigmoid((overshoot - 0.08) / 0.10) + boundary_weight = get_genesis_g70_overshoot_blend(desired_lateral_accel, measured_lateral_accel) + boundary_weight *= min(abs(desired_lateral_jerk) / 0.15, 1.0) + deadzone += (GENESIS_G70_CURVE_UNWIND_FRICTION_JERK_DEADZONE_MAX * curve_speed_weight * + curve_onset_weight * curve_cutoff_weight * jerk_weight * overshoot_weight * boundary_weight) + return deadzone + +def get_genesis_g70_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sigmoid((v_ego - GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED) / + GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((GENESIS_G70_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH) + reduction = GENESIS_G70_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight + low_speed_weight = _sigmoid((GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) / + GENESIS_G70_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH) + low_speed_center_weight = _sigmoid((GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + GENESIS_G70_LOW_SPEED_CENTER_TAPER_LAT_WIDTH) + reduction += GENESIS_G70_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight + return 1.0 - reduction + +def get_genesis_g70_low_speed_angle_damping(desired_angle_deg: float, actual_angle_deg: float, + current_output_torque: float, v_ego: float) -> float: + angle_error = desired_angle_deg - actual_angle_deg + speed_weight = _sigmoid((GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED - max(v_ego, 0.0)) / + GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED_WIDTH) + error_weight = _sigmoid((abs(angle_error) - GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR) / + GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ERROR_WIDTH) + actual_angle_weight = _sigmoid((abs(actual_angle_deg) - GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL) / + GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_ACTUAL_WIDTH) + damping_torque = math.copysign( + GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_MAX * speed_weight * error_weight * actual_angle_weight, + -angle_error, + ) + if abs(damping_torque) < 1e-4: + return current_output_torque + if current_output_torque * damping_torque >= 0.0: + damping_torque *= GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_BLEND + return float(np.clip(current_output_torque + damping_torque, -1.0, 1.0)) + +def get_genesis_g70_low_speed_output_limit(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sigmoid((GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED - max(v_ego, 0.0)) / + GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH) + center_weight = _sigmoid((GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT - abs(desired_lateral_accel)) / + GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH) + return max(0.05, 1.0 - GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_REDUCTION * speed_weight * center_weight) + +def get_genesis_g70_angle_output_scale(steering_angle_deg: float, output_torque: float) -> float: + """Ease G70 torque as it approaches the EPS high-angle protection threshold.""" + if steering_angle_deg == 0.0 or output_torque * steering_angle_deg <= 0.0: + return 1.0 + + angle_weight = _sigmoid((abs(steering_angle_deg) - GENESIS_G70_ANGLE_OUTPUT_TAPER_START) / + GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH) + return 1.0 - ((1.0 - GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN) * angle_weight) + +def get_genesis_g70_overshoot_blend(setpoint: float, measured_lateral_accel: float) -> float: + if setpoint * measured_lateral_accel <= 0.0: + return 0.0 + overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0) + return float(np.interp(abs(setpoint), [0.10, 0.35], [0.0, 1.0]) * min(overshoot / 0.15, 1.0)) + +def get_genesis_g70_unwind_ff_scale(setpoint: float, measured_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0: + return 1.0 + + overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0) + if overshoot <= 0.0: + return 1.0 + overshoot_weight = _sigmoid((overshoot - GENESIS_G70_UNWIND_FF_OVERSHOOT) / + GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH) + jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_UNWIND_FF_JERK) / + GENESIS_G70_UNWIND_FF_JERK_WIDTH) + speed_weight = _sigmoid((v_ego - GENESIS_G70_UNWIND_FF_SPEED) / + GENESIS_G70_UNWIND_FF_SPEED_WIDTH) + boundary_weight = get_genesis_g70_overshoot_blend(setpoint, measured_lateral_accel) + boundary_weight *= min(abs(desired_lateral_jerk) / 0.15, 1.0) + return 1.0 - GENESIS_G70_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight * speed_weight * boundary_weight + +def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + if (setpoint == 0.0 or setpoint * measured_lateral_accel <= 0.0 or + abs(measured_lateral_accel) <= abs(setpoint)): + return 1.0 + tracking_error = abs(measured_lateral_accel - setpoint) + speed_weight = _sigmoid((v_ego - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED) / + GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH) + error_weight = _sigmoid((tracking_error - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR) / + GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH) + jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK) / + GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH) + unwind_jerk = -math.copysign(1.0, setpoint) * desired_lateral_jerk + phase_weight = float(np.interp(unwind_jerk, [0.0, 0.15], [0.45, 1.0])) + reduction = (GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX * speed_weight * error_weight * + (0.35 + (0.65 * jerk_weight)) * phase_weight * + get_genesis_g70_overshoot_blend(setpoint, measured_lateral_accel)) + return 1.0 - reduction + +def get_genesis_g70_stabilized_output(output_torque: float, prev_output_torque: float, + desired_lateral_accel: float, measured_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float, dt: float) -> float: + speed_weight = _sigmoid((max(v_ego, 0.0) - GENESIS_G70_OUTPUT_SMOOTHING_SPEED) / + GENESIS_G70_OUTPUT_SMOOTHING_SPEED_WIDTH) + center_weight = _sigmoid((GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT - abs(desired_lateral_accel)) / + GENESIS_G70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH) + response_time = (GENESIS_G70_OUTPUT_SMOOTHING_CURVE_RC * (1.0 - center_weight) + + GENESIS_G70_OUTPUT_SMOOTHING_CENTER_RC * center_weight) + + measured_overshoot = (desired_lateral_accel * measured_lateral_accel > 0.0 and + abs(measured_lateral_accel) > abs(desired_lateral_accel) + GENESIS_G70_OUTPUT_SMOOTHING_OVERSHOOT) + reducing_output = (prev_output_torque * output_torque <= 0.0 or + abs(output_torque) < abs(prev_output_torque)) + if reducing_output or (desired_lateral_accel * desired_lateral_jerk < 0.0 and measured_overshoot): + response_time = GENESIS_G70_OUTPUT_SMOOTHING_RELEASE_RC + + output_alpha = dt / (response_time + dt) + smoothed_output = prev_output_torque + output_alpha * (output_torque - prev_output_torque) + return float(output_torque + speed_weight * (smoothed_output - output_torque)) + + +GENESIS_G70_CENTER_MEASUREMENT_DAMPING_MAX = 0.09 +GENESIS_G70_CENTER_MEASUREMENT_DAMPING_SPEED_BP = [50.0 * CV.MPH_TO_MS, 60.0 * CV.MPH_TO_MS] +GENESIS_G70_CENTER_MEASUREMENT_DAMPING_LAT_BP = [0.15, 0.35] +GENESIS_G70_CENTER_MEASUREMENT_DAMPING_JERK_BP = [0.20, 0.50] + + +def get_genesis_g70_center_measurement_damping_gain(v_ego: float, desired_lateral_accel: float, + measured_lateral_accel: float, desired_lateral_jerk: float) -> float: + speed_weight = np.interp(v_ego, GENESIS_G70_CENTER_MEASUREMENT_DAMPING_SPEED_BP, [0.0, 1.0]) + center_weight = np.interp(max(abs(desired_lateral_accel), abs(measured_lateral_accel)), + GENESIS_G70_CENTER_MEASUREMENT_DAMPING_LAT_BP, [1.0, 0.0]) + jerk_weight = np.interp(abs(desired_lateral_jerk), GENESIS_G70_CENTER_MEASUREMENT_DAMPING_JERK_BP, [1.0, 0.0]) + return float(GENESIS_G70_CENTER_MEASUREMENT_DAMPING_MAX * speed_weight * center_weight * jerk_weight) + + +class GenesisG70TorquePolicy: + def __init__(self, parent, CP): + if not supported_cp(CP): + raise ValueError("Genesis G70 2020 policy does not match CarParams") + self.parent = parent + self.dt = parent.dt + parent.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1 / self.dt) + parent.update_limits() + self.request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) + self.curvature_request_buffer = deque([0.0] * self.request_buffer_len, maxlen=self.request_buffer_len) + self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) + self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) + self.previous_measurement = 0.0 + self.prev_desired_lateral_accel = 0.0 + self.prev_steering_pressed = False + self.prev_output_torque = 0.0 + self.low_speed_reset_threshold = max(CP.minSteerSpeed, 0.3) + + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + parent = self.parent + pid_log = log.ControlsState.LateralTorqueState.new_message() + pid_log.version = 2 + measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + measurement = measured_curvature * CS.vEgo**2 + future_desired_lateral_accel = desired_curvature * CS.vEgo**2 + if not active: + parent.pid.reset() + self.prev_output_torque = 0.0 + self.curvature_request_buffer.append(desired_curvature) + self.previous_measurement = measurement + self.measurement_rate_filter.x = 0.0 + self.jerk_filter.x = 0.0 + self.prev_desired_lateral_accel = future_desired_lateral_accel + self.prev_steering_pressed = CS.steeringPressed + pid_log.active = False + return 0.0, 0.0, pid_log + + if self.prev_steering_pressed and not CS.steeringPressed: + parent.pid.i *= 0.8 + roll_offset_fade = float(np.interp(CS.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V)) + roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY * roll_offset_fade + curvature_deadzone = abs(VM.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), CS.vEgo, 0.0)) + lateral_accel_deadzone = curvature_deadzone * CS.vEgo**2 + delay_frames = int(np.clip(lat_delay / self.dt, 1, self.request_buffer_len)) + expected_lateral_accel = self.curvature_request_buffer[-delay_frames] * CS.vEgo**2 + self.curvature_request_buffer.append(desired_curvature) + raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt) + raw_lateral_jerk = float(np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + desired_lateral_jerk = float(np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation + setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay + desired_lateral_accel_magnitude_rate = (abs(setpoint) - abs(self.prev_desired_lateral_accel)) / self.dt + unwind_detected = desired_lateral_accel_magnitude_rate < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO + self.prev_desired_lateral_accel = setpoint + measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt) + measurement_rate = float(np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + self.previous_measurement = measurement + low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2 + current_kp = np.interp(CS.vEgo, parent.pid._k_p[0], parent.pid._k_p[1]) + error = setpoint - measurement + error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3)) + pid_log.error = float(error_with_lsf) + ff = gravity_adjusted_future_lateral_accel - parent.torque_params.latAccelOffset * roll_offset_fade + ff *= get_genesis_g70_unwind_ff_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo) + friction_threshold = get_genesis_g70_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + if not CS.steeringPressed and not self.prev_steering_pressed: + friction_threshold += get_genesis_g70_traffic_friction_increment( + CS.vEgo, setpoint, desired_lateral_jerk, desired_curvature, measured_curvature) + vehicle_deadzone = get_genesis_g70_friction_jerk_deadzone(CS.vEgo, setpoint, desired_lateral_jerk, measurement) + friction_jerk_deadzone = center_chatter_friction_jerk_deadzone(CS.vEgo, setpoint, vehicle_deadzone) + friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - friction_jerk_deadzone, 0.0), desired_lateral_jerk) + ff += get_friction(error_with_lsf + JERK_GAIN * friction_jerk, lateral_accel_deadzone, friction_threshold, parent.torque_params) + if CS.vEgo < self.low_speed_reset_threshold: + parent.pid.reset() + damping_gain = 0.0 if CS.steeringPressed else get_genesis_g70_center_measurement_damping_gain( + CS.vEgo, setpoint, measurement, desired_lateral_jerk, + ) + parent.pid._k_d = [[0.0], [damping_gain]] + freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < self.low_speed_reset_threshold or unwind_detected + output_lataccel = parent.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator) + output_torque = parent.torque_from_lateral_accel(output_lataccel, parent.torque_params) + output_torque = apply_turn_assist(parent, CS, VM, params, desired_curvature, output_torque) + if not CS.steeringPressed and CS.vEgo < GENESIS_G70_LOW_SPEED_ANGLE_DAMPING_SPEED + 2.0: + desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) + actual_angle = CS.steeringAngleDeg - params.angleOffsetDeg + output_torque = get_genesis_g70_low_speed_angle_damping(desired_angle, actual_angle, output_torque, CS.vEgo) + output_torque *= get_genesis_g70_center_output_scale(setpoint, CS.vEgo) + output_torque *= get_genesis_g70_high_speed_error_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo) + output_torque *= get_genesis_g70_angle_output_scale(CS.steeringAngleDeg, -output_torque) + output_limit = get_genesis_g70_low_speed_output_limit(setpoint, CS.vEgo) + output_torque = float(np.clip(output_torque, -output_limit, output_limit)) + if not CS.steeringPressed: + output_torque = get_genesis_g70_stabilized_output(output_torque, self.prev_output_torque, setpoint, measurement, + desired_lateral_jerk, CS.vEgo, self.dt) + pid_log.active = True + pid_log.p = float(parent.pid.p) + pid_log.i = float(parent.pid.i) + pid_log.d = float(parent.pid.d) + pid_log.f = float(parent.pid.f) + pid_log.output = float(-output_torque) + pid_log.actualLateralAccel = float(measurement) + pid_log.desiredLateralAccel = float(setpoint) + pid_log.desiredLateralJerk = float(desired_lateral_jerk) + pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) + self.prev_steering_pressed = CS.steeringPressed + self.prev_output_torque = float(output_torque) + return -output_torque, 0.0, pid_log + + +GENESIS_G70_TRAFFIC_FRICTION_CURVATURE_BP = [0.0035, 0.0080] + +GENESIS_G70_TRAFFIC_FRICTION_INCREMENT = 0.90 + +GENESIS_G70_TRAFFIC_FRICTION_JERK_BP = [0.15, 0.50] + +GENESIS_G70_TRAFFIC_FRICTION_LAT_BP = [0.12, 0.30] + +GENESIS_G70_TRAFFIC_FRICTION_SPEED_BP = [6.0, 8.0] + +def get_genesis_g70_traffic_friction_increment(v_ego: float, desired_lateral_accel: float, + desired_lateral_jerk: float, future_curvature: float, + measured_curvature: float) -> float: + speed = max(v_ego, 0.0) + speed_weight = np.interp(speed, GENESIS_G70_TRAFFIC_FRICTION_SPEED_BP, [1.0, 0.0]) + request_accel = max(abs(desired_lateral_accel), abs(future_curvature) * speed ** 2) + accel_weight = np.interp(request_accel, GENESIS_G70_TRAFFIC_FRICTION_LAT_BP, [1.0, 0.0]) + curvature = max(abs(future_curvature), abs(measured_curvature), abs(desired_lateral_accel) / max(speed ** 2, 1.0)) + curve_weight = np.interp(curvature, GENESIS_G70_TRAFFIC_FRICTION_CURVATURE_BP, [1.0, 0.0]) + calm_weight = np.interp(abs(desired_lateral_jerk), GENESIS_G70_TRAFFIC_FRICTION_JERK_BP, [1.0, 0.0]) + return float(GENESIS_G70_TRAFFIC_FRICTION_INCREMENT * speed_weight * accel_weight * curve_weight * calm_weight) diff --git a/openpilot/starpilot/lateral/genesis_gv70_policy.py b/openpilot/starpilot/lateral/genesis_gv70_policy.py new file mode 100644 index 0000000000..26ef1853dc --- /dev/null +++ b/openpilot/starpilot/lateral/genesis_gv70_policy.py @@ -0,0 +1,159 @@ +"""First-generation Electrified GV70 torque shaping.""" + +import math +from collections import deque + +import numpy as np + +from openpilot.cereal import log +from opendbc.car import structs +from opendbc.car.lateral import get_friction +from opendbc.car.hyundai.values import CAR +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.pid import PIDController +from openpilot.starpilot.lateral.torque_shaping import ( + INTERP_SPEEDS, + KP_INTERP, + KI, + LAT_ACCEL_REQUEST_BUFFER_SECONDS, + LP_FILTER_CUTOFF_HZ, + MAX_LAT_JERK_UP, + LOW_SPEED_X, + LOW_SPEED_Y, + MIN_SPEED, + JERK_GAIN, + FF_ROLL_OFFSET_FADE_BP, + FF_ROLL_OFFSET_FADE_V, + UNWIND_D_DES_THRESHOLD, + UNWIND_LAT_ACCEL_NEAR_ZERO, + center_chatter_friction_jerk_deadzone, +) + + +from openpilot.starpilot.lateral.gv70_shaping import ( + get_genesis_gv70_friction_threshold, + get_genesis_gv70_friction_jerk_deadzone, + get_genesis_gv70_center_output_scale, + get_genesis_gv70_unwind_ff_scale, + get_genesis_gv70_high_speed_error_scale, + get_genesis_gv70_reversal_output_scale, + get_genesis_gv70_low_speed_center_overshoot_scale, + get_genesis_gv70_stabilized_output, + GENESIS_GV70_MEASUREMENT_DAMPING_SPEED_BP, GENESIS_GV70_MEASUREMENT_DAMPING_V, +) + +def supported_cp(CP): + return ( + CP.brand == "hyundai" + and CP.carFingerprint == CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN + and CP.steerControlType == structs.CarParams.SteerControlType.torque + and CP.lateralTuning.which() == "torque" + and not CP.dashcamOnly + and not CP.passive + and not CP.notCar + ) + + + +class GenesisGV70TorquePolicy: + def __init__(self, parent, CP): + if not supported_cp(CP): + raise ValueError("Electrified GV70 policy does not match CarParams") + self.parent = parent + self.dt = parent.dt + parent.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, + [GENESIS_GV70_MEASUREMENT_DAMPING_SPEED_BP, GENESIS_GV70_MEASUREMENT_DAMPING_V], + rate=1 / self.dt) + parent.update_limits() + self.request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) + self.curvature_request_buffer = deque([0.0] * self.request_buffer_len, maxlen=self.request_buffer_len) + self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) + self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) + self.previous_measurement = 0.0 + self.prev_desired_lateral_accel = 0.0 + self.prev_steering_pressed = False + self.prev_output_torque = 0.0 + self.previous_feedforward = None + self.low_speed_reset_threshold = max(CP.minSteerSpeed, 0.3) + + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + parent = self.parent + pid_log = log.ControlsState.LateralTorqueState.new_message() + pid_log.version = 2 + measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + measurement = measured_curvature * CS.vEgo**2 + future_desired_lateral_accel = desired_curvature * CS.vEgo**2 + if not active: + parent.pid.reset() + self.prev_output_torque = 0.0 + self.previous_feedforward = None + self.curvature_request_buffer.append(desired_curvature) + self.previous_measurement = measurement + self.measurement_rate_filter.x = 0.0 + self.jerk_filter.x = 0.0 + self.prev_desired_lateral_accel = future_desired_lateral_accel + self.prev_steering_pressed = CS.steeringPressed + pid_log.active = False + return 0.0, 0.0, pid_log + + if self.prev_steering_pressed and not CS.steeringPressed: + parent.pid.i *= 0.8 + roll_offset_fade = float(np.interp(CS.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V)) + roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY * roll_offset_fade + curvature_deadzone = abs(VM.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), CS.vEgo, 0.0)) + lateral_accel_deadzone = curvature_deadzone * CS.vEgo**2 + delay_frames = int(np.clip(lat_delay / self.dt, 1, self.request_buffer_len)) + expected_lateral_accel = self.curvature_request_buffer[-delay_frames] * CS.vEgo**2 + self.curvature_request_buffer.append(desired_curvature) + raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt) + raw_lateral_jerk = float(np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + desired_lateral_jerk = float(np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation + setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay + desired_lateral_accel_rate = (setpoint - self.prev_desired_lateral_accel) / self.dt + unwind_detected = desired_lateral_accel_rate < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO + self.prev_desired_lateral_accel = setpoint + measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt) + measurement_rate = float(np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + self.previous_measurement = measurement + low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2 + current_kp = np.interp(CS.vEgo, parent.pid._k_p[0], parent.pid._k_p[1]) + error = setpoint - measurement + error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3)) + pid_log.error = float(error_with_lsf) + ff = gravity_adjusted_future_lateral_accel - parent.torque_params.latAccelOffset * roll_offset_fade + ff *= get_genesis_gv70_unwind_ff_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo) + friction_threshold = get_genesis_gv70_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + vehicle_deadzone = get_genesis_gv70_friction_jerk_deadzone(CS.vEgo, setpoint) + friction_jerk_deadzone = center_chatter_friction_jerk_deadzone(CS.vEgo, setpoint, vehicle_deadzone) + friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - friction_jerk_deadzone, 0.0), desired_lateral_jerk) + ff += get_friction(error_with_lsf + JERK_GAIN * friction_jerk, lateral_accel_deadzone, friction_threshold, parent.torque_params) + if CS.vEgo < self.low_speed_reset_threshold: + parent.pid.reset() + # Filter feedforward before PID so feedback remains immediate. + ff *= get_genesis_gv70_center_output_scale(setpoint, CS.vEgo) + ff *= get_genesis_gv70_low_speed_center_overshoot_scale(setpoint, measurement, CS.vEgo) + ff *= get_genesis_gv70_high_speed_error_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo) + ff *= get_genesis_gv70_reversal_output_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo) + if not CS.steeringPressed and not self.prev_steering_pressed and self.previous_feedforward is not None: + ff = get_genesis_gv70_stabilized_output(ff, self.previous_feedforward, setpoint, + desired_lateral_jerk, CS.vEgo, self.dt) + self.previous_feedforward = ff + error_rate = 0.0 if CS.steeringPressed else -measurement_rate + freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < self.low_speed_reset_threshold or unwind_detected + output_lataccel = parent.pid.update(pid_log.error, error_rate=error_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator) + output_torque = parent.torque_from_lateral_accel(output_lataccel, parent.torque_params) + pid_log.active = True + pid_log.p = float(parent.pid.p) + pid_log.i = float(parent.pid.i) + pid_log.d = float(parent.pid.d) + pid_log.f = float(parent.pid.f) + pid_log.output = float(-output_torque) + pid_log.actualLateralAccel = float(measurement) + pid_log.desiredLateralAccel = float(setpoint) + pid_log.desiredLateralJerk = float(desired_lateral_jerk) + pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) + self.prev_steering_pressed = CS.steeringPressed + self.prev_output_torque = float(output_torque) + return -output_torque, 0.0, pid_log diff --git a/openpilot/starpilot/lateral/gm_geometry_runtime.py b/openpilot/starpilot/lateral/gm_geometry_runtime.py new file mode 100644 index 0000000000..6ae9b77c26 --- /dev/null +++ b/openpilot/starpilot/lateral/gm_geometry_runtime.py @@ -0,0 +1,83 @@ +"""Canonical exact-GM geometry projection and explicit learner intent.""" +from dataclasses import dataclass +import time + +from openpilot.starpilot.lateral.torque_settings import DOCUMENT_KEY, MAX_DOCUMENT_BYTES, GeometryProfile, parse_document +from openpilot.starpilot.saved_source import read_saved + + +@dataclass(frozen=True) +class GeometrySelection: + profile: GeometryProfile | None = None + force_off: bool = False + + @property + def force_auto(self): + return self.profile is not None and self.profile.learning == 'force_auto' and not self.force_off + + +def geometry_basis(CP): + return float(CP.steerRatio), float(CP.steerActuatorDelay) + .2 + + +def read_geometry(params, CP): + from openpilot.starpilot.lateral.torque_runtime import gm_manual_supported_cp + if not gm_manual_supported_cp(CP): + return GeometrySelection() + try: + raw, readable = read_saved(params, DOCUMENT_KEY, MAX_DOCUMENT_BYTES) + if not readable or raw is None: + return GeometrySelection() + profile = parse_document(raw).get(str(CP.carFingerprint)) + tune = CP.lateralTuning.torque + if (profile is None or profile.geometry is None or + profile.basis != (tune.latAccelFactor, tune.latAccelOffset, tune.friction) or + profile.geometry.basis != geometry_basis(CP)): + return GeometrySelection() + from openpilot.starpilot.lateral.learning_preferences import read_preference + preference = read_preference(params, CP) + if not preference.valid: + return GeometrySelection() + return GeometrySelection(profile.geometry, not preference.enabled or profile.geometry.learning == 'force_off') + except (OSError, ValueError, TypeError, UnicodeError, OverflowError): + return GeometrySelection() + + +class GeometryPublicationOwner: + """Project a copy only; estimator messages/state/cache remain upstream-owned.""" + def __init__(self, params, CP): + self.params, self.CP = params, CP + self.last_read_ns = None + self.selected = GeometrySelection() + + def selection(self): + now = time.monotonic_ns() + if self.last_read_ns is None or not 0 <= now - self.last_read_ns < 1_000_000_000: + self.selected = read_geometry(self.params, self.CP) + self.last_read_ns = now + return self.selected + + def parameters(self, original): + selection = self.selection() + profile = selection.profile + if profile is None: + return original + ratio = profile.ratio.custom_value if profile.ratio.mode == 'custom' and not selection.force_auto else None + if selection.force_off and ratio is None: + ratio = profile.basis[0] + if ratio is None and not selection.force_off: + return original + message = original.as_reader().as_builder() + if ratio is not None: + message.vehicleParameters.steerRatio = ratio + if selection.force_off: + message.vehicleParameters.stiffnessFactor = 1. + return message + + def delay(self, original): + profile = self.selection().profile + if profile is None or profile.automatic_delay: + return original + message = original.as_reader().as_builder() + message.lateralDelay.lateralDelay = profile.full_delay.custom_value if profile.full_delay.mode == 'custom' else profile.basis[1] + return message diff --git a/openpilot/starpilot/lateral/gm_ordinary_policy.py b/openpilot/starpilot/lateral/gm_ordinary_policy.py new file mode 100644 index 0000000000..1bbfc54837 --- /dev/null +++ b/openpilot/starpilot/lateral/gm_ordinary_policy.py @@ -0,0 +1,105 @@ +"""Shared ordinary GM torque law, without vehicle callback selection.""" + +from collections import deque +import math + +import numpy as np + +from opendbc.car.lateral import get_friction +from openpilot.cereal import log +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.pid import PIDController +from openpilot.starpilot.lateral import bolt_shaping as shaping +from openpilot.starpilot.lateral.torque_shaping import ( + FF_ROLL_OFFSET_FADE_BP, + FF_ROLL_OFFSET_FADE_V, + JERK_GAIN, + KI, + LAT_ACCEL_REQUEST_BUFFER_SECONDS, + LOW_SPEED_X, + LOW_SPEED_Y, + LP_FILTER_CUTOFF_HZ, + MAX_LAT_JERK_UP, + MIN_SPEED, + UNWIND_D_DES_THRESHOLD, + UNWIND_LAT_ACCEL_NEAR_ZERO, + center_chatter_friction_jerk_deadzone, +) + +class GMOrdinaryTorquePolicy: + def __init__(self, parent, cp): + self.parent = parent + self.dt = parent.dt + parent.pid = PIDController(0.6, KI, rate=1 / self.dt) + parent.update_limits() + self.buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) + self.curvature_buffer = deque([0.0] * self.buffer_len, maxlen=self.buffer_len) + self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) + self.measurement_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) + self.low_speed_reset_threshold = max(cp.minSteerSpeed, 0.3) + self.previous_measurement = 0.0 + self.previous_setpoint = 0.0 + self.previous_pressed = False + + def feedforward(self, value, setpoint, jerk, speed): + return value + + def output(self, value, setpoint, speed): + return value + + def update(self, active, cs, vm, params, safety_limited, curvature, curvature_limited, delay): + parent = self.parent + pid_log = log.ControlsState.LateralTorqueState.new_message() + pid_log.version = 2 + measurement = -vm.calc_curvature(math.radians(cs.steeringAngleDeg - params.angleOffsetDeg), cs.vEgo, params.roll) * cs.vEgo**2 + future = curvature * cs.vEgo**2 + if not active: + parent.pid.reset() + self.curvature_buffer.append(curvature) + self.previous_measurement = measurement + self.measurement_filter.x = self.jerk_filter.x = 0.0 + self.previous_setpoint = future + self.previous_pressed = cs.steeringPressed + pid_log.active = False + return 0.0, 0.0, pid_log + if self.previous_pressed and not cs.steeringPressed: + parent.pid.i *= 0.8 + fade = np.interp(cs.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V) + roll = params.roll * ACCELERATION_DUE_TO_GRAVITY * fade + deadzone = abs(vm.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), cs.vEgo, 0.0)) * cs.vEgo**2 + delay_frames = int(np.clip(delay / self.dt, 1, self.buffer_len)) + expected = self.curvature_buffer[-delay_frames] * cs.vEgo**2 + self.curvature_buffer.append(curvature) + raw_jerk = np.clip((future - expected) / max(delay, self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) + jerk = np.clip(self.jerk_filter.update(raw_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) + setpoint = expected + jerk * delay + unwind = (setpoint - self.previous_setpoint) / self.dt < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO + self.previous_setpoint = setpoint + rate = np.clip(self.measurement_filter.update((measurement - self.previous_measurement) / self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) + self.previous_measurement = measurement + lsf = (np.interp(cs.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(cs.vEgo, MIN_SPEED)) ** 2 + kp = np.interp(cs.vEgo, parent.pid._k_p[0], parent.pid._k_p[1]) + error = (setpoint - measurement) * (1 + lsf / max(kp, 1e-3)) + gravity_adjusted = future - roll + ff = gravity_adjusted - parent.torque_params.latAccelOffset * fade + ff = self.feedforward(ff, setpoint, jerk, cs.vEgo) + threshold = shaping.get_gm_base_friction_threshold(cs.vEgo) + jerk_deadzone = center_chatter_friction_jerk_deadzone(cs.vEgo, setpoint, 0.0) + friction_jerk = math.copysign(max(abs(jerk) - jerk_deadzone, 0.0), jerk) + ff += get_friction(error + JERK_GAIN * friction_jerk, deadzone, threshold, parent.torque_params) + if cs.vEgo < self.low_speed_reset_threshold: + parent.pid.reset() + freeze = safety_limited or cs.steeringPressed or cs.vEgo < self.low_speed_reset_threshold or unwind + pid_log.error = float(error) + output = parent.torque_from_lateral_accel( + parent.pid.update(pid_log.error, error_rate=-rate, speed=cs.vEgo, feedforward=ff, freeze_integrator=freeze), parent.torque_params + ) + output = self.output(output, setpoint, cs.vEgo) + self.previous_pressed = cs.steeringPressed + pid_log.active = True + pid_log.p, pid_log.i, pid_log.d, pid_log.f = map(float, (parent.pid.p, parent.pid.i, parent.pid.d, parent.pid.f)) + pid_log.output = float(-output) + pid_log.actualLateralAccel, pid_log.desiredLateralAccel, pid_log.desiredLateralJerk = map(float, (measurement, setpoint, jerk)) + pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output) < 1e-3, cs, safety_limited, curvature_limited)) + return -float(output), 0.0, pid_log diff --git a/openpilot/starpilot/lateral/gv70_shaping.py b/openpilot/starpilot/lateral/gv70_shaping.py new file mode 100644 index 0000000000..a7d79ac24b --- /dev/null +++ b/openpilot/starpilot/lateral/gv70_shaping.py @@ -0,0 +1,272 @@ +"""Exact default first-generation Electrified GV70 shaping from Dom385428f6. + +Only default source formulas; learned torque calibration remains caller-owned. +See the repository's retained StarPilot attribution and licenses. +""" +import numpy as np + +from openpilot.common.constants import CV +from openpilot.starpilot.lateral.torque_shaping import sigmoid as _sigmoid + + +def get_hkg_canfd_base_friction_threshold(v_ego): + # Dom's default GM schedule tops out at0.27, below HKG's0.39 floor. + return 0.39 + +GENESIS_GV70_FRICTION_THRESHOLD_GAIN = 0.12 + +GENESIS_GV70_FRICTION_SPEED_ONSET = 8.0 * CV.MPH_TO_MS + +GENESIS_GV70_FRICTION_SPEED_ONSET_WIDTH = 4.0 * CV.MPH_TO_MS + +GENESIS_GV70_FRICTION_SPEED_CUTOFF = 60.0 * CV.MPH_TO_MS + +GENESIS_GV70_FRICTION_SPEED_CUTOFF_WIDTH = 10.0 * CV.MPH_TO_MS + +GENESIS_GV70_FRICTION_CENTER_LAT = 0.28 + +GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH = 0.12 + +GENESIS_GV70_FRICTION_CALM_JERK = 0.35 + +GENESIS_GV70_FRICTION_CALM_JERK_WIDTH = 0.10 + +GENESIS_GV70_FRICTION_JERK_DEADZONE_MAX = 0.55 + +GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT = 0.30 + +GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08 + +GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED = 12.0 * CV.MPH_TO_MS + +GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5 * CV.MPH_TO_MS + +GENESIS_GV70_CENTER_OUTPUT_TAPER_MAX = 0.20 + +GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT = 0.30 + +GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10 + +GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED = 22.0 * CV.MPH_TO_MS + +GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 3.0 * CV.MPH_TO_MS + +GENESIS_GV70_UNWIND_FF_REDUCTION_MAX = 0.35 + +GENESIS_GV70_UNWIND_FF_OVERSHOOT = 0.15 + +GENESIS_GV70_UNWIND_FF_OVERSHOOT_WIDTH = 0.18 + +GENESIS_GV70_UNWIND_FF_JERK = 0.10 + +GENESIS_GV70_UNWIND_FF_JERK_WIDTH = 0.10 + +GENESIS_GV70_UNWIND_FF_SPEED = 10.0 * CV.MPH_TO_MS + +GENESIS_GV70_UNWIND_FF_SPEED_WIDTH = 4.0 * CV.MPH_TO_MS + +GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.20 + +GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED = 50.0 * CV.MPH_TO_MS + +GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH = 8.0 * CV.MPH_TO_MS + +GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18 + +GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH = 0.15 + +GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK = 0.15 + +GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH = 0.10 + +GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_MAX = 0.28 + +GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_SPEED = 25.0 * CV.MPH_TO_MS + +GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_SPEED_WIDTH = 5.0 * CV.MPH_TO_MS + +GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_ERROR = 0.30 + +GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_ERROR_WIDTH = 0.16 + +GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_JERK = 0.20 + +GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_JERK_WIDTH = 0.10 + +GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_MAX = 0.28 + +GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED = 18.0 * CV.MPH_TO_MS + +GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_WIDTH = 3.5 * CV.MPH_TO_MS + +GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_CUTOFF = 34.0 * CV.MPH_TO_MS + +GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_CUTOFF_WIDTH = 4.5 * CV.MPH_TO_MS + +GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_CENTER_LAT = 0.22 + +GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_CENTER_LAT_WIDTH = 0.08 + +GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_MIN = 0.06 + +GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT = 0.12 + +GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT_WIDTH = 0.10 + +GENESIS_GV70_OUTPUT_SMOOTHING_SPEED = 38.0 * CV.MPH_TO_MS + +GENESIS_GV70_OUTPUT_SMOOTHING_SPEED_WIDTH = 6.0 * CV.MPH_TO_MS + +GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT = 0.60 + +GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH = 0.16 + +GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_RC = 0.42 + +GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC = 0.14 + +GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC = 0.12 + +GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04 + +GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08 + +GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.55 + +GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.065 + +GENESIS_GV70_MEASUREMENT_DAMPING_SPEED_BP = [10.0 * CV.MPH_TO_MS, 35.0 * CV.MPH_TO_MS, 60.0 * CV.MPH_TO_MS] + +GENESIS_GV70_MEASUREMENT_DAMPING_V = [0.0, 0.08, 0.12] + +def get_genesis_gv70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, + desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_hkg_canfd_base_friction_threshold(v_ego) + speed_onset = _sigmoid((v_ego - GENESIS_GV70_FRICTION_SPEED_ONSET) / GENESIS_GV70_FRICTION_SPEED_ONSET_WIDTH) + speed_cutoff = _sigmoid((GENESIS_GV70_FRICTION_SPEED_CUTOFF - v_ego) / GENESIS_GV70_FRICTION_SPEED_CUTOFF_WIDTH) + center_weight = _sigmoid((GENESIS_GV70_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) / + GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH) + calm_jerk_weight = _sigmoid((GENESIS_GV70_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) / + GENESIS_GV70_FRICTION_CALM_JERK_WIDTH) + gain = (GENESIS_GV70_FRICTION_THRESHOLD_GAIN * speed_onset * speed_cutoff * + center_weight * calm_jerk_weight) + return base_threshold * (1.0 + gain) + +def get_genesis_gv70_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float) -> float: + """Suppress small jerk-driven friction flips around the GV70 lane center.""" + speed_weight = _sigmoid((v_ego - GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED) / + GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED_WIDTH) + center_weight = _sigmoid((GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) / + GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT_WIDTH) + return GENESIS_GV70_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight + +def get_genesis_gv70_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + """Dampen high-speed center corrections without reducing turn authority.""" + speed_weight = _sigmoid((v_ego - GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED) / + GENESIS_GV70_CENTER_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + GENESIS_GV70_CENTER_OUTPUT_TAPER_LAT_WIDTH) + return 1.0 - (GENESIS_GV70_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight) + +def get_genesis_gv70_unwind_ff_scale(setpoint: float, measured_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + """Remove old-turn feedforward when the GV70 has already over-rotated.""" + if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0: + return 1.0 + + overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0) + overshoot_weight = _sigmoid((overshoot - GENESIS_GV70_UNWIND_FF_OVERSHOOT) / + GENESIS_GV70_UNWIND_FF_OVERSHOOT_WIDTH) + jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_GV70_UNWIND_FF_JERK) / + GENESIS_GV70_UNWIND_FF_JERK_WIDTH) + speed_weight = _sigmoid((v_ego - GENESIS_GV70_UNWIND_FF_SPEED) / + GENESIS_GV70_UNWIND_FF_SPEED_WIDTH) + tracking_weight = _genesis_gv70_curve_damping_weight(setpoint, measured_lateral_accel) + return 1.0 - GENESIS_GV70_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight * speed_weight * tracking_weight + +def get_genesis_gv70_high_speed_error_scale(setpoint: float, measured_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + tracking_error = abs(measured_lateral_accel - setpoint) + if tracking_error <= 0.0: + return 1.0 + speed_weight = _sigmoid((v_ego - GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED) / + GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH) + error_weight = _sigmoid((tracking_error - GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR) / + GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH) + jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK) / + GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH) + phase_weight = 1.0 if setpoint * desired_lateral_jerk < 0.0 else 0.45 + reduction = (GENESIS_GV70_HIGH_SPEED_ERROR_DAMPING_MAX * speed_weight * error_weight * + (0.35 + (0.65 * jerk_weight)) * phase_weight) + return 1.0 - reduction * _genesis_gv70_curve_damping_weight(setpoint, measured_lateral_accel) + +def get_genesis_gv70_reversal_output_scale(setpoint: float, measured_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + commanded_unwind = setpoint * desired_lateral_jerk < 0.0 + measured_reversal = setpoint * measured_lateral_accel < 0.0 + if not commanded_unwind and not measured_reversal: + return 1.0 + + tracking_error = abs(measured_lateral_accel - setpoint) + speed_weight = _sigmoid((v_ego - GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_SPEED) / + GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_SPEED_WIDTH) + error_weight = _sigmoid((tracking_error - GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_ERROR) / + GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_ERROR_WIDTH) + jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_JERK) / + GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_JERK_WIDTH) + reduction = (GENESIS_GV70_REVERSAL_OUTPUT_DAMPING_MAX * speed_weight * error_weight * jerk_weight) + return 1.0 - reduction * _genesis_gv70_curve_damping_weight(setpoint, measured_lateral_accel) + +def get_genesis_gv70_low_speed_center_overshoot_scale(setpoint: float, measured_lateral_accel: float, + v_ego: float) -> float: + if abs(setpoint) > 0.08 and setpoint * measured_lateral_accel < 0.0: + return 1.0 + overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0) + if overshoot < GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_MIN: + return 1.0 + overshoot_weight = _sigmoid((overshoot - GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT) / + GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_LAT_WIDTH) + center_weight = _sigmoid((GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_CENTER_LAT - abs(setpoint)) / + GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_CENTER_LAT_WIDTH) + speed_weight = _sigmoid((v_ego - GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED) / + GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_WIDTH) + speed_cutoff = _sigmoid((GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_CUTOFF - v_ego) / + GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_SPEED_CUTOFF_WIDTH) + return 1.0 - (GENESIS_GV70_LOW_SPEED_CENTER_OVERSHOOT_MAX * overshoot_weight * center_weight * + speed_weight * speed_cutoff) + +def get_genesis_gv70_stabilized_output(output_torque: float, prev_output_torque: float, + desired_lateral_accel: float, desired_lateral_jerk: float, + v_ego: float, dt: float) -> float: + speed_weight = _sigmoid((max(v_ego, 0.0) - GENESIS_GV70_OUTPUT_SMOOTHING_SPEED) / + GENESIS_GV70_OUTPUT_SMOOTHING_SPEED_WIDTH) + center_weight = _sigmoid((GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT - abs(desired_lateral_accel)) / + GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_LAT_WIDTH) + curve_weight = 1.0 - center_weight + response_time = (GENESIS_GV70_OUTPUT_SMOOTHING_CURVE_RC * curve_weight + + GENESIS_GV70_OUTPUT_SMOOTHING_CENTER_RC * center_weight) + + unwind_phase = -desired_lateral_accel * desired_lateral_jerk + unwind_weight = _sigmoid((unwind_phase - GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE) / + GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH) + response_time += GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_RC * curve_weight * unwind_weight + + changing_direction = (abs(desired_lateral_accel) >= GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT and + prev_output_torque * desired_lateral_accel <= 0.0) + if changing_direction: + response_time = min(response_time, GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC) + + output_alpha = dt / (max(response_time, 0.0) + dt) + smoothed_output = prev_output_torque + output_alpha * (output_torque - prev_output_torque) + return float(output_torque + speed_weight * (smoothed_output - output_torque)) + + +GENESIS_GV70_CURVE_DAMPING_LAT_BP = [0.25, 0.55] + +GENESIS_GV70_CURVE_DAMPING_OVERSHOOT_BP = [0.0, 0.20] + +def _genesis_gv70_curve_damping_weight(setpoint: float, measured_lateral_accel: float) -> float: + curve_weight = float(np.interp(abs(setpoint), GENESIS_GV70_CURVE_DAMPING_LAT_BP, [0.0, 1.0])) + overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0) if setpoint * measured_lateral_accel > 0.0 else 0.0 + overshoot_weight = float(np.interp(overshoot, GENESIS_GV70_CURVE_DAMPING_OVERSHOOT_BP, [0.0, 1.0])) + return 1.0 - curve_weight * (1.0 - overshoot_weight) diff --git a/openpilot/starpilot/lateral/hkg_torque_policy.py b/openpilot/starpilot/lateral/hkg_torque_policy.py new file mode 100644 index 0000000000..298c286958 --- /dev/null +++ b/openpilot/starpilot/lateral/hkg_torque_policy.py @@ -0,0 +1,129 @@ +from collections import deque +import math + +import numpy as np + +from openpilot.starpilot.lateral.torque_extension import apply_turn_assist +from openpilot.cereal import log +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.pid import PIDController +from opendbc.car import structs +from openpilot.starpilot.lateral.torque_shaping import ( + FF_ROLL_OFFSET_FADE_BP, + FF_ROLL_OFFSET_FADE_V, + JERK_GAIN, + KI, + INTERP_SPEEDS, + KP_INTERP, + LAT_ACCEL_REQUEST_BUFFER_SECONDS, + LOW_SPEED_X, + LOW_SPEED_Y, + LP_FILTER_CUTOFF_HZ, + MAX_LAT_JERK_UP, + MIN_SPEED, + UNWIND_D_DES_THRESHOLD, + UNWIND_LAT_ACCEL_NEAR_ZERO, + center_chatter_friction_jerk_deadzone, +) + +def supported_torque_cp(cp, vehicles): + if (cp.brand != 'hyundai' or cp.carFingerprint not in vehicles or cp.notCar or cp.passive or cp.dashcamOnly or + cp.steerControlType != structs.CarParams.SteerControlType.torque or cp.lateralTuning.which() != 'torque'): + return False + tune = cp.lateralTuning.torque + return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and + math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0) + + +class HKGShapedTorquePolicy: + FACTOR_MULT = 1.0 + + def __init__(self, parent, cp): + if not self.supported_cp(cp): + raise ValueError("Unsupported HKG torque profile") + self.parent = parent + self.dt = parent.dt + parent.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1 / self.dt) + parent.update_limits() + parent.torque_params.latAccelFactor *= self.FACTOR_MULT + self.buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) + self.curvature_buffer = deque([0.0] * self.buffer_len, maxlen=self.buffer_len) + self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) + self.measurement_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) + self.low_speed_reset_threshold = max(cp.minSteerSpeed, 0.3) + self.previous_measurement = 0.0 + self.previous_setpoint = 0.0 + self.previous_pressed = False + + def feedforward_context(self, value, setpoint, jerk, measurement, speed): + return self.feedforward(value, setpoint, jerk, speed) + + def friction_context(self, setpoint, jerk, measurement, speed): + return self.friction(setpoint, jerk, speed) + + def jerk_deadzone(self, setpoint, jerk, measurement, speed): + return 0.0 + + def pid_error_context(self, error): + return error + + def output_context(self, value, setpoint, jerk, measurement, speed): + return self.output(value, setpoint, speed) + + def update(self, active, cs, vm, params, safety_limited, curvature, curvature_limited, delay): + parent = self.parent + pid_log = log.ControlsState.LateralTorqueState.new_message() + pid_log.version = 2 + measurement = -vm.calc_curvature(math.radians(cs.steeringAngleDeg - params.angleOffsetDeg), cs.vEgo, params.roll) * cs.vEgo**2 + future = curvature * cs.vEgo**2 + if not active: + parent.pid.reset() + self.curvature_buffer.append(curvature) + self.previous_measurement = measurement + self.measurement_filter.x = self.jerk_filter.x = 0.0 + self.previous_setpoint = future + self.previous_pressed = cs.steeringPressed + pid_log.active = False + return 0.0, 0.0, pid_log + if self.previous_pressed and not cs.steeringPressed: + parent.pid.i *= 0.8 + fade = np.interp(cs.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V) + roll = params.roll * ACCELERATION_DUE_TO_GRAVITY * fade + deadzone = abs(vm.calc_curvature(math.radians(parent.steering_angle_deadzone_deg), cs.vEgo, 0.0)) * cs.vEgo**2 + delay_frames = int(np.clip(delay / self.dt, 1, self.buffer_len)) + expected = self.curvature_buffer[-delay_frames] * cs.vEgo**2 + self.curvature_buffer.append(curvature) + raw_jerk = np.clip((future - expected) / max(delay, self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) + jerk = np.clip(self.jerk_filter.update(raw_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) + setpoint = expected + jerk * delay + unwind = (setpoint - self.previous_setpoint) / self.dt < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO + self.previous_setpoint = setpoint + rate = np.clip(self.measurement_filter.update((measurement - self.previous_measurement) / self.dt), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) + self.previous_measurement = measurement + lsf = (np.interp(cs.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(cs.vEgo, MIN_SPEED)) ** 2 + kp = np.interp(cs.vEgo, parent.pid._k_p[0], parent.pid._k_p[1]) + error = (setpoint - measurement) * (1 + lsf / max(kp, 1e-3)) + gravity_adjusted = future - roll + ff = gravity_adjusted - parent.torque_params.latAccelOffset * fade + ff = self.feedforward_context(ff, setpoint, jerk, measurement, cs.vEgo) + threshold, friction_scale = self.friction_context(setpoint, jerk, measurement, cs.vEgo) + jerk_deadzone = center_chatter_friction_jerk_deadzone(cs.vEgo, setpoint, self.jerk_deadzone(setpoint, jerk, measurement, cs.vEgo)) + friction_jerk = math.copysign(max(abs(jerk) - jerk_deadzone, 0.0), jerk) + ff += friction_scale * parent.friction(error + JERK_GAIN * friction_jerk, deadzone, threshold, cs, setpoint) + if cs.vEgo < self.low_speed_reset_threshold: + parent.pid.reset() + freeze = safety_limited or cs.steeringPressed or cs.vEgo < self.low_speed_reset_threshold or unwind + pid_log.error = float(self.pid_error_context(error)) + output = parent.torque_from_lateral_accel( + parent.pid.update(pid_log.error, error_rate=-rate, speed=cs.vEgo, feedforward=ff, freeze_integrator=freeze), parent.torque_params + ) + output = apply_turn_assist(parent, cs, vm, params, curvature, output) + output = self.output_context(output, setpoint, jerk, measurement, cs.vEgo) + self.previous_pressed = cs.steeringPressed + pid_log.active = True + pid_log.p, pid_log.i, pid_log.d, pid_log.f = map(float, (parent.pid.p, parent.pid.i, parent.pid.d, parent.pid.f)) + pid_log.output = float(-output) + pid_log.actualLateralAccel, pid_log.desiredLateralAccel, pid_log.desiredLateralJerk = map(float, (measurement, setpoint, jerk)) + pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output) < 1e-3, cs, safety_limited, curvature_limited)) + return -float(output), 0.0, pid_log diff --git a/openpilot/starpilot/lateral/hkg_vehicle_policy.py b/openpilot/starpilot/lateral/hkg_vehicle_policy.py new file mode 100644 index 0000000000..4a0a76e3c5 --- /dev/null +++ b/openpilot/starpilot/lateral/hkg_vehicle_policy.py @@ -0,0 +1,178 @@ +import numpy as np + +from opendbc.car.hyundai.values import CAR, HyundaiFlags + +from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping + +from openpilot.starpilot.lateral.hkg_torque_policy import HKGShapedTorquePolicy, supported_torque_cp + +PROFILES = { + 'ioniq_5': (frozenset((CAR.HYUNDAI_IONIQ_5,)), shaping.IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT), + 'ioniq_ev_old': ( + frozenset( + ( + CAR.HYUNDAI_IONIQ_EV_LTD, + CAR.HYUNDAI_IONIQ_EV_2020, + ) + ), + shaping.IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT, + ), + 'sonata': (frozenset((CAR.HYUNDAI_SONATA,)), 1), + 'elantra_non_scc': ( + frozenset( + ( + CAR.HYUNDAI_ELANTRA_2022_NON_SCC, + CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC, + ) + ), + 1, + ), + 'kia_xceed': (frozenset((CAR.KIA_XCEED_PHEV,)), 1), + 'kia_niro_ev': (frozenset((CAR.KIA_NIRO_EV,)), 1), + 'kia_niro_phev_2022': (frozenset((CAR.KIA_NIRO_PHEV_2022,)), 1), + 'kia_stinger_2022': (frozenset((CAR.KIA_STINGER_2022,)), 1), + 'kia_forte': ( + frozenset( + ( + CAR.KIA_FORTE, + CAR.KIA_FORTE_2019_NON_SCC, + CAR.KIA_FORTE_2021_NON_SCC, + ) + ), + shaping.KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT, + ), + 'kia_ev6': (frozenset((CAR.KIA_EV6,)), 1), + 'kia_carnival': ( + frozenset( + ( + CAR.KIA_CARNIVAL_2025, + CAR.KIA_CARNIVAL_HEV_4TH_GEN, + ) + ), + 1, + ), + 'tucson_4th_gen': (frozenset((CAR.HYUNDAI_TUCSON_4TH_GEN,)), 1), + 'kona_non_scc': (frozenset((CAR.HYUNDAI_KONA_NON_SCC,)), 1), + 'kona_ev_2022': (frozenset((CAR.HYUNDAI_KONA_EV_2022,)), 1), +} + + +def policy_for(cp): + return next((key for key, (cars, _) in PROFILES.items() if supported_torque_cp(cp, cars)), None) + + +class HKGVehicleTorquePolicy(HKGShapedTorquePolicy): + def __init__(self, parent, cp): + self.profile = policy_for(cp) + if self.profile is None: + raise ValueError("Unsupported HKG vehicle torque profile") + self.FACTOR_MULT = PROFILES[self.profile][1] + self.canfd = bool(cp.flags & HyundaiFlags.CANFD) + self.carnival_hev = cp.carFingerprint == CAR.KIA_CARNIVAL_HEV_4TH_GEN + self.requested_curvature = 0.0 + self.future_lateral_accel = 0.0 + self.steering_pressed = False + self.hev_center_weight = 0.0 + super().__init__(parent, cp) + + supported_cp = staticmethod(lambda cp: policy_for(cp) is not None) + + def update(self, active, cs, vm, params, safety_limited, curvature, curvature_limited, delay): + self.requested_curvature = curvature + self.future_lateral_accel = curvature * cs.vEgo ** 2 + self.steering_pressed = bool(cs.steeringPressed) + return super().update(active, cs, vm, params, safety_limited, curvature, curvature_limited, delay) + + def feedforward_context(self, value, setpoint, jerk, measurement, speed): + if self.profile == 'ioniq_5': + return value * shaping.get_ioniq_5_ff_scale(setpoint, jerk, speed) * shaping.get_ioniq_5_center_taper_scale(setpoint, speed) + if self.profile == 'ioniq_ev_old': + return value * shaping.get_ioniq_ev_old_ff_scale(setpoint, jerk, speed) * shaping.get_ioniq_ev_old_center_taper_scale(setpoint, speed) + if self.profile == 'sonata': + return value * shaping.get_sonata_ff_scale(setpoint, jerk, speed) * shaping.get_sonata_center_taper_scale(setpoint, speed) + if self.profile == 'elantra_non_scc': + return value * shaping.get_elantra_non_scc_ff_scale(setpoint, jerk, speed) + if self.profile == 'kia_xceed': + return value * shaping.get_kia_xceed_ff_scale(setpoint, jerk, speed) * shaping.get_kia_xceed_center_taper_scale(setpoint, speed) + if self.profile == 'kia_forte': + return value * shaping.get_kia_forte_ff_scale(setpoint, jerk, speed) * shaping.get_kia_forte_center_taper_scale(setpoint, speed) + if self.profile == 'kia_ev6': + return value * shaping.get_kia_ev6_ff_scale(setpoint, jerk, speed) * shaping.get_kia_ev6_center_taper_scale(setpoint, speed) + if self.profile == 'kia_carnival': + return value * shaping.get_kia_carnival_unwind_ff_scale(setpoint, measurement, jerk, speed) + return value + + def friction_context(self, setpoint, jerk, measurement, speed): + base = shaping.get_hkg_canfd_base_friction_threshold(speed) if self.canfd else shaping.get_standard_friction_threshold(speed) + if self.profile == 'ioniq_5': + return shaping.get_ioniq_5_friction_threshold(speed, setpoint, jerk), 1 + ( + shaping.get_ioniq_5_friction_scale(speed, setpoint, jerk) - 1 + ) * shaping.get_ioniq_5_center_taper_scale(setpoint, speed) + if self.profile == 'kia_niro_ev' and not self.steering_pressed: + return shaping.get_kia_niro_ev_friction_threshold(speed, setpoint, jerk, self.future_lateral_accel), 1 + if self.profile == 'kia_niro_phev_2022': + return shaping.get_kia_niro_phev_2022_friction_threshold(speed, setpoint, jerk), 1 + if self.profile == 'kia_stinger_2022': + return shaping.get_kia_stinger_2022_friction_threshold( + speed, setpoint, jerk, self.future_lateral_accel, self.steering_pressed or self.previous_pressed), 1 + if self.profile == 'kia_forte': + return shaping.get_kia_forte_friction_threshold(speed, setpoint, jerk), 1 + if self.profile == 'kia_ev6': + return shaping.get_kia_ev6_friction_threshold(speed, setpoint, jerk), 1 + ( + shaping.get_kia_ev6_friction_scale(speed, setpoint, jerk) - 1 + ) * shaping.get_kia_ev6_center_taper_scale(setpoint, speed) + if self.profile == 'kia_carnival': + self.hev_center_weight = (shaping.get_kia_carnival_hev_center_weight( + speed, setpoint, measurement, jerk, self.future_lateral_accel) + if self.carnival_hev and not self.steering_pressed and not self.previous_pressed else 0.0) + threshold = shaping.get_kia_carnival_friction_threshold(speed, setpoint, jerk) + return (threshold + shaping.KIA_CARNIVAL_HEV_CENTER_FRICTION_INCREMENT * self.hev_center_weight, + shaping.get_kia_carnival_friction_center_fade_scale(setpoint, speed)) + if self.profile == 'tucson_4th_gen': + return shaping.get_tucson_4th_gen_friction_threshold(speed, setpoint, jerk), 1 + if self.profile == 'kona_non_scc': + return shaping.get_kona_non_scc_friction_threshold(speed, setpoint, jerk), 1 + if self.profile == 'kona_ev_2022': + return shaping.get_kona_ev_2022_friction_threshold(speed, setpoint), 1 + return (base, 1.0) + + def pid_error_context(self, error): + return error * (1.0 - shaping.KIA_CARNIVAL_HEV_CENTER_ERROR_REDUCTION * self.hev_center_weight) + + def jerk_deadzone(self, setpoint, jerk, measurement, speed): + if self.profile == 'ioniq_5': + return shaping.get_ioniq_5_friction_jerk_deadzone(speed, setpoint) + if self.profile == 'kia_carnival': + return shaping.get_kia_carnival_friction_jerk_deadzone(speed, setpoint, jerk) + return 0.0 + + def output_context(self, value, setpoint, jerk, measurement, speed): + if self.profile == 'ioniq_5': + limit = shaping.get_ioniq_5_low_speed_output_limit( + setpoint, jerk, speed, self.requested_curvature if not self.steering_pressed and not self.previous_pressed else 0.0, value) + return float(np.clip(value, -limit, limit)) + if self.profile == 'kia_niro_phev_2022': + return value * shaping.get_kia_niro_phev_2022_center_taper_scale(setpoint, speed) + if self.profile == 'kia_stinger_2022': + return value * shaping.get_kia_stinger_2022_center_taper_scale(setpoint, speed) + if self.profile == 'kia_ev6': + return value * shaping.get_kia_ev6_low_speed_center_taper_scale(setpoint, speed) * shaping.get_kia_ev6_center_output_scale(setpoint, speed) + if self.profile == 'kia_carnival': + return ( + value + * shaping.get_kia_carnival_center_taper_scale(setpoint, speed) + * shaping.get_kia_carnival_unwind_output_scale( + setpoint, measurement, jerk, speed, + output_torque=value if self.carnival_hev and not self.steering_pressed and not self.previous_pressed else None) + * shaping.get_kia_carnival_highway_transition_output_scale(setpoint, jerk, speed) + ) + if self.profile == 'tucson_4th_gen': + return value * shaping.get_tucson_4th_gen_center_taper_scale(setpoint, speed) + if self.profile == 'kona_non_scc': + value *= shaping.get_kona_non_scc_center_taper_scale(setpoint, speed) + if value * setpoint > 0 or setpoint * jerk < 0: + value *= shaping.get_kona_non_scc_highway_transition_output_scale(setpoint, jerk, speed) + return value + if self.profile == 'kona_ev_2022': + return value * shaping.get_kona_ev_2022_center_output_scale(setpoint, speed) + return value diff --git a/openpilot/starpilot/lateral/hkg_vehicle_shaping.py b/openpilot/starpilot/lateral/hkg_vehicle_shaping.py new file mode 100644 index 0000000000..97567cb2ba --- /dev/null +++ b/openpilot/starpilot/lateral/hkg_vehicle_shaping.py @@ -0,0 +1,1354 @@ +import math + +import numpy as np + +from openpilot.common.constants import CV + +STANDARD_FRICTION_THRESHOLD = 0.30 + +HKG_CANFD_BASE_FRICTION_THRESHOLD = 0.39 + +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_STINGER_2022_CENTER_TAPER_MAX = 0.12 + +KIA_STINGER_2022_CENTER_TAPER_LAT = 0.30 + +KIA_STINGER_2022_CENTER_TAPER_LAT_WIDTH = 0.05 + +KIA_STINGER_2022_CENTER_TAPER_SPEED = 10.0 + +KIA_STINGER_2022_CENTER_TAPER_SPEED_WIDTH = 2.5 + +KIA_STINGER_2022_FRICTION_THRESHOLD_GAIN = 0.10 + +KIA_CARNIVAL_CENTER_TAPER_MAX = 0.20 + +KIA_CARNIVAL_CENTER_TAPER_LAT = 0.20 + +KIA_CARNIVAL_CENTER_TAPER_LAT_WIDTH = 0.055 + +KIA_CARNIVAL_CENTER_TAPER_SPEED = 3.5 + +KIA_CARNIVAL_CENTER_TAPER_SPEED_WIDTH = 1.8 + +KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX = 14.5 + +KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX_WIDTH = 2.0 + +KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN = 0.24 + +KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX = 0.34 + +KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_MAX = 0.14 + +KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT = 0.24 + +KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.06 + +KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED = 28.0 + +KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 2.0 + +KIA_CARNIVAL_HIGHWAY_FRICTION_THRESHOLD_GAIN = 0.14 + +KIA_CARNIVAL_HIGHWAY_FRICTION_CENTER_FADE_MAX = 0.20 + +KIA_CARNIVAL_HIGHWAY_TRANSITION_TAPER_MAX = 0.28 + +KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK = 0.45 + +KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK_WIDTH = 0.15 + +KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_CUTOFF = 1.20 + +KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_WIDTH = 0.20 + +KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_MAX = 0.34 + +KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED = 15.0 + +KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 2.0 + +KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF = 23.0 + +KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF_WIDTH = 2.0 + +KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT = 0.35 + +KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.18 + +KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK = 0.65 + +KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH = 0.25 + +KIA_CARNIVAL_UNWIND_FF_REDUCTION_MAX = 0.45 + +KIA_CARNIVAL_UNWIND_FF_SPEED = 9.0 + +KIA_CARNIVAL_UNWIND_FF_SPEED_WIDTH = 2.0 + +KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF = 23.0 + +KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF_WIDTH = 2.0 + +KIA_CARNIVAL_UNWIND_FF_OVERSHOOT = 0.08 + +KIA_CARNIVAL_UNWIND_FF_OVERSHOOT_WIDTH = 0.06 + +KIA_CARNIVAL_UNWIND_FF_JERK = 0.45 + +KIA_CARNIVAL_UNWIND_FF_JERK_WIDTH = 0.20 + +KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_MAX = 0.35 + +KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED = 8.0 + +KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_WIDTH = 2.0 + +KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_CUTOFF = 16.0 + +KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_CUTOFF_WIDTH = 2.5 + +KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_OVERSHOOT = 0.25 + +KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_OVERSHOOT_WIDTH = 0.15 + +KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_JERK = 0.45 + +KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_JERK_WIDTH = 0.20 + +TUCSON_4TH_GEN_CENTER_TAPER_MAX = 0.44 + +TUCSON_4TH_GEN_CENTER_TAPER_LAT = 0.28 + +TUCSON_4TH_GEN_CENTER_TAPER_LAT_WIDTH = 0.055 + +TUCSON_4TH_GEN_CENTER_TAPER_SPEED_MAX = 14.0 + +TUCSON_4TH_GEN_CENTER_TAPER_SPEED_WIDTH = 1.5 + +TUCSON_4TH_GEN_FRICTION_THRESHOLD_GAIN = 0.28 + +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 + +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_5_SUSTAINED_TURN_IN_FF_BOOST_LEFT = 0.10 + +IONIQ_5_SUSTAINED_TURN_IN_FF_BOOST_RIGHT = 0.16 + +IONIQ_5_SUSTAINED_TURN_IN_FF_SPEED = 13.5 + +IONIQ_5_SUSTAINED_TURN_IN_FF_SPEED_WIDTH = 1.8 + +IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_START = 1.10 + +IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_END = 3.60 + +IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_WIDTH = 0.30 + +IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE = 0.05 + +IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF = 0.95 + +IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED = 8.0 + +IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 0.8 + +IONIQ_5_LOW_SPEED_CENTER_LAT = 0.40 + +IONIQ_5_LOW_SPEED_CENTER_LAT_WIDTH = 0.10 + +IONIQ_5_LOW_SPEED_CENTER_JERK = 0.40 + +IONIQ_5_LOW_SPEED_CENTER_JERK_WIDTH = 0.12 + +IONIQ_5_FRICTION_JERK_DEADZONE_MAX = 0.36 + +IONIQ_5_FRICTION_JERK_DEADZONE_LAT = 1.25 + +IONIQ_5_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.35 + +IONIQ_5_FRICTION_JERK_DEADZONE_SPEED = 18.0 + +IONIQ_5_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.0 + +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 + +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.54 + +KIA_EV6_TURN_IN_BOOST_RIGHT = 0.60 + +KIA_EV6_UNWIND_TAPER_LEFT = 0.56 + +KIA_EV6_UNWIND_TAPER_RIGHT = 0.54 + +KIA_EV6_BASE_UNWIND_TAPER_LEFT = 0.10 + +KIA_EV6_BASE_UNWIND_TAPER_RIGHT = 0.13 + +KIA_EV6_JWARM_BASE_TURN_IN_BOOST_LEFT = 0.12 + +KIA_EV6_JWARM_BASE_TURN_IN_BOOST_RIGHT = 0.14 + +KIA_EV6_JWARM_BASE_UNWIND_TAPER_LEFT = 0.15 + +KIA_EV6_JWARM_BASE_UNWIND_TAPER_RIGHT = 0.16 + +KIA_EV6_JWARM_PHASE_STABILITY_MAX_REDUCTION = 0.25 + +KIA_EV6_JWARM_PHASE_STABILITY_SPEED = 10.0 + +KIA_EV6_JWARM_PHASE_STABILITY_SPEED_WIDTH = 1.8 + +KIA_EV6_JWARM_PHASE_STABILITY_JERK = 0.70 + +KIA_EV6_JWARM_PHASE_STABILITY_JERK_WIDTH = 0.18 + +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.34 + +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.18 + +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_CENTER_FRICTION_THRESHOLD_GAIN = 0.14 + +KIA_EV6_CENTER_FRICTION_THRESHOLD_LAT = 0.30 + +KIA_EV6_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.07 + +KIA_EV6_CENTER_FRICTION_THRESHOLD_SPEED = 18.0 + +KIA_EV6_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.5 + +KIA_EV6_LOW_SPEED_CENTER_TAPER_MAX = 0.19 + +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 + +KIA_EV6_CENTER_OUTPUT_TAPER_MAX = 0.14 + +KIA_EV6_CENTER_OUTPUT_TAPER_LAT = 0.30 + +KIA_EV6_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08 + +KIA_EV6_CENTER_OUTPUT_TAPER_SPEED = 12.0 + +KIA_EV6_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.4 + +KONA_NON_SCC_TRANSITION_TURN_IN_TAPER_MAX = 0.24 + +KONA_NON_SCC_TRANSITION_UNWIND_TAPER_MAX = 0.42 + +KONA_NON_SCC_TRANSITION_SPEED_ONSET = 22.0 + +KONA_NON_SCC_TRANSITION_SPEED_FULL = 29.0 + +KONA_NON_SCC_TRANSITION_JERK_ONSET = 0.35 + +KONA_NON_SCC_TRANSITION_JERK_FULL = 1.20 + +KONA_NON_SCC_TRANSITION_LAT_FADE_START = 0.45 + +KONA_NON_SCC_TRANSITION_LAT_FADE_END = 1.60 + +KONA_NON_SCC_CENTER_TAPER_MAX = 0.14 + +KONA_NON_SCC_CENTER_TAPER_LAT = 0.28 + +KONA_NON_SCC_CENTER_TAPER_SPEED_ONSET = 12.0 + +KONA_NON_SCC_CENTER_TAPER_SPEED_FULL = 24.0 + +KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_GAIN = 0.14 + +KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_LAT = 0.28 + +KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.07 + +KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_SPEED_ONSET = 11.0 + +KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.5 + +KONA_EV_2022_CENTER_FRICTION_THRESHOLD_GAIN = 0.08 + +KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT = 0.20 + +KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.05 + +KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED = 18.0 + +KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.5 + +KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_GAIN = 0.30 +KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_SPEED_ONSET = 100.0 / 3.6 +KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_SPEED_FULL = 110.0 / 3.6 +KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_LAT_FADE_START = 1.20 +KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_LAT_FADE_END = 2.00 + +KONA_EV_2022_CENTER_OUTPUT_TAPER_MAX = 0.08 + +KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT = 0.20 + +KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.05 + +KONA_EV_2022_CENTER_OUTPUT_TAPER_SPEED = 18.0 + +KONA_EV_2022_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.5 + +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 _gm_base_friction_threshold_default(v_ego: float) -> float: + 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 _standard_friction_threshold_default(v_ego: float) -> float: + return max(_gm_base_friction_threshold_default(v_ego), STANDARD_FRICTION_THRESHOLD) + +def _hkg_canfd_base_friction_threshold_default(v_ego: float) -> float: + return max(_gm_base_friction_threshold_default(v_ego), HKG_CANFD_BASE_FRICTION_THRESHOLD) + +def _flm_vehicle_knob(name, default_value): + return float(default_value) + +def get_gm_base_friction_threshold(v_ego): + return _gm_base_friction_threshold_default(v_ego) + +def get_standard_friction_threshold(v_ego): + return _standard_friction_threshold_default(v_ego) + +def get_hkg_canfd_base_friction_threshold(v_ego): + return _hkg_canfd_base_friction_threshold_default(v_ego) + +def get_kona_non_scc_highway_transition_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float, + v_ego: float) -> float: + speed_weight = float(np.interp(v_ego, [KONA_NON_SCC_TRANSITION_SPEED_ONSET, KONA_NON_SCC_TRANSITION_SPEED_FULL], [0.0, 1.0])) + jerk_weight = float(np.interp(abs(desired_lateral_jerk), + [KONA_NON_SCC_TRANSITION_JERK_ONSET, KONA_NON_SCC_TRANSITION_JERK_FULL], [0.0, 1.0])) + lat_weight = 1.0 - float(np.interp(abs(desired_lateral_accel), + [KONA_NON_SCC_TRANSITION_LAT_FADE_START, KONA_NON_SCC_TRANSITION_LAT_FADE_END], [0.0, 1.0])) + taper_max = (KONA_NON_SCC_TRANSITION_UNWIND_TAPER_MAX + if desired_lateral_accel * desired_lateral_jerk < 0.0 + else KONA_NON_SCC_TRANSITION_TURN_IN_TAPER_MAX) + return 1.0 - (taper_max * speed_weight * jerk_weight * lat_weight) + +def get_kona_non_scc_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, + desired_lateral_jerk: float = 0.0) -> float: + del desired_lateral_jerk + speed_weight = _sigmoid((v_ego - KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_SPEED_ONSET) / + KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH) + center_weight = _sigmoid((KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) / + KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_LAT_WIDTH) + return get_standard_friction_threshold(v_ego) * ( + 1.0 + KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight + ) + +def get_kona_non_scc_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = float(np.interp(v_ego, [KONA_NON_SCC_CENTER_TAPER_SPEED_ONSET, KONA_NON_SCC_CENTER_TAPER_SPEED_FULL], [0.0, 1.0])) + center_weight = float(np.interp(abs(desired_lateral_accel), [0.0, KONA_NON_SCC_CENTER_TAPER_LAT], [1.0, 0.0])) + return 1.0 - (KONA_NON_SCC_CENTER_TAPER_MAX * speed_weight * center_weight) + +def _kona_ev_2022_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]: + speed_weight = _sigmoid((v_ego - KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED) / + KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH) + center_weight = _sigmoid((KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) / + KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT_WIDTH) + return speed_weight, center_weight + +def get_kona_ev_2022_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0) -> float: + speed_weight, center_weight = _kona_ev_2022_center_weights(desired_lateral_accel, v_ego) + base_threshold = get_standard_friction_threshold(v_ego) * ( + 1.0 + KONA_EV_2022_CENTER_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight + ) + high_speed_weight = float(np.interp( + v_ego, + [KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_SPEED_ONSET, KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_SPEED_FULL], + [0.0, 1.0], + )) + curve_weight = float(np.interp( + abs(desired_lateral_accel), + [KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_LAT_FADE_START, KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_LAT_FADE_END], + [1.0, 0.0], + )) + return base_threshold + KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_GAIN * high_speed_weight * curve_weight + +def get_kona_ev_2022_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sigmoid((v_ego - KONA_EV_2022_CENTER_OUTPUT_TAPER_SPEED) / + KONA_EV_2022_CENTER_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT_WIDTH) + return 1.0 - (KONA_EV_2022_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight) + +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_gm_base_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_stinger_2022_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]: + speed_weight = _sigmoid((v_ego - KIA_STINGER_2022_CENTER_TAPER_SPEED) / KIA_STINGER_2022_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((KIA_STINGER_2022_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + KIA_STINGER_2022_CENTER_TAPER_LAT_WIDTH) + return speed_weight, center_weight + +def get_kia_stinger_2022_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight, center_weight = _kia_stinger_2022_center_weights(desired_lateral_accel, v_ego) + return 1.0 - (KIA_STINGER_2022_CENTER_TAPER_MAX * speed_weight * center_weight) + +def get_kia_stinger_2022_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, + desired_lateral_jerk: float = 0.0, + future_lateral_accel: float = 0.0, steering_pressed: bool = False) -> float: + speed_weight, center_weight = _kia_stinger_2022_center_weights(desired_lateral_accel, v_ego) + threshold = get_standard_friction_threshold(v_ego) * (1.0 + KIA_STINGER_2022_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight) + if not steering_pressed: + highway_weight = np.interp(v_ego, KIA_STINGER_2022_HIGHWAY_FRICTION_SPEED_BP, [0.0, 1.0]) + requested_accel = max(abs(desired_lateral_accel), abs(future_lateral_accel)) + highway_weight *= np.interp(requested_accel, KIA_STINGER_2022_HIGHWAY_FRICTION_LAT_BP, [1.0, 0.0]) + highway_weight *= np.interp(abs(desired_lateral_jerk), KIA_STINGER_2022_HIGHWAY_FRICTION_JERK_BP, [1.0, 0.0]) + threshold += KIA_STINGER_2022_HIGHWAY_FRICTION_INCREMENT * highway_weight + return float(threshold) + +def _kia_carnival_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]: + speed_onset = _sigmoid((v_ego - KIA_CARNIVAL_CENTER_TAPER_SPEED) / KIA_CARNIVAL_CENTER_TAPER_SPEED_WIDTH) + speed_cutoff = _sigmoid((KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX - v_ego) / KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX_WIDTH) + speed_weight = speed_onset * speed_cutoff + center_weight = _sigmoid((KIA_CARNIVAL_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_CARNIVAL_CENTER_TAPER_LAT_WIDTH) + return speed_weight, center_weight + +def _kia_carnival_highway_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]: + speed_weight = _sigmoid((v_ego - KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED) / + KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_LAT_WIDTH) + return speed_weight, center_weight + +def get_kia_carnival_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego) + highway_speed_weight, highway_center_weight = _kia_carnival_highway_center_weights(desired_lateral_accel, v_ego) + reduction = KIA_CARNIVAL_CENTER_TAPER_MAX * speed_weight * center_weight + reduction += KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_MAX * highway_speed_weight * highway_center_weight + return 1.0 - min(reduction, 0.95) + +def get_kia_carnival_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + del desired_lateral_jerk + speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego) + highway_speed_weight, highway_center_weight = _kia_carnival_highway_center_weights(desired_lateral_accel, v_ego) + gain = KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight + gain += KIA_CARNIVAL_HIGHWAY_FRICTION_THRESHOLD_GAIN * highway_speed_weight * highway_center_weight + return get_hkg_canfd_base_friction_threshold(v_ego) * (1.0 + gain) + +def get_kia_carnival_friction_center_fade_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego) + highway_speed_weight, highway_center_weight = _kia_carnival_highway_center_weights(desired_lateral_accel, v_ego) + reduction = KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight + reduction += KIA_CARNIVAL_HIGHWAY_FRICTION_CENTER_FADE_MAX * highway_speed_weight * highway_center_weight + return 1.0 - min(reduction, 0.95) + +def get_kia_carnival_highway_transition_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float, + v_ego: float) -> float: + speed_weight = _sigmoid((v_ego - KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED) / + KIA_CARNIVAL_HIGHWAY_CENTER_TAPER_SPEED_WIDTH) + jerk_weight = _sigmoid((abs(desired_lateral_jerk) - KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK) / + KIA_CARNIVAL_HIGHWAY_TRANSITION_JERK_WIDTH) + lat_weight = _sigmoid((KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_CUTOFF - abs(desired_lateral_accel)) / + KIA_CARNIVAL_HIGHWAY_TRANSITION_LAT_WIDTH) + return 1.0 - (KIA_CARNIVAL_HIGHWAY_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight) + +def get_kia_carnival_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float, + desired_lateral_jerk: float) -> float: + speed_weight = _sigmoid((v_ego - KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED) / + KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_WIDTH) + speed_cutoff = _sigmoid((KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF - v_ego) / + KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_SPEED_CUTOFF_WIDTH) + center_weight = _sigmoid((KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) / + KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_LAT_WIDTH) + jerk_weight = _sigmoid((abs(desired_lateral_jerk) - KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK) / + KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_JERK_WIDTH) + return KIA_CARNIVAL_UNWIND_FRICTION_JERK_DEADZONE_MAX * speed_weight * speed_cutoff * center_weight * jerk_weight + +def get_kia_carnival_unwind_ff_scale(setpoint: float, measured_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + if setpoint * desired_lateral_jerk >= 0.0: + return 1.0 + + overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0) + if overshoot <= 0.0: + return 1.0 + + speed_weight = (_sigmoid((v_ego - KIA_CARNIVAL_UNWIND_FF_SPEED) / + KIA_CARNIVAL_UNWIND_FF_SPEED_WIDTH) * + _sigmoid((KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF - v_ego) / + KIA_CARNIVAL_UNWIND_FF_SPEED_CUTOFF_WIDTH)) + overshoot_weight = _sigmoid((overshoot - KIA_CARNIVAL_UNWIND_FF_OVERSHOOT) / + KIA_CARNIVAL_UNWIND_FF_OVERSHOOT_WIDTH) + jerk_weight = _sigmoid((abs(desired_lateral_jerk) - KIA_CARNIVAL_UNWIND_FF_JERK) / + KIA_CARNIVAL_UNWIND_FF_JERK_WIDTH) + return 1.0 - (KIA_CARNIVAL_UNWIND_FF_REDUCTION_MAX * speed_weight * overshoot_weight * jerk_weight) + +def get_kia_carnival_unwind_output_scale(setpoint: float, measured_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float, + *, output_torque: float | None = None) -> float: + if output_torque is not None and output_torque * setpoint <= 0.0: + return 1.0 + if (setpoint * desired_lateral_jerk >= 0.0 or + setpoint * measured_lateral_accel <= 0.0): + return 1.0 + + overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0) + if overshoot <= 0.0: + return 1.0 + + speed_weight = (_sigmoid((v_ego - KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED) / + KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_WIDTH) * + _sigmoid((KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_CUTOFF - v_ego) / + KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_SPEED_CUTOFF_WIDTH)) + overshoot_weight = _sigmoid((overshoot - KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_OVERSHOOT) / + KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_OVERSHOOT_WIDTH) + jerk_weight = _sigmoid((abs(desired_lateral_jerk) - KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_JERK) / + KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_JERK_WIDTH) + return 1.0 - (KIA_CARNIVAL_UNWIND_OUTPUT_DAMPING_MAX * speed_weight * overshoot_weight * jerk_weight) + +def _tucson_4th_gen_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]: + speed_weight = _sigmoid((TUCSON_4TH_GEN_CENTER_TAPER_SPEED_MAX - v_ego) / TUCSON_4TH_GEN_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((TUCSON_4TH_GEN_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / TUCSON_4TH_GEN_CENTER_TAPER_LAT_WIDTH) + return speed_weight, center_weight + +def get_tucson_4th_gen_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight, center_weight = _tucson_4th_gen_center_weights(desired_lateral_accel, v_ego) + return 1.0 - (TUCSON_4TH_GEN_CENTER_TAPER_MAX * speed_weight * center_weight) + +def get_tucson_4th_gen_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, + desired_lateral_jerk: float = 0.0) -> float: + del desired_lateral_jerk + speed_weight, center_weight = _tucson_4th_gen_center_weights(desired_lateral_accel, v_ego) + return get_hkg_canfd_base_friction_threshold(v_ego) * (1.0 + TUCSON_4TH_GEN_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight) + +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_gm_base_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 _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) + abs_lateral_accel = abs(desired_lateral_accel) + + 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)) + sustained_turn_in_scale = 0.0 + if desired_lateral_accel * desired_lateral_jerk > 0.0: + sustained_speed_weight = _ioniq_5_sigmoid((max(v_ego, 0.0) - IONIQ_5_SUSTAINED_TURN_IN_FF_SPEED) / + IONIQ_5_SUSTAINED_TURN_IN_FF_SPEED_WIDTH) + sustained_lat_onset = _ioniq_5_sigmoid((abs_lateral_accel - IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_START) / + IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_WIDTH) + sustained_lat_cutoff = _ioniq_5_sigmoid((IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_END - abs_lateral_accel) / + IONIQ_5_SUSTAINED_TURN_IN_FF_LAT_WIDTH) + sustained_turn_in_scale = (_ioniq_5_side_value(desired_lateral_accel, + IONIQ_5_SUSTAINED_TURN_IN_FF_BOOST_LEFT, + IONIQ_5_SUSTAINED_TURN_IN_FF_BOOST_RIGHT) * + sustained_speed_weight * sustained_lat_onset * sustained_lat_cutoff) + return (1.0 + sustained_turn_in_scale) * (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_hkg_canfd_base_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 get_ioniq_5_low_speed_output_limit(desired_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float, + desired_curvature: float = 0.0, output_torque: float = 0.0) -> float: + """Bound stop-transition center chatter without blunting actual turns.""" + speed_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED - max(v_ego, 0.0)) / + IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH) + center_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_CENTER_LAT - abs(desired_lateral_accel)) / + IONIQ_5_LOW_SPEED_CENTER_LAT_WIDTH) + calm_weight = _ioniq_5_sigmoid((IONIQ_5_LOW_SPEED_CENTER_JERK - abs(desired_lateral_jerk)) / + IONIQ_5_LOW_SPEED_CENTER_JERK_WIDTH) + center_weight *= calm_weight + center_limit = (IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE + + IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF * (1.0 - center_weight)) + limit = 1.0 - speed_weight * (1.0 - center_limit) + if desired_curvature * output_torque > 0.0: + turn_weight = np.interp(abs(desired_curvature), IONIQ_5_LOW_SPEED_TURN_CURVATURE_BP, [0.0, 1.0]) + limit += max(IONIQ_5_LOW_SPEED_TURN_OUTPUT_FLOOR - limit, 0.0) * turn_weight + return float(np.clip(limit, IONIQ_5_LOW_SPEED_OUTPUT_LIMIT_BASE, 1.0)) + +def get_ioniq_5_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float) -> float: + speed_weight = _ioniq_5_sigmoid((max(v_ego, 0.0) - IONIQ_5_FRICTION_JERK_DEADZONE_SPEED) / + IONIQ_5_FRICTION_JERK_DEADZONE_SPEED_WIDTH) + curve_weight = _ioniq_5_sigmoid((IONIQ_5_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) / + IONIQ_5_FRICTION_JERK_DEADZONE_LAT_WIDTH) + return IONIQ_5_FRICTION_JERK_DEADZONE_MAX * speed_weight * curve_weight + +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 kia_ev6_lateral_testing_ground_active(): + return False + +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_jwarm_phase_confidence(v_ego: float, desired_lateral_jerk: float) -> float: + low_speed_weight = _kia_ev6_sigmoid( + (KIA_EV6_JWARM_PHASE_STABILITY_SPEED - v_ego) / KIA_EV6_JWARM_PHASE_STABILITY_SPEED_WIDTH + ) + abrupt_transition_weight = _kia_ev6_sigmoid( + (abs(desired_lateral_jerk) - KIA_EV6_JWARM_PHASE_STABILITY_JERK) / KIA_EV6_JWARM_PHASE_STABILITY_JERK_WIDTH + ) + return 1.0 - (KIA_EV6_JWARM_PHASE_STABILITY_MAX_REDUCTION * low_speed_weight * abrupt_transition_weight) + +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, + _flm_vehicle_knob("hyundai_kia_ev6.ff_gain_left", KIA_EV6_FF_GAIN_LEFT), + _flm_vehicle_knob("hyundai_kia_ev6.ff_gain_right", 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, + _flm_vehicle_knob("hyundai_kia_ev6.turn_in_boost_left", KIA_EV6_TURN_IN_BOOST_LEFT), + _flm_vehicle_knob("hyundai_kia_ev6.turn_in_boost_right", 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, + _flm_vehicle_knob("hyundai_kia_ev6.unwind_taper_left", KIA_EV6_UNWIND_TAPER_LEFT), + _flm_vehicle_knob("hyundai_kia_ev6.unwind_taper_right", KIA_EV6_UNWIND_TAPER_RIGHT), + ) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + jwarm_tune = kia_ev6_lateral_testing_ground_active() + jwarm_phase_confidence = get_kia_ev6_jwarm_phase_confidence(v_ego, desired_lateral_jerk) if jwarm_tune else 0.0 + base_turn_in_boost = 1.0 + ((_kia_ev6_side_value( + desired_lateral_accel, + KIA_EV6_JWARM_BASE_TURN_IN_BOOST_LEFT, + KIA_EV6_JWARM_BASE_TURN_IN_BOOST_RIGHT, + ) if jwarm_tune else 0.0) * + jwarm_phase_confidence * turn_in_weight * onset * cutoff) + base_unwind_taper_left = _flm_vehicle_knob("hyundai_kia_ev6.base_unwind_taper_left", KIA_EV6_BASE_UNWIND_TAPER_LEFT) + base_unwind_taper_right = _flm_vehicle_knob("hyundai_kia_ev6.base_unwind_taper_right", KIA_EV6_BASE_UNWIND_TAPER_RIGHT) + if jwarm_tune: + base_unwind_taper_left += (KIA_EV6_JWARM_BASE_UNWIND_TAPER_LEFT - base_unwind_taper_left) * jwarm_phase_confidence + base_unwind_taper_right += (KIA_EV6_JWARM_BASE_UNWIND_TAPER_RIGHT - base_unwind_taper_right) * jwarm_phase_confidence + base_unwind_taper = 1.0 - (_kia_ev6_side_value( + desired_lateral_accel, + base_unwind_taper_left, + base_unwind_taper_right, + ) * unwind_weight * onset * cutoff) + return (base_unwind_taper * base_turn_in_boost) + (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_hkg_canfd_base_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, + _flm_vehicle_knob("hyundai_kia_ev6.turn_in_threshold_reduction_left", KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_LEFT), + _flm_vehicle_knob("hyundai_kia_ev6.turn_in_threshold_reduction_right", KIA_EV6_TURN_IN_THRESHOLD_REDUCTION_RIGHT), + ) * + transition_envelope * turn_in_weight) + threshold_scale += (_kia_ev6_side_value( + desired_lateral_accel, + _flm_vehicle_knob("hyundai_kia_ev6.unwind_threshold_increase_left", KIA_EV6_UNWIND_THRESHOLD_INCREASE_LEFT), + _flm_vehicle_knob("hyundai_kia_ev6.unwind_threshold_increase_right", KIA_EV6_UNWIND_THRESHOLD_INCREASE_RIGHT), + ) * + transition_envelope * unwind_weight) + center_speed_weight = _kia_ev6_sigmoid((v_ego - KIA_EV6_CENTER_FRICTION_THRESHOLD_SPEED) / + KIA_EV6_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH) + center_lat_weight = _kia_ev6_sigmoid((KIA_EV6_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) / + KIA_EV6_CENTER_FRICTION_THRESHOLD_LAT_WIDTH) + threshold_scale += (_flm_vehicle_knob("hyundai_kia_ev6.center_friction_threshold_gain", + KIA_EV6_CENTER_FRICTION_THRESHOLD_GAIN) * + center_speed_weight * center_lat_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 = _flm_vehicle_knob("hyundai_kia_ev6.center_taper_max", 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 get_kia_ev6_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _kia_ev6_sigmoid((v_ego - KIA_EV6_CENTER_OUTPUT_TAPER_SPEED) / + KIA_EV6_CENTER_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _kia_ev6_sigmoid((KIA_EV6_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + KIA_EV6_CENTER_OUTPUT_TAPER_LAT_WIDTH) + reduction = _flm_vehicle_knob("hyundai_kia_ev6.center_output_taper_max", KIA_EV6_CENTER_OUTPUT_TAPER_MAX) * speed_weight * center_weight + return 1.0 - reduction + + +IONIQ_5_LOW_SPEED_TURN_CURVATURE_BP = [0.005, 0.020] +IONIQ_5_LOW_SPEED_TURN_OUTPUT_FLOOR = 0.30 +KIA_STINGER_2022_HIGHWAY_FRICTION_INCREMENT = 0.20 +KIA_STINGER_2022_HIGHWAY_FRICTION_JERK_BP = [0.20, 0.75] +KIA_STINGER_2022_HIGHWAY_FRICTION_LAT_BP = [0.15, 0.40] +KIA_STINGER_2022_HIGHWAY_FRICTION_SPEED_BP = [50.0 * CV.MPH_TO_MS, 60.0 * CV.MPH_TO_MS] + + +KIA_NIRO_EV_CENTER_FRICTION_SPEED_BP = [60.0 * CV.MPH_TO_MS, 65.0 * CV.MPH_TO_MS] + +KIA_NIRO_EV_CENTER_FRICTION_LAT_BP = [0.15, 0.35] + +KIA_NIRO_EV_CENTER_FRICTION_JERK_BP = [0.20, 0.50] + +KIA_NIRO_EV_CENTER_FRICTION_THRESHOLD_GAIN = 0.35 + +KIA_CARNIVAL_HEV_CENTER_SPEED_BP = [1.0, 4.0, 20.0, 25.0] + +KIA_CARNIVAL_HEV_CENTER_LAT_BP = [0.18, 0.40] + +KIA_CARNIVAL_HEV_CENTER_ERROR_BP = [0.12, 0.30] + +KIA_CARNIVAL_HEV_CENTER_JERK_BP = [0.25, 0.75] + +KIA_CARNIVAL_HEV_CENTER_FRICTION_INCREMENT = 0.20 + +KIA_CARNIVAL_HEV_CENTER_ERROR_REDUCTION = 0.25 + +def get_kia_niro_ev_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, + desired_lateral_jerk: float = 0.0, + future_lateral_accel: float | None = None) -> float: + speed_weight = np.interp(v_ego, KIA_NIRO_EV_CENTER_FRICTION_SPEED_BP, [0.0, 1.0]) + requested_accel = max(abs(desired_lateral_accel), abs(future_lateral_accel or 0.0)) + center_weight = np.interp(requested_accel, KIA_NIRO_EV_CENTER_FRICTION_LAT_BP, [1.0, 0.0]) + calm_weight = np.interp(abs(desired_lateral_jerk), KIA_NIRO_EV_CENTER_FRICTION_JERK_BP, [1.0, 0.0]) + gain = KIA_NIRO_EV_CENTER_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight * calm_weight + return float(get_standard_friction_threshold(v_ego) * (1.0 + gain)) + +def get_kia_carnival_hev_center_weight(v_ego: float, setpoint: float, measurement: float, jerk: float, + future_lateral_accel: float = 0.0) -> float: + speed_weight = np.interp(v_ego, KIA_CARNIVAL_HEV_CENTER_SPEED_BP, [0.0, 1.0, 1.0, 0.0]) + center_weight = np.interp(max(abs(setpoint), abs(measurement), abs(future_lateral_accel)), KIA_CARNIVAL_HEV_CENTER_LAT_BP, [1.0, 0.0]) + tracking_weight = np.interp(abs(setpoint - measurement), KIA_CARNIVAL_HEV_CENTER_ERROR_BP, [1.0, 0.0]) + calm_weight = np.interp(abs(jerk), KIA_CARNIVAL_HEV_CENTER_JERK_BP, [1.0, 0.0]) + return float(speed_weight * center_weight * tracking_weight * calm_weight) diff --git a/openpilot/starpilot/lateral/ioniq6_policy.py b/openpilot/starpilot/lateral/ioniq6_policy.py new file mode 100644 index 0000000000..62971d26f6 --- /dev/null +++ b/openpilot/starpilot/lateral/ioniq6_policy.py @@ -0,0 +1,897 @@ +"""Ioniq 6 torque shaping; native control retains output ownership and limits.""" + +from __future__ import annotations + +import math +import numpy as np + +from opendbc.car import structs +from opendbc.car.hyundai.values import CAR +from openpilot.common.constants import CV +from openpilot.starpilot.lateral.torque_shaping import ( + KP_INTERP, + KI, + LAT_ACCEL_REQUEST_BUFFER_SECONDS, + LP_FILTER_CUTOFF_HZ, + MAX_LAT_JERK_UP, + LOW_SPEED_X, + LOW_SPEED_Y, + MIN_SPEED, + JERK_GAIN, + FF_ROLL_OFFSET_FADE_BP, + FF_ROLL_OFFSET_FADE_V, + UNWIND_D_DES_THRESHOLD, + UNWIND_LAT_ACCEL_NEAR_ZERO, + center_chatter_friction_jerk_deadzone, + sigmoid as _sigmoid, +) + +HKG_CANFD_BASE_FRICTION_THRESHOLD = 0.39 +IONIQ_6_CARS = (CAR.HYUNDAI_IONIQ_6,) + + +def get_hkg_canfd_base_friction_threshold(v_ego: float) -> float: + base = 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])) + return max(base, HKG_CANFD_BASE_FRICTION_THRESHOLD) + + +def is_ioniq_6_2025_model(CP) -> bool: + if getattr(CP, "carFingerprint", None) not in IONIQ_6_CARS: + return False + try: + versions = [fw.fwVersion.decode("ascii", errors="ignore") if isinstance(fw.fwVersion, bytes) else str(fw.fwVersion) + for fw in CP.carFw] + except (AttributeError, TypeError, ValueError): + return False + return any("230915" in version for version in versions) and any("240206" in version for version in versions) + +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 = HKG_CANFD_BASE_FRICTION_THRESHOLD + +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 = 1.10 + +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 2.10 + +IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48 + +IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52 + +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.20 + +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.10 + +IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 1.00 + +IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.30 + +IONIQ_6_DIRECTIONAL_TAPER_PHASE_SCALE = 0.45 + +IONIQ_6_DIRECTIONAL_TAPER_FILTER_RC = 0.4 + +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_UNWIND_HIGH_SPEED_SPEED = 23.2 + +IONIQ_6_UNWIND_HIGH_SPEED_SPEED_WIDTH = 1.7 + +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_CURVY_SPEED_MIN = 7.2 + +IONIQ_6_CURVY_SPEED_MAX = 21.5 + +IONIQ_6_CURVY_SPEED_MIN_WIDTH = 1.1 + +IONIQ_6_CURVY_SPEED_MAX_WIDTH = 1.8 + +IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT = 0.26 + +IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT = 0.30 + +IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT = 0.22 + +IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT = 0.28 + +IONIQ_6_CURVY_UNWIND_LAT_START = 0.45 + +IONIQ_6_CURVY_UNWIND_LAT_END = 3.6 + +IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH = 0.14 + +IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH = 0.55 + +IONIQ_6_CURVY_RIGHT_UNWIND_JERK_ONSET = 0.40 + +IONIQ_6_CURVY_RIGHT_UNWIND_JERK_WIDTH = 0.22 + +IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN = 11.5 + +IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MAX = 20.5 + +IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH = 1.2 + +IONIQ_6_CURVY_TURN_IN_TRIM_LEFT = 0.08 + +IONIQ_6_CURVY_TURN_IN_TRIM_RIGHT = 0.09 + +IONIQ_6_CURVY_TURN_IN_TRIM_LAT_START = 1.0 + +IONIQ_6_CURVY_TURN_IN_TRIM_LAT_END = 2.5 + +IONIQ_6_CURVY_TURN_IN_TRIM_LAT_ONSET_WIDTH = 0.18 + +IONIQ_6_CURVY_TURN_IN_TRIM_LAT_CUTOFF_WIDTH = 0.30 + +IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX = 0.24 + +IONIQ_6_2023_UNWIND_FF_OVERSHOOT = 0.15 + +IONIQ_6_2023_UNWIND_FF_OVERSHOOT_WIDTH = 0.18 + +IONIQ_6_2023_UNWIND_FF_JERK = 0.10 + +IONIQ_6_2023_UNWIND_FF_JERK_WIDTH = 0.10 + +IONIQ_6_2023_UNWIND_FF_SPEED_ONSET = 8.0 + +IONIQ_6_2023_UNWIND_FF_SPEED_ONSET_WIDTH = 2.5 + +IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF = 23.5 + +IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF_WIDTH = 2.0 + +IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS + +IONIQ_6_FRICTION_JERK_DEADZONE = 0.30 + +IONIQ_6_FRICTION_CENTER_FADE_MAX = 0.50 + +IONIQ_6_FRICTION_CENTER_FADE_LAT = 0.15 + +IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH = 0.06 + +IONIQ_6_FRICTION_CENTER_FADE_SPEED = 18.0 + +IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH = 2.5 + +IONIQ_6_2025_FRICTION_SCALE_MULT = 0.80 + +IONIQ_6_2025_FRICTION_JERK_DEADZONE = 0.45 + +IONIQ_6_2025_CENTER_OUTPUT_TAPER_MAX = 0.32 + +IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT = 0.35 + +IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10 + +IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED = 22.0 + +IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.5 + +IONIQ_6_2025_LOW_SPEED_CENTER_ERROR_SCALE = 0.68 + +IONIQ_6_2025_LOW_SPEED_CENTER_FRICTION_SCALE = 0.72 + +IONIQ_6_2025_LOW_SPEED_CENTER_SPEED = 5.0 + +IONIQ_6_2025_LOW_SPEED_CENTER_SPEED_WIDTH = 1.3 + +IONIQ_6_2025_LOW_SPEED_CENTER_LAT = 0.22 + +IONIQ_6_2025_LOW_SPEED_CENTER_LAT_WIDTH = 0.10 + +IONIQ_6_2025_LOW_SPEED_CENTER_JERK = 0.30 + +IONIQ_6_2025_LOW_SPEED_CENTER_JERK_WIDTH = 0.13 + +IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_BASE = 0.22 + +IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF = 0.50 + +IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_RELIEF = 0.20 + +IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.5 + +IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5 + +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.03 + +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.11 + +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.40 + +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.55 + +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 _ioniq_6_curvy_speed_weight(v_ego: float) -> float: + curvy_speed_min = IONIQ_6_CURVY_SPEED_MIN + curvy_speed_max = IONIQ_6_CURVY_SPEED_MAX + onset = _ioniq_6_sigmoid((max(v_ego, 0.0) - curvy_speed_min) / IONIQ_6_CURVY_SPEED_MIN_WIDTH) + cutoff = _ioniq_6_sigmoid((curvy_speed_max - max(v_ego, 0.0)) / IONIQ_6_CURVY_SPEED_MAX_WIDTH) + return onset * cutoff + +def _ioniq_6_curvy_turn_in_trim_speed_weight(v_ego: float) -> float: + curvy_turn_in_speed_min = IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MIN + curvy_turn_in_speed_max = IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_MAX + onset = _ioniq_6_sigmoid((max(v_ego, 0.0) - curvy_turn_in_speed_min) / IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH) + cutoff = _ioniq_6_sigmoid((curvy_turn_in_speed_max - max(v_ego, 0.0)) / IONIQ_6_CURVY_TURN_IN_TRIM_SPEED_WIDTH) + return onset * cutoff + +def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float, + directional_taper_scale: float | None = None) -> 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 + if directional_taper_scale is None: + directional_taper_scale = get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) + return (1.0 + crawl_turn_in_scale + high_speed_right_turn_in_scale + + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * directional_taper_scale + +def get_ioniq_6_2023_unwind_ff_scale(setpoint: float, measured_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + """Trim residual curve feedforward when the 2023 car is already over-rotated.""" + if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0: + return 1.0 + + overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0) + if overshoot <= 0.0: + return 1.0 + + overshoot_weight = _ioniq_6_sigmoid((overshoot - IONIQ_6_2023_UNWIND_FF_OVERSHOOT) / + IONIQ_6_2023_UNWIND_FF_OVERSHOOT_WIDTH) + jerk_weight = _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_2023_UNWIND_FF_JERK) / + IONIQ_6_2023_UNWIND_FF_JERK_WIDTH) + speed_onset = _ioniq_6_sigmoid((v_ego - IONIQ_6_2023_UNWIND_FF_SPEED_ONSET) / + IONIQ_6_2023_UNWIND_FF_SPEED_ONSET_WIDTH) + speed_cutoff = _ioniq_6_sigmoid((IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF - v_ego) / + IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF_WIDTH) + reduction = (IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight * + speed_onset * speed_cutoff) + return 1.0 - reduction + +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_hkg_canfd_base_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) + unwind_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_UNWIND_HIGH_SPEED_SPEED) / IONIQ_6_UNWIND_HIGH_SPEED_SPEED_WIDTH) + 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 * unwind_speed_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) + unwind_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_UNWIND_HIGH_SPEED_SPEED) / IONIQ_6_UNWIND_HIGH_SPEED_SPEED_WIDTH) + 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 * unwind_speed_weight) + return min(max(friction_scale, 0.82), 1.08) + +def get_ioniq_6_friction_center_fade_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_FRICTION_CENTER_FADE_SPEED) / IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH) + center_weight = _ioniq_6_sigmoid((IONIQ_6_FRICTION_CENTER_FADE_LAT - abs(desired_lateral_accel)) / IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH) + return 1.0 - IONIQ_6_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight + +def get_ioniq_6_2025_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED) / + IONIQ_6_2025_CENTER_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _ioniq_6_sigmoid((IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + IONIQ_6_2025_CENTER_OUTPUT_TAPER_LAT_WIDTH) + return 1.0 - IONIQ_6_2025_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight + +def get_ioniq_6_2025_low_speed_output_limit(desired_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + """Limit small-signal torque at crawl speed while leaving real turn commands open.""" + speed_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED - max(v_ego, 0.0)) / + IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH) + center_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_LAT - abs(desired_lateral_accel)) / + IONIQ_6_2025_LOW_SPEED_CENTER_LAT_WIDTH) + calm_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_JERK - abs(desired_lateral_jerk)) / + IONIQ_6_2025_LOW_SPEED_CENTER_JERK_WIDTH) + center_weight *= calm_weight + limit = (IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_BASE + + IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_TURN_RELIEF * (1.0 - center_weight) + + IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_SPEED_RELIEF * (1.0 - speed_weight)) + return float(np.clip(limit, IONIQ_6_2025_LOW_SPEED_OUTPUT_LIMIT_BASE, 1.0)) + +def _ioniq_6_2025_low_speed_center_envelope(desired_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + speed_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_SPEED - max(v_ego, 0.0)) / + IONIQ_6_2025_LOW_SPEED_CENTER_SPEED_WIDTH) + center_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_LAT - abs(desired_lateral_accel)) / + IONIQ_6_2025_LOW_SPEED_CENTER_LAT_WIDTH) + calm_weight = _ioniq_6_sigmoid((IONIQ_6_2025_LOW_SPEED_CENTER_JERK - abs(desired_lateral_jerk)) / + IONIQ_6_2025_LOW_SPEED_CENTER_JERK_WIDTH) + return speed_weight * center_weight * calm_weight + +def get_ioniq_6_2025_low_speed_center_error_scale(desired_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + envelope = _ioniq_6_2025_low_speed_center_envelope(desired_lateral_accel, desired_lateral_jerk, v_ego) + return 1.0 - (1.0 - IONIQ_6_2025_LOW_SPEED_CENTER_ERROR_SCALE) * envelope + +def get_ioniq_6_2025_low_speed_center_friction_scale(desired_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + envelope = _ioniq_6_2025_low_speed_center_envelope(desired_lateral_accel, desired_lateral_jerk, v_ego) + return 1.0 - (1.0 - IONIQ_6_2025_LOW_SPEED_CENTER_FRICTION_SCALE) * envelope + +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 = math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_6_DIRECTIONAL_TAPER_PHASE_SCALE) + 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 + curvy_turn_in_trim_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) + turn_in_weight = max(phase, 0.0) + curvy_turn_in_speed_weight = _ioniq_6_curvy_turn_in_trim_speed_weight(v_ego) + curvy_turn_in_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CURVY_TURN_IN_TRIM_LAT_START) / + IONIQ_6_CURVY_TURN_IN_TRIM_LAT_ONSET_WIDTH) + curvy_turn_in_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_TURN_IN_TRIM_LAT_END - abs_lateral_accel) / + IONIQ_6_CURVY_TURN_IN_TRIM_LAT_CUTOFF_WIDTH) + curvy_turn_in_trim_weight = curvy_turn_in_speed_weight * curvy_turn_in_lat_onset * curvy_turn_in_lat_cutoff * turn_in_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) + reduction += (_ioniq_6_side_value(desired_lateral_accel, + IONIQ_6_CURVY_TURN_IN_TRIM_LEFT, + IONIQ_6_CURVY_TURN_IN_TRIM_RIGHT) * + curvy_turn_in_trim_weight) + curvy_unwind_weight = 0.0 + curvy_unwind_floor_relief = 0.0 + if v_ego is not None: + curvy_unwind_phase_weight = unwind_weight + if desired_lateral_accel < 0.0: + curvy_unwind_phase_weight = max(-phase, 0.0) * _ioniq_6_sigmoid( + (abs(desired_lateral_jerk) - IONIQ_6_CURVY_RIGHT_UNWIND_JERK_ONSET) / IONIQ_6_CURVY_RIGHT_UNWIND_JERK_WIDTH) + curvy_unwind_speed_weight = _ioniq_6_curvy_speed_weight(v_ego) + curvy_unwind_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CURVY_UNWIND_LAT_START) / + IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH) + curvy_unwind_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_UNWIND_LAT_END - abs_lateral_accel) / + IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH) + curvy_unwind_weight = curvy_unwind_speed_weight * curvy_unwind_lat_onset * curvy_unwind_lat_cutoff * curvy_unwind_phase_weight + curvy_unwind_floor_relief = (_ioniq_6_side_value(desired_lateral_accel, + IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT, + IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT) * + curvy_unwind_weight) + reduction += (_ioniq_6_side_value(desired_lateral_accel, + IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT, + IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT) * + curvy_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 + floor -= curvy_unwind_floor_relief + return max(1.0 - reduction, floor) + +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) + # Sigmoid tails remain nonzero at zero error. Taper only inside the + # existing transition width so sign reversals cannot introduce a torque step. + error_weight *= min(abs(angle_error) / IONIQ_6_LOW_SPEED_ANGLE_ASSIST_ERROR_WIDTH, 1.0) + 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) + error_weight *= min(abs(angle_error) / IONIQ_6_LOW_SPEED_UNWIND_ASSIST_ERROR_WIDTH, 1.0) + 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)) + + +# Buffer curvature and scale at the current speed to avoid pull-away unwind +# from lateral acceleration recorded at old speeds. +from collections import deque + +from openpilot.cereal import log +from opendbc.car.lateral import get_friction +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.pid import PIDController + +class Ioniq6TorquePolicy: + """Per-car controller state; the native LatControlTorque still owns output.""" + + FACTOR_MULT = IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT + + def __init__(self, parent, CP, *, surface=None, turn_assist: bool = False): + # Validate the replay-only surface before changing the parent's PID, factor or limits. + surface_math = None + if surface is not None: + from openpilot.starpilot.flm import torque_surface as surface_math + expected_variant = "firmware_2025" if is_ioniq_6_2025_model(CP) else "standard" + if (str(CP.carFingerprint) != "HYUNDAI_IONIQ_6" or + not isinstance(surface, surface_math.Ioniq6Surface) or surface.variant != expected_variant): + raise ValueError("Ioniq 6 surface does not match CarParams") + self.turn_assist = bool(turn_assist) + minimum = float(CP.minSteerSpeed) + self.turn_assist_min_speed = max(0.044704, minimum) if math.isfinite(minimum) else math.inf + self.surface = surface + self.surface_math = surface_math + self.parent = parent + self.dt = parent.dt + self.is_2025 = is_ioniq_6_2025_model(CP) + self.vehicle_factor = float(CP.lateralTuning.torque.latAccelFactor) + # Startup limits use raw CP calibration before the 1.22 factor; using compensated + # parameters would widen them before the first live torque update. + startup_params = CP.lateralTuning.torque.as_builder() + startup_pos_limit = parent.lateral_accel_from_torque(parent.steer_max, startup_params) + startup_neg_limit = parent.lateral_accel_from_torque(-parent.steer_max, startup_params) + parent.pid = PIDController([[0.0], [KP_INTERP[-1]]], KI, rate=1 / self.dt) + parent.pid.set_limits(startup_pos_limit, startup_neg_limit) + parent.torque_params.latAccelFactor = self.vehicle_factor * IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT + self.request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) + self.curvature_request_buffer = deque([0.0] * self.request_buffer_len, maxlen=self.request_buffer_len) + self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) + self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) + self.directional_taper_filter = FirstOrderFilter(1.0, IONIQ_6_DIRECTIONAL_TAPER_FILTER_RC, self.dt) + self.previous_measurement = 0.0 + self.prev_desired_lateral_accel = 0.0 + self.prev_steering_pressed = False + self.low_speed_reset_threshold = min(max(CP.minSteerSpeed, 0.3), IONIQ_6_LOW_SPEED_PID_RESET_SPEED) + + def turn_assist_active(self, CS) -> bool: + return bool(self.turn_assist and math.isfinite(CS.vEgo) and + CS.vEgo >= self.turn_assist_min_speed and not CS.standstill and + CS.gearShifter == structs.CarState.GearShifter.drive and + not CS.steeringPressed and not CS.steerFaultTemporary and not CS.steerFaultPermanent) + + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + parent = self.parent + pid_log = log.ControlsState.LateralTorqueState.new_message() + pid_log.version = 2 + measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + measurement = measured_curvature * CS.vEgo ** 2 + future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + if not active: + parent.pid.reset() + self.curvature_request_buffer.append(desired_curvature) + self.previous_measurement = measurement + self.measurement_rate_filter.x = 0.0 + self.jerk_filter.x = 0.0 + self.directional_taper_filter.x = 1.0 + self.prev_desired_lateral_accel = future_desired_lateral_accel + self.prev_steering_pressed = CS.steeringPressed + pid_log.active = False + return 0.0, 0.0, pid_log + + if self.prev_steering_pressed and not CS.steeringPressed: + parent.pid.i *= 0.8 + roll_offset_fade = float(np.interp(CS.vEgo, FF_ROLL_OFFSET_FADE_BP, FF_ROLL_OFFSET_FADE_V)) + roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY * roll_offset_fade + effective_deadzone_deg = parent.steering_angle_deadzone_deg + if self.surface is not None: + effective_deadzone_deg += self.surface_math.center_deadband(self.surface, CS.vEgo) + curvature_deadzone = abs(VM.calc_curvature(math.radians(effective_deadzone_deg), CS.vEgo, 0.0)) + lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 + delay_frames = int(np.clip(lat_delay / self.dt, 1, self.request_buffer_len)) + expected_lateral_accel = self.curvature_request_buffer[-delay_frames] * CS.vEgo ** 2 + self.curvature_request_buffer.append(desired_curvature) + raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt) + raw_lateral_jerk = float(np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + desired_lateral_jerk = float(np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation + setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay + desired_lateral_accel_rate = (setpoint - self.prev_desired_lateral_accel) / self.dt + unwind_detected = desired_lateral_accel_rate < UNWIND_D_DES_THRESHOLD and abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO + self.prev_desired_lateral_accel = setpoint + measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt) + measurement_rate = float(np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)) + self.previous_measurement = measurement + low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2 + current_kp = np.interp(CS.vEgo, parent.pid._k_p[0], parent.pid._k_p[1]) + error = setpoint - measurement + error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3)) + if self.is_2025: + error_with_lsf *= get_ioniq_6_2025_low_speed_center_error_scale(setpoint, desired_lateral_jerk, CS.vEgo) + pid_log.error = float(error_with_lsf) + ff = gravity_adjusted_future_lateral_accel - parent.torque_params.latAccelOffset * roll_offset_fade + if self.surface is None: + center_taper = get_ioniq_6_center_taper_scale(setpoint, CS.vEgo) + directional_target = get_ioniq_6_directional_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo) + else: + center_taper = self.surface_math.center_taper(self.surface, CS.vEgo, setpoint) + directional_target = self.surface_math.directional_taper_target(self.surface, CS.vEgo, setpoint, desired_lateral_jerk) + directional_taper = self.directional_taper_filter.update(directional_target) + if self.surface is None: + ff *= get_ioniq_6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo, directional_taper_scale=directional_taper) * center_taper + else: + ff *= self.surface_math.feedforward_scale(self.surface, CS.vEgo, setpoint, desired_lateral_jerk, directional_taper) * center_taper + if not self.is_2025: + ff *= get_ioniq_6_2023_unwind_ff_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo) + if self.surface is None: + friction_threshold = get_ioniq_6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) / max(center_taper, 1e-3) + else: + friction_threshold = self.surface_math.friction_threshold(self.surface, CS.vEgo, setpoint, desired_lateral_jerk) / max(center_taper, 1e-3) + friction_scale = get_ioniq_6_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) + friction_scale = 1.0 + ((friction_scale - 1.0) * center_taper) + friction_scale *= get_ioniq_6_friction_center_fade_scale(setpoint, CS.vEgo) + if self.is_2025: + friction_scale *= IONIQ_6_2025_FRICTION_SCALE_MULT + friction_scale *= get_ioniq_6_2025_low_speed_center_friction_scale(setpoint, desired_lateral_jerk, CS.vEgo) + vehicle_jerk_deadzone = IONIQ_6_2025_FRICTION_JERK_DEADZONE if self.is_2025 else IONIQ_6_FRICTION_JERK_DEADZONE + friction_jerk_deadzone = center_chatter_friction_jerk_deadzone(CS.vEgo, setpoint, vehicle_jerk_deadzone) + friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - friction_jerk_deadzone, 0.0), desired_lateral_jerk) + ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * friction_jerk, lateral_accel_deadzone, + friction_threshold, parent.torque_params) + if CS.vEgo < self.low_speed_reset_threshold: + parent.pid.reset() + freeze_integrator = (steer_limited_by_safety or CS.steeringPressed or + CS.vEgo < self.low_speed_reset_threshold or unwind_detected) + output_lataccel = parent.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo, + feedforward=ff, freeze_integrator=freeze_integrator) + output_torque = parent.torque_from_lateral_accel(output_lataccel, parent.torque_params) + if self.turn_assist_active(CS): + desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) + actual_angle = CS.steeringAngleDeg - params.angleOffsetDeg + if math.isfinite(desired_angle) and math.isfinite(actual_angle) and math.isfinite(output_torque): + if self.surface is None: + output_torque = get_ioniq_6_low_speed_angle_assist_torque(desired_angle, actual_angle, output_torque, CS.vEgo) + else: + output_torque = self.surface_math.low_speed_output(self.surface, self.surface_math.SurfaceInput( + CS.vEgo, setpoint, desired_lateral_jerk, desired_angle, actual_angle, output_torque, False, directional_taper)) + output_torque *= get_ioniq_6_highway_output_taper_scale(setpoint, CS.vEgo) + output_torque *= get_ioniq_6_highway_transition_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo) + if self.is_2025: + output_torque *= get_ioniq_6_2025_center_output_scale(setpoint, CS.vEgo) + output_limit = get_ioniq_6_2025_low_speed_output_limit(setpoint, desired_lateral_jerk, CS.vEgo) + output_torque = float(np.clip(output_torque, -output_limit, output_limit)) + pid_log.active = True + pid_log.p = float(parent.pid.p) + pid_log.i = float(parent.pid.i) + pid_log.d = float(parent.pid.d) + pid_log.f = float(parent.pid.f) + pid_log.output = float(-output_torque) + pid_log.actualLateralAccel = float(measurement) + pid_log.desiredLateralAccel = float(setpoint) + pid_log.desiredLateralJerk = float(desired_lateral_jerk) + pid_log.saturated = bool(parent._check_saturation(parent.steer_max - abs(output_torque) < 1e-3, + CS, steer_limited_by_safety, curvature_limited)) + self.prev_steering_pressed = CS.steeringPressed + return -output_torque, 0.0, pid_log diff --git a/openpilot/starpilot/lateral/lane_centering.py b/openpilot/starpilot/lateral/lane_centering.py new file mode 100644 index 0000000000..07c76c1fe9 --- /dev/null +++ b/openpilot/starpilot/lateral/lane_centering.py @@ -0,0 +1,275 @@ +"""Bounded lane-centering contribution for lateral control. + +The caller owns model freshness, axis authority, and final curvature limiting. +This module neither reads settings nor sends commands. +""" + +from dataclasses import dataclass +from enum import StrEnum +import math +from numbers import Real +from typing import Any + +import capnp +import numpy as np + +from openpilot.cereal import log + + +STRENGTH_BASELINE = 1.5 +STRENGTH_MAX = 2 * STRENGTH_BASELINE + +MIN_SPEED = 5.0 +MIN_LANE_PROB = 0.6 +MAX_LANE_STD = 0.3 +MIN_LANE_WIDTH = 2.6 +MAX_LANE_WIDTH = 4.8 +MAX_OFFSET = 0.3 +MIN_CENTER_TO_LINE = 1.1 +MAX_CORRECTION = 0.004 * 0.30 +CENTER_ERROR_DEADBAND = 0.08 +E2E_MAX_PATH_STD = 0.35 +E2E_BREAK_IN_START = 0.15 +E2E_BREAK_IN_FULL = 0.50 +ACQUIRE_TAU = 0.4 +RELEASE_TAU = 0.2 + + +class ControlMode(StrEnum): + OFF = "off" + LATERAL_ONLY = "lateral_only" + LONGITUDINAL_ONLY = "longitudinal_only" + COMBINED = "combined" + + @property + def lateral_authority(self) -> bool: + return self in (ControlMode.LATERAL_ONLY, ControlMode.COMBINED) + + +@dataclass(frozen=True) +class LaneCenteringSettings: + enabled: bool = False + offset_m: float = 0.0 + e2e_authority: float = 1.0 + pause_on_signal: bool = True + strength: float = STRENGTH_BASELINE + + +@dataclass(frozen=True) +class LaneCenteringRequest: + """Optional host configuration; vehicle/model facts come from the host itself.""" + + mode: ControlMode + settings: LaneCenteringSettings + elapsed_seconds: float + time_discontinuity: bool = False + + +@dataclass(frozen=True) +class LaneCenteringInput: + model: Any + model_sample_id: int + model_valid: bool + base_curvature: float + speed_mps: float + mode: ControlMode + lateral_active: bool + settings: LaneCenteringSettings + elapsed_seconds: float + turn_signal_active: bool = False + driver_override: bool = False + time_discontinuity: bool = False + + +@dataclass(frozen=True) +class LaneCenteringResult: + # None means the caller's proposed curvature was invalid and must be rejected. + candidate_curvature: float | None + correction: float + direction: int + reason: str + + +def _finite_number(value: Any) -> float | None: + if isinstance(value, bool) or not isinstance(value, Real): + return None + try: + result = float(value) + except (TypeError, ValueError, OverflowError): + return None + return result if math.isfinite(result) else None + + +def _path(model_path: Any) -> tuple[np.ndarray, np.ndarray] | None: + try: + return _path_xy(model_path.x, model_path.y) + except (AttributeError, TypeError, ValueError, OverflowError): + return None + + +def _path_xy(xs: Any, ys: Any) -> tuple[np.ndarray, np.ndarray] | None: + try: + x = np.asarray(xs) + y = np.asarray(ys) + except (TypeError, ValueError, OverflowError): + return None + if x.ndim != 1 or y.ndim != 1 or len(x) < 2 or len(x) != len(y): + return None + if x.dtype.kind not in "fiu" or y.dtype.kind not in "fiu": + return None + x, y = x.astype(float), y.astype(float) + if not np.isfinite(x).all() or not np.isfinite(y).all() or not np.all(np.diff(x) > 0): + return None + return x, y + + +def _at(path: tuple[np.ndarray, np.ndarray], distance: float) -> float | None: + x, y = path + if not x[0] <= distance <= x[-1]: + return None + return float(np.interp(distance, x, y)) + + +def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority: float, *, _paths=None) -> float | None: + """Return a finite raw curvature correction, or None for unqualified model data.""" + speed = _finite_number(speed_mps) + offset = _finite_number(offset_m) + authority = _finite_number(e2e_authority) + if speed is None or offset is None or authority is None or speed < MIN_SPEED: + return None + try: + lines = model.laneLines + probs = model.laneLineProbs + stds = model.laneLineStds + if len(lines) < 3 or len(probs) < 3 or len(stds) < 3: + return None + for i in (1, 2): + prob = _finite_number(probs[i]) + std = _finite_number(stds[i]) + if prob is None or std is None or not MIN_LANE_PROB <= prob <= 1.0 or not 0.0 <= std <= MAX_LANE_STD: + return None + left_path, right_path, model_path = (_path(lines[1]), _path(lines[2]), _path(model.position)) if _paths is None else _paths[:3] + if left_path is None or right_path is None or model_path is None: + return None + lookahead = float(np.clip(speed, 8.0, 35.0)) + left, right, model_y = _at(left_path, lookahead), _at(right_path, lookahead), _at(model_path, lookahead) + if left is None or right is None or model_y is None: + return None + width = right - left + if not MIN_LANE_WIDTH <= width <= MAX_LANE_WIDTH: + return None + safe_offset = min(MAX_OFFSET, max(0.0, width * 0.5 - MIN_CENTER_TO_LINE)) + target_y = (left + right) * 0.5 + float(np.clip(offset, -safe_offset, safe_offset)) + error = target_y - model_y + error_abs = abs(error) + error = math.copysign(max(0.0, error_abs - CENTER_ERROR_DEADBAND), error) + + # Reduce lane authority only when path variance establishes confidence. + try: + std_path = _path_xy(model.position.x, model.position.yStd) if _paths is None else _paths[3] + except (AttributeError, TypeError, ValueError, OverflowError): + std_path = None + if std_path is not None: + path_std = _at(std_path, lookahead) + if path_std is not None and 0.0 <= path_std <= E2E_MAX_PATH_STD: + break_in = float(np.clip((error_abs - E2E_BREAK_IN_START) / + (E2E_BREAK_IN_FULL - E2E_BREAK_IN_START), 0.0, 1.0)) + error *= 1.0 - float(np.clip(authority, 0.0, 1.0)) * break_in + correction = 2.0 * error / lookahead ** 2 + return correction if math.isfinite(correction) else None + except (AttributeError, IndexError, TypeError, ValueError, OverflowError): + return None + + +def _reader_geometry_key(model): + if not isinstance(model, capnp.lib.capnp._DynamicStructReader): + return None + try: + return (tuple(model.laneLineProbs), tuple(model.laneLineStds), + *((tuple(line.x), tuple(line.y)) for line in (model.laneLines[1], model.laneLines[2])), + tuple(model.position.x), tuple(model.position.y), tuple(model.position.yStd)) + except (AttributeError, IndexError, TypeError, ValueError, OverflowError): + return None + + +class LaneCenteringController: + def __init__(self) -> None: + self._correction = 0.0 + self._geometry_cache = None + + def reset(self) -> None: + self._correction = 0.0 + self._geometry_cache = None + + def _result(self, base: float, reason: str) -> LaneCenteringResult: + correction = self._correction + return LaneCenteringResult(base + correction, correction, (correction > 0) - (correction < 0), reason) + + def _fade(self, base: float, dt: float, reason: str) -> LaneCenteringResult: + self._correction *= math.exp(-dt / RELEASE_TAU) + if abs(self._correction) < 1e-12: + self._correction = 0.0 + return self._result(base, reason) + + def update(self, observation: LaneCenteringInput) -> LaneCenteringResult: + if not isinstance(observation, LaneCenteringInput): + self.reset() + return LaneCenteringResult(None, 0.0, 0, "invalid_input") + base = _finite_number(observation.base_curvature) + speed = _finite_number(observation.speed_mps) + dt = _finite_number(observation.elapsed_seconds) + settings = observation.settings + offset = _finite_number(settings.offset_m) if isinstance(settings, LaneCenteringSettings) else None + authority = _finite_number(settings.e2e_authority) if isinstance(settings, LaneCenteringSettings) else None + strength = _finite_number(settings.strength) if isinstance(settings, LaneCenteringSettings) else None + if (base is None or speed is None or dt is None or dt <= 0 or + offset is None or authority is None or strength is None or not 0.5 <= strength <= STRENGTH_MAX or type(observation.model_sample_id) is not int or + observation.model_sample_id < 0 or not isinstance(observation.mode, ControlMode) or + type(observation.model_valid) is not bool or type(observation.lateral_active) is not bool or + type(observation.turn_signal_active) is not bool or type(observation.driver_override) is not bool or + type(observation.time_discontinuity) is not bool or + type(settings.enabled) is not bool or type(settings.pause_on_signal) is not bool): + self.reset() + return LaneCenteringResult(base, 0.0, 0, "invalid_input") + if observation.time_discontinuity: + self.reset() + return self._result(base, "time_discontinuity") + if not observation.mode.lateral_authority or not settings.enabled or not observation.lateral_active or \ + not observation.model_valid or speed < MIN_SPEED: + self.reset() + return self._result(base, "inactive") + if observation.driver_override: + self.reset() + return self._result(base, "driver_override") + if settings.pause_on_signal and observation.turn_signal_active: + return self._fade(base, dt, "signal_release") + try: + if observation.model.meta.laneChangeState != log.LaneChangeState.off: + self.reset() + return self._result(base, "lane_change") + except (AttributeError, TypeError, ValueError): + self.reset() + return self._result(base, "invalid_model") + # Filtering runs per control tick, including repeated model samples. + geometry = _reader_geometry_key(observation.model) + cache_key = (observation.model_sample_id, geometry) + cached = self._geometry_cache + paths = None + if geometry is not None: + if cached is not None and cached[0] is observation.model and cached[1] == cache_key: + paths = cached[2] + else: + paths = (_path_xy(*geometry[2]), _path_xy(*geometry[3]), + _path_xy(geometry[4], geometry[5]), _path_xy(geometry[4], geometry[6])) + self._geometry_cache = (observation.model, cache_key, paths) + else: + self._geometry_cache = None + raw = raw_correction(observation.model, speed, offset, authority, _paths=paths) + if raw is None or not math.isfinite(raw): + return self._fade(base, dt, "unqualified_model") + target = float(np.clip(raw, -0.004, 0.004)) * 0.30 + if strength != 1.0: + target = float(np.clip(target * strength, -MAX_CORRECTION, MAX_CORRECTION)) + self._correction += (target - self._correction) * (1.0 - math.exp(-dt / ACQUIRE_TAU)) + self._correction = float(np.clip(self._correction, -MAX_CORRECTION, MAX_CORRECTION)) + return self._result(base, "qualified") diff --git a/openpilot/starpilot/lateral/lane_change_preferences.py b/openpilot/starpilot/lateral/lane_change_preferences.py new file mode 100644 index 0000000000..5611fca6cd --- /dev/null +++ b/openpilot/starpilot/lateral/lane_change_preferences.py @@ -0,0 +1,204 @@ +"""Optional, session-scoped preferences for driver-nudged lane changes.""" + +from dataclasses import dataclass, replace +import json +import math +import os +import stat +from typing import Any + +from openpilot.common.constants import CV + + +KEY = "LaneChangePreferences" +MAX_BYTES = 512 +MAX_SPEED_MPS = 100 * CV.MPH_TO_MS +MINIMUM_SPEED_MPS = 5 * CV.MPH_TO_MS +DEFAULT_SPEED_MPS = MINIMUM_SPEED_MPS + + +@dataclass(frozen=True) +class LaneChangePolicy: + enabled: bool = True + minimum_speed_mps: float = DEFAULT_SPEED_MPS + one_per_signal: bool = True + auto_lane_change: bool = False + auto_delay_s: float = 1.0 + minimum_lane_width_m: float = 0.0 + close_gap: bool = False + close_gap_seconds: float = 0.75 + duration_s: float = 3.0 + 25.0 / 9.0 + + +@dataclass(frozen=True) +class SavedLaneChange: + raw: bytes | None + policy: LaneChangePolicy = LaneChangePolicy() + valid: bool = True + readable: bool = True + + +def _unique_object(pairs: list[tuple[str, object]]) -> dict[str, object]: + value: dict[str, object] = {} + for key, item in pairs: + if key in value: + raise ValueError("duplicate lane-change field") + value[key] = item + return value + + +def decode(raw: bytes) -> LaneChangePolicy | None: + if len(raw) > MAX_BYTES: + return None + try: + data = json.loads(raw, object_pairs_hook=_unique_object, + parse_constant=lambda _value: (_ for _ in ()).throw(ValueError("nonfinite"))) + except (UnicodeDecodeError, ValueError, TypeError, RecursionError, OverflowError): + return None + if not isinstance(data, dict): + return None + version = data.get("version") + fields = {"version", "enabled", "minimumSpeedMps", "onePerSignal"} + if version in (2, 3, 4): + fields |= {"autoLaneChange", "autoDelayS", "minimumLaneWidthM"} + if version in (3, 4): + fields |= {"closeGap", "closeGapSeconds"} + if version == 4: + fields |= {"durationS"} + if set(data) != fields: + return None + enabled = data["enabled"] + speed, once = data["minimumSpeedMps"], data["onePerSignal"] + if (type(version) is not int or version not in (1, 2, 3, 4) or type(enabled) is not bool or type(once) is not bool or + type(speed) not in (int, float)): + return None + try: + finite_speed = float(speed) + except (ValueError, OverflowError): + return None + if not math.isfinite(finite_speed) or not 0 <= finite_speed <= MAX_SPEED_MPS: + return None + if version == 1: + return LaneChangePolicy(enabled, finite_speed, once) + auto, delay, width = data["autoLaneChange"], data["autoDelayS"], data["minimumLaneWidthM"] + if type(auto) is not bool or type(delay) not in (int, float) or type(width) not in (int, float): + return None + try: + delay, width = float(delay), float(width) + except (ValueError, OverflowError): + return None + if not (math.isfinite(delay) and math.isfinite(width) and 0 <= delay <= 5 and 0 <= width <= 15 * 0.3048): + return None + if version == 2: + return LaneChangePolicy(enabled, finite_speed, once, auto, delay, width) + close_gap, seconds = data["closeGap"], data["closeGapSeconds"] + if type(close_gap) is not bool or type(seconds) not in (int, float): + return None + try: + seconds = float(seconds) + except (ValueError, OverflowError): + return None + if not math.isfinite(seconds) or not 0.75 <= seconds <= 1.0: + return None + duration = data.get("durationS", LaneChangePolicy().duration_s) + if type(duration) not in (int, float) or not math.isfinite(duration) or not 3.0 <= duration <= 8.0: + return None + return LaneChangePolicy(enabled, finite_speed, once, auto, delay, width, close_gap, seconds, float(duration)) + + +def to_value(policy: LaneChangePolicy) -> dict[str, int | bool | float]: + if (type(policy.enabled) is not bool or type(policy.one_per_signal) is not bool or + type(policy.minimum_speed_mps) not in (int, float)): + raise ValueError("invalid lane-change policy") + try: + finite_speed = float(policy.minimum_speed_mps) + except (ValueError, OverflowError) as error: + raise ValueError("invalid lane-change speed") from error + if not math.isfinite(finite_speed) or not 0 <= finite_speed <= MAX_SPEED_MPS: + raise ValueError("invalid lane-change speed") + if type(policy.auto_lane_change) is not bool or type(policy.auto_delay_s) not in (int, float) or type(policy.minimum_lane_width_m) not in (int, float): + raise ValueError("invalid automatic lane-change policy") + try: + delay, width = float(policy.auto_delay_s), float(policy.minimum_lane_width_m) + except (ValueError, OverflowError) as error: + raise ValueError("invalid automatic lane-change bounds") from error + if not (math.isfinite(delay) and math.isfinite(width) and 0 <= delay <= 5 and 0 <= width <= 15 * 0.3048): + raise ValueError("invalid automatic lane-change bounds") + if type(policy.close_gap) is not bool or type(policy.close_gap_seconds) not in (int, float): + raise ValueError("invalid lane-change close gap") + try: + seconds = float(policy.close_gap_seconds) + except (ValueError, OverflowError) as error: + raise ValueError("invalid lane-change close gap") from error + if not math.isfinite(seconds) or not 0.75 <= seconds <= 1.0: + raise ValueError("invalid lane-change close gap") + if type(policy.duration_s) not in (int, float) or not math.isfinite(policy.duration_s) or not 3.0 <= policy.duration_s <= 8.0: + raise ValueError("invalid lane-change duration") + return {"version": 4, "enabled": policy.enabled, "minimumSpeedMps": finite_speed, + "onePerSignal": policy.one_per_signal, "autoLaneChange": policy.auto_lane_change, + "autoDelayS": delay, "minimumLaneWidthM": width, + "closeGap": policy.close_gap, "closeGapSeconds": seconds, "durationS": policy.duration_s} + + +def read_saved(params: Any) -> SavedLaneChange: + try: + fd = os.open(params.get_param_path(KEY), os.O_RDONLY | os.O_NONBLOCK | os.O_NOFOLLOW) + except FileNotFoundError: + return SavedLaneChange(None) + except (AttributeError, OSError, TypeError, ValueError): + return SavedLaneChange(None, valid=False, readable=False) + try: + if not stat.S_ISREG(os.fstat(fd).st_mode): + return SavedLaneChange(None, valid=False, readable=False) + raw = os.read(fd, MAX_BYTES + 1) + except OSError: + return SavedLaneChange(None, valid=False, readable=False) + finally: + os.close(fd) + if len(raw) > MAX_BYTES: + return SavedLaneChange(raw, valid=False, readable=False) + policy = decode(raw) + return SavedLaneChange(raw, policy if policy is not None else LaneChangePolicy(), valid=policy is not None) + + +def effective(saved: SavedLaneChange) -> LaneChangePolicy: + policy = saved.policy if saved.readable and saved.valid else LaneChangePolicy() + return replace(policy, minimum_speed_mps=max(MINIMUM_SPEED_MPS, policy.minimum_speed_mps)) + + +class LiveLaneChangePolicy: + """One canonical document, bounded refresh, safe per-maneuver adoption. + + Disabling assistance is immediate on the next successful refresh. Enabling + it, or changing maneuver parameters, cannot turn a held signal into a newly + authorized maneuver or alter an already committed maneuver's pace. + """ + REFRESH_NS = 1_000_000_000 + + def __init__(self, params, *, now_ns=0): + self.params = params + saved = read_saved(params) + self.requested = effective(saved) if saved.readable and saved.valid else self._disabled() + self.applied = self.requested + self.last_read_ns = now_ns if type(now_ns) is int and now_ns > 0 else 0 + + @staticmethod + def _disabled(): + return replace(LaneChangePolicy(), enabled=False, auto_lane_change=False) + + def sample(self, now_ns, *, signaling=False, maneuver_active=False): + if type(now_ns) is not int or now_ns <= 0 or now_ns < self.last_read_ns: + self.last_read_ns = 0 + self.requested = self.applied = self._disabled() + return self.applied + if not self.last_read_ns or now_ns - self.last_read_ns >= self.REFRESH_NS: + saved = read_saved(self.params) + self.requested = effective(saved) if saved.readable and saved.valid else self._disabled() + self.last_read_ns = now_ns + if not self.requested.enabled: + self.applied = replace(self.applied, enabled=False, auto_lane_change=False) + elif not self.requested.auto_lane_change and self.applied.auto_lane_change: + self.applied = replace(self.applied, auto_lane_change=False) + if not signaling and not maneuver_active: + self.applied = self.requested + return self.applied diff --git a/openpilot/starpilot/lateral/lane_change_smoothing.py b/openpilot/starpilot/lateral/lane_change_smoothing.py new file mode 100644 index 0000000000..bc1d596033 --- /dev/null +++ b/openpilot/starpilot/lateral/lane_change_smoothing.py @@ -0,0 +1,65 @@ +"""Jerk-only lane-change shaping with a gradual return to native limits.""" + +import math + +from openpilot.cereal import log +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN +from openpilot.selfdrive.controls.lib.drive_helpers import MAX_LATERAL_JERK, MIN_SPEED + + +class LaneChangeSmoother: + def __init__(self): + self.reset() + + def reset(self): + self.release = 0.0 + self.entry_sign = 0.0 + self.previous_factor = 1.0 + self.intersection_maneuver = False + + def factor(self, *, active, lane_change_state, speed, minimum_speed, duration, previous, desired, turn_assist=False): + if (not active or not all(math.isfinite(value) for value in (speed, minimum_speed, duration, previous, desired)) or + not 3.0 < duration <= 8.0): + self.reset() + return 1.0 + if not turn_assist: + self.intersection_maneuver = False + maneuver = lane_change_state in (log.LaneChangeState.laneChangeStarting, log.LaneChangeState.laneChangeFinishing) + # A model lane-change state can outlive an intersection turn while the car + # accelerates across the highway lane-change floor. Finish that exemption + # before admitting pacing for a new maneuver. + intersection = bool(turn_assist and maneuver and + (speed < LANE_CHANGE_SPEED_MIN or self.intersection_maneuver)) + if turn_assist and (speed < LANE_CHANGE_SPEED_MIN or self.intersection_maneuver): + self.reset() + self.intersection_maneuver = intersection + return 1.0 + base = min(1., math.pi ** 3 * 3.5 / duration ** 3 * 1.3 / MAX_LATERAL_JERK) + if maneuver and speed >= minimum_speed: + self.release = 2.0 + if self.entry_sign == 0.0 and abs(desired - previous) > 2e-4: + self.entry_sign = math.copysign(1., desired - previous) + else: + self.release = max(0., self.release - DT_CTRL) + if self.release <= 0.: + self.entry_sign = 0. + factor = 1.0 + if self.release > 0.: + release = 1. - self.release / 2. + factor = base + (1. - base) * release + step = desired - previous + if self.entry_sign and abs(step) > 5e-5 and math.copysign(1., step) == -self.entry_sign: + pursuit = (max(abs(step) - 5e-5, 0.) / .2) * max(speed, 1.) ** 2 / MAX_LATERAL_JERK + factor = max(factor, min(.6 + .4 * release, factor + pursuit)) + if factor > self.previous_factor: + factor = self.previous_factor + (1. - math.exp(-DT_CTRL / .2)) * (factor - self.previous_factor) + self.previous_factor = factor + return max(0., min(1., factor)) + + +def limit_rate(speed, previous, desired, factor): + if factor >= 1.: + return desired + step = MAX_LATERAL_JERK / max(speed, MIN_SPEED) ** 2 * DT_CTRL * factor + return max(previous - step, min(previous + step, desired)) diff --git a/openpilot/starpilot/lateral/lane_change_status_wire.py b/openpilot/starpilot/lateral/lane_change_status_wire.py new file mode 100644 index 0000000000..be5505b8dd --- /dev/null +++ b/openpilot/starpilot/lateral/lane_change_status_wire.py @@ -0,0 +1,132 @@ +"""Bounded typed status for optional lane-change alert wording. + +This status is presentation evidence, never engagement authority. +""" + +from dataclasses import dataclass +from enum import IntEnum +import struct + +import capnp + +from openpilot.cereal import custom + + +SERVICE = "laneChangeAssistWire" +KIND = 3 +VERSION = 1 +MAX_BYTES = 512 +MAX_WORDS = (MAX_BYTES - 8) // 8 + + +class Phase(IntEnum): + MANUAL_REQUIRED = 0 + WAITING_FOR_DELAY = 1 + LANE_UNAVAILABLE = 2 + BLINDSPOT_BLOCKED = 3 + + +class Direction(IntEnum): + NONE = 0 + LEFT = 1 + RIGHT = 2 + + +@dataclass(frozen=True) +class LaneChangeStatus: + producer_session_id: str + sequence: int + model_frame_id: int + model_timestamp_eof_ns: int + observed_mono_time_ns: int + valid_until_mono_time_ns: int + phase: Phase + direction: Direction + auto_configured: bool + engaged: bool + + +def _valid(value: LaneChangeStatus) -> bool: + return (type(value.producer_session_id) is str and 1 <= len(value.producer_session_id.encode("utf-8")) <= 96 and + 0 <= value.sequence < 2**64 and 0 <= value.model_frame_id < 2**32 and + 0 < value.model_timestamp_eof_ns < 2**64 and + 0 < value.observed_mono_time_ns <= value.valid_until_mono_time_ns < 2**64 and + type(value.phase) is Phase and type(value.direction) is Direction and + type(value.auto_configured) is bool and type(value.engaged) is bool) + + +def encode(value: LaneChangeStatus) -> bytes: + if not _valid(value): + raise ValueError("invalid lane-change status") + msg = custom.AolAxisState.LaneChangeStatusWire.new_message( + kind=KIND, version=VERSION, producerSessionId=value.producer_session_id, + sequence=value.sequence, modelFrameId=value.model_frame_id, + modelTimestampEofNs=value.model_timestamp_eof_ns, + observedMonoTimeNs=value.observed_mono_time_ns, validUntilMonoTimeNs=value.valid_until_mono_time_ns, + phase=int(value.phase), direction=int(value.direction), + autoConfigured=value.auto_configured, engaged=value.engaged) + raw = msg.to_bytes() + if len(raw) > MAX_BYTES: + raise ValueError("lane-change status exceeds wire bound") + return raw + + +def encode_optional(value: LaneChangeStatus) -> bytes | None: + """Bad optional camera metadata must not interrupt model publication.""" + try: + return encode(value) + except (ValueError, OverflowError, RuntimeError, capnp.KjException): + return None + + +def decode(raw_value: bytes) -> LaneChangeStatus | None: + if not isinstance(raw_value, (bytes, bytearray, memoryview)): + return None + raw = bytes(raw_value) + if not 16 <= len(raw) <= MAX_BYTES or len(raw) % 8: + return None + try: + segments, words = struct.unpack_from(" MAX_WORDS or len(raw) != 8 + 8 * words: + return None + with custom.AolAxisState.LaneChangeStatusWire.from_bytes(raw, traversal_limit_in_words=MAX_WORDS, nesting_limit=4) as msg: + if msg.kind != KIND or msg.version != VERSION: + return None + result = LaneChangeStatus(str(msg.producerSessionId), int(msg.sequence), int(msg.modelFrameId), + int(msg.modelTimestampEofNs), int(msg.observedMonoTimeNs), + int(msg.validUntilMonoTimeNs), Phase(int(msg.phase.raw)), Direction(int(msg.direction.raw)), + bool(msg.autoConfigured), bool(msg.engaged)) + return result if _valid(result) else None + except (ValueError, OverflowError, RuntimeError, UnicodeDecodeError, capnp.KjException): + return None + + +def fresh_for_model(status: LaneChangeStatus | None, model, now_mono_ns: int, recv_mono_ns: int, + last_sequence: int = -1, last_session: str = "", message_mono_ns: int | None = None) -> bool: + if status is None or (status.phase != Phase.MANUAL_REQUIRED and not status.engaged) or not 0 <= now_mono_ns - status.observed_mono_time_ns <= 100_000_000: + return False + if not 0 <= now_mono_ns - recv_mono_ns <= 100_000_000 or now_mono_ns > status.valid_until_mono_time_ns: + return False + if message_mono_ns is not None and not 0 <= now_mono_ns - message_mono_ns <= 100_000_000: + return False + if status.producer_session_id == last_session and status.sequence < last_sequence: + return False + try: + return (int(model.frameId) == status.model_frame_id and + int(model.timestampEof) == status.model_timestamp_eof_ns and + ((status.direction == Direction.LEFT and str(model.meta.laneChangeDirection) == "left") or + (status.direction == Direction.RIGHT and str(model.meta.laneChangeDirection) == "right"))) + except (AttributeError, TypeError, ValueError): + return False + + +def alert_wording(status: LaneChangeStatus | None, model, now_mono_ns: int, recv_mono_ns: int, + last_sequence: int = -1, last_session: str = "", message_mono_ns: int | None = None) -> tuple[str, str] | None: + """None preserves stock manual copy only when a fresh status proves that path.""" + if not fresh_for_model(status, model, now_mono_ns, recv_mono_ns, last_sequence, last_session, message_mono_ns): + return "Lane Change Pending", "Check surroundings" + assert status is not None + return {Phase.MANUAL_REQUIRED: None, + Phase.WAITING_FOR_DELAY: ("Automatic Lane Change Pending", "Check surroundings; steer to confirm now"), + Phase.LANE_UNAVAILABLE: ("Automatic Change Unavailable", "Steer to confirm when safe"), + Phase.BLINDSPOT_BLOCKED: ("Car Detected in Blindspot", "")}[status.phase] diff --git a/openpilot/starpilot/lateral/lane_feedback.py b/openpilot/starpilot/lateral/lane_feedback.py new file mode 100644 index 0000000000..f0aef1f836 --- /dev/null +++ b/openpilot/starpilot/lateral/lane_feedback.py @@ -0,0 +1,50 @@ +"""Display the lane contribution that survives the host curvature limiter.""" + +import math + +from openpilot.cereal import messaging +from openpilot.starpilot.lateral.lane_centering import MAX_CORRECTION + +SERVICE = 'starpilotLateralState' +MAX_AGE_NS = 250_000_000 +MAX_PAIR_SKEW_NS = 100_000_000 +DIRECTION_EPSILON = 1e-6 +BLUE = (0, 176, 220) + + +def feedback_message(result, applied, lateral_active, *, model_mono_time, car_control_mono_time, valid): + event = messaging.new_message(SERVICE, valid=valid) + state = event.starpilotLateralState.laneCentering + state.version = 1 + state.modelMonoTime = model_mono_time + state.carControlMonoTime = car_control_mono_time + state.lateralActive = lateral_active + state.requestedCorrection = result.correction if result is not None else 0.0 + state.appliedCorrection = applied if lateral_active else 0.0 + state.reason = result.reason if result is not None else 'inactive' + return event + + +def direction(sm, now_ns: int, after_frame: int) -> int: + try: + clocks = {} + for service in (SERVICE, 'carControl', 'modelV2'): + stamp = int(sm.logMonoTime[service]) + if (not sm.alive[service] or not sm.valid[service] or sm.recv_frame[service] <= after_frame or + not 0 < stamp <= now_ns or now_ns - stamp > MAX_AGE_NS): + return 0 + clocks[service] = stamp + state = sm[SERVICE].laneCentering + requested, applied = float(state.requestedCorrection), float(state.appliedCorrection) + if (state.version != 1 or not state.lateralActive or not sm['carControl'].latActive or + not 0 < state.modelMonoTime <= now_ns or now_ns - state.modelMonoTime > MAX_AGE_NS or + not 0 < state.carControlMonoTime <= clocks[SERVICE] or + abs(state.carControlMonoTime - clocks['carControl']) > MAX_PAIR_SKEW_NS or + abs(state.modelMonoTime - clocks['modelV2']) > MAX_PAIR_SKEW_NS or + not math.isfinite(requested) or not math.isfinite(applied) or + abs(requested) > MAX_CORRECTION + 1e-8 or abs(applied) > abs(requested) + 1e-8 or + requested * applied < 0): + return 0 + return (applied > DIRECTION_EPSILON) - (applied < -DIRECTION_EPSILON) + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return 0 diff --git a/openpilot/starpilot/lateral/lane_runtime.py b/openpilot/starpilot/lateral/lane_runtime.py new file mode 100644 index 0000000000..776b199e08 --- /dev/null +++ b/openpilot/starpilot/lateral/lane_runtime.py @@ -0,0 +1,85 @@ +"""Saved lane-centering preferences and fresh host authority, opt-in at startup.""" + +import math +import os +from pathlib import Path + +from openpilot.cereal.services import SERVICE_LIST +from openpilot.common.realtime import DT_CTRL +from openpilot.common.params import UnknownKeyName +from openpilot.starpilot.lateral.lane_centering import ControlMode, LaneCenteringRequest, LaneCenteringSettings, STRENGTH_BASELINE, STRENGTH_MAX +from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_long_eligible + + +def read_settings(params) -> LaneCenteringSettings: + def raw(key, default): + try: + return Path(params.get_param_path(key)).read_bytes() + except FileNotFoundError: + return default + + def boolean(key, default): + value = raw(key, default) + if value not in (b'0', b'1'): + raise ValueError('Invalid lane preference') + return value == b'1' + + try: + enabled = boolean('LaneCentering', b'0') + pause = boolean('LaneCenteringPauseOnSignal', b'1') + offset = float(raw('LaneCenterOffset', b'0.0')) + authority = float(raw('LaneCenteringE2EAuthority', b'1.0')) + try: + strength = float(raw('LaneCenteringStrength', str(STRENGTH_BASELINE).encode())) + except UnknownKeyName: + strength = STRENGTH_BASELINE + if not math.isfinite(strength) or not 0.5 <= strength <= STRENGTH_MAX: + raise ValueError('Invalid lane strength') + if not math.isfinite(offset) or not -0.3 <= offset <= 0.3 or not math.isfinite(authority) or not 0 <= authority <= 1: + raise ValueError('Invalid lane tuning') + return LaneCenteringSettings(enabled, offset, authority, pause, strength) + except (OSError, ValueError): + return LaneCenteringSettings() + + +def runtime_supported(CP) -> bool: + """Keep the preference reader available when a supported drive starts Off.""" + from openpilot.starpilot.lateral.bolt_policy import supported_cp as bolt_supported_cp + from opendbc.car.gm.lateral import lane_centering_supported as gm_supported_cp + return (os.getenv('LANE_CENTERING_REPLAY_RUNTIME') == '1' or + ioniq6_long_eligible(CP) or (CP is not None and bolt_supported_cp(CP)) or gm_supported_cp(CP)) + + +class LaneCenteringHost: + REFRESH_NS = 1_000_000_000 + MAX_TICK_NS = 50_000_000 + REQUIRED = ('carState', 'selfdriveState', 'modelV2', 'vehicleParameters') + + def __init__(self, params): + self.params = params + self.settings = LaneCenteringSettings() + self.last_refresh_ns: int | None = None + self.last_tick_ns: int | None = None + + def sample(self, sm, *, now_ns: int, lateral_active: bool, longitudinal_active: bool) -> LaneCenteringRequest | None: + previous = self.last_tick_ns + self.last_tick_ns = now_ns + discontinuity = previous is not None and not 0 < now_ns - previous <= self.MAX_TICK_NS + if now_ns <= 0: + self.settings = LaneCenteringSettings() + self.last_refresh_ns = None + return None + if self.last_refresh_ns is None or now_ns < self.last_refresh_ns or now_ns - self.last_refresh_ns >= self.REFRESH_NS: + self.settings = read_settings(self.params) + self.last_refresh_ns = now_ns + if not self.settings.enabled or not sm['carState'].canValid or sm['carState'].canTimeout: + return None + if not sm.all_checks(list(self.REQUIRED)) or any( + int(sm.logMonoTime[name]) <= 0 or not 0 <= now_ns - int(sm.logMonoTime[name]) <= int(2.5e9 / SERVICE_LIST[name].frequency) + for name in self.REQUIRED + ): + return None + mode = (ControlMode.COMBINED if longitudinal_active else ControlMode.LATERAL_ONLY) if lateral_active else ( + ControlMode.LONGITUDINAL_ONLY if longitudinal_active else ControlMode.OFF) + dt = DT_CTRL if previous is None or discontinuity else (now_ns - previous) / 1e9 + return LaneCenteringRequest(mode, self.settings, dt, discontinuity) diff --git a/openpilot/starpilot/lateral/learning_preferences.py b/openpilot/starpilot/lateral/learning_preferences.py new file mode 100644 index 0000000000..3b98708156 --- /dev/null +++ b/openpilot/starpilot/lateral/learning_preferences.py @@ -0,0 +1,88 @@ +"""Vehicle-scoped user choice, independent of learning compatibility and results.""" + +from dataclasses import dataclass +import json + +from openpilot.starpilot.saved_source import read_saved + +KEY = 'SteeringLearningPreferences' +MAX_BYTES = 4096 +LEGACY_KEY = 'ForceAutoTuneOff' + + +@dataclass(frozen=True) +class LearningPreference: + enabled: bool + valid: bool + source: str + raw: bytes | None + + +def _identity(CP): + brand, fingerprint = str(CP.brand), str(CP.carFingerprint) + if not brand or not fingerprint or len(brand) > 64 or len(fingerprint) > 256: + raise ValueError('Invalid vehicle identity') + return brand, fingerprint + + +def _unique(pairs): + result = {} + for key, value in pairs: + if key in result: + raise ValueError('Duplicate field') + result[key] = value + return result + + +def _decode(raw): + if raw is None: + return {'version': 1, 'vehicles': {}} + if not raw or len(raw) > MAX_BYTES: + raise ValueError('Invalid preference size') + try: + document = json.loads(raw, object_pairs_hook=_unique) + except RecursionError as error: + raise ValueError('Preference nesting exceeds parser limits') from error + if (not isinstance(document, dict) or set(document) != {'version', 'vehicles'} or + type(document['version']) is not int or document['version'] != 1 or + not isinstance(document['vehicles'], dict)): + raise ValueError('Invalid preference document') + count = 0 + for brand, vehicles in document['vehicles'].items(): + if not brand or len(brand) > 64 or not isinstance(vehicles, dict): + raise ValueError('Invalid preference brand') + for fingerprint, enabled in vehicles.items(): + if not fingerprint or len(fingerprint) > 256 or type(enabled) is not bool: + raise ValueError('Invalid vehicle preference') + count += 1 + if count > 64: + raise ValueError('Too many vehicle preferences') + return document + + +def read_preference(params, CP, *, legacy_snapshot=None) -> LearningPreference: + raw, readable = read_saved(params, KEY, MAX_BYTES) + try: + if not readable: + raise ValueError('Unreadable preferences') + brand, fingerprint = _identity(CP) + vehicles = _decode(raw)['vehicles'] + if fingerprint in vehicles.get(brand, {}): + return LearningPreference(vehicles[brand][fingerprint], True, 'scope', raw) + legacy, readable = read_saved(params, LEGACY_KEY, 1) if legacy_snapshot is None else legacy_snapshot + if not readable or legacy not in (None, b'0', b'1'): + raise ValueError('Invalid legacy preference') + return LearningPreference(legacy != b'1', True, 'default' if legacy is None else 'legacy', raw) + except (AttributeError, TypeError, ValueError, UnicodeError): + return LearningPreference(False, False, 'invalid', raw) + + +def replace_preference(raw: bytes | None, CP, enabled: bool) -> bytes: + if type(enabled) is not bool: + raise ValueError('Invalid preference') + brand, fingerprint = _identity(CP) + document = _decode(raw) + document['vehicles'].setdefault(brand, {})[fingerprint] = enabled + encoded = json.dumps(document, sort_keys=True, separators=(',', ':'), allow_nan=False).encode() + _decode(encoded) + return encoded diff --git a/openpilot/starpilot/lateral/low_speed_advisory.py b/openpilot/starpilot/lateral/low_speed_advisory.py new file mode 100644 index 0000000000..c2b9963fc5 --- /dev/null +++ b/openpilot/starpilot/lateral/low_speed_advisory.py @@ -0,0 +1,29 @@ +"""The first below-minimum-steering-speed episode in an onroad session.""" + +import math + + +class LowSpeedAdvisory: + def __init__(self): + self.drive_id = 0 + self.has_been_above = False + self.shown = False + self.showing = False + + def update(self, speed: float, minimum: float, *, drive_id: int = 0) -> bool: + # Unknown/transient device state does not restart the advisory lifecycle. + if drive_id > 0 and drive_id != self.drive_id: + self.drive_id = drive_id + self.has_been_above = False + self.shown = False + self.showing = False + if not math.isfinite(speed) or not math.isfinite(minimum) or minimum <= 0: + return False + under = speed < minimum + if not under: + self.has_been_above = True + self.showing = False + elif self.has_been_above and not self.shown: + self.shown = True + self.showing = True + return self.showing and under diff --git a/openpilot/starpilot/lateral/model_turn_assist.py b/openpilot/starpilot/lateral/model_turn_assist.py new file mode 100644 index 0000000000..57c8af5446 --- /dev/null +++ b/openpilot/starpilot/lateral/model_turn_assist.py @@ -0,0 +1,259 @@ +"""Rolling model turn encouragement and curvature handoff for Ioniq 6.""" +import math +from openpilot.common.realtime import DT_CTRL +from openpilot.common.constants import CV + +CURVATURE_HOLD_HARD_SPEED = 4.5 * CV.MPH_TO_MS +CURVATURE_HOLD_RELEASE_SPEED = 10.0 * CV.MPH_TO_MS +CURVATURE_HOLD_PLAN_SOURCE_SPEED = 2.0 * CV.MPH_TO_MS +CURVATURE_HOLD_RATCHET_RATE = 0.04 +CURVATURE_HOLD_HANDOFF_FRAC = 0.75 +CURVATURE_HOLD_HANDOFF_TIME = 0.3 +CURVATURE_HOLD_DECAY_TAU = 2.0 +CURVATURE_HOLD_SWEPT_EXIT = 0.9 +CURVATURE_HOLD_EXIT_DECAY_TAU = 0.5 +CURVATURE_HOLD_STANDSTILL_TIMEOUT = 30.0 +CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR = 4.0 +CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR = 7.0 +CURVATURE_HOLD_PLAN_SCALE = 0.85 +CURVATURE_HOLD_PLAN_CAP = 0.12 +CURVATURE_HOLD_ONSET_HEADING = 10.0 +CURVATURE_HOLD_ONSET_NEAR = 1.5 +CURVATURE_HOLD_ONSET_FAR_GATE = 5.0 +CURVATURE_HOLD_ONSET_FAR = 100.0 +CURVATURE_HOLD_REACH_MIN = 7.0 +CURVATURE_HOLD_REACH_FULL = 12.0 +CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01 +CURVATURE_HOLD_CONFIRM_MIN = 0.003 +CURVATURE_HOLD_CONFIRM_SWEPT = 0.6 +TWITCH_GUARD_MAX_SPEED = 4.0 +TWITCH_GUARD_FADE_SPEED = 3.0 +TWITCH_GUARD_DURATION = 1.5 +TWITCH_GUARD_PLAN_RATIO = 4.0 +TWITCH_GUARD_FLOOR = 0.002 +TWITCH_GUARD_STRAIGHT_LO = 0.005 +TWITCH_GUARD_STRAIGHT_HI = 0.014 +TWITCH_GUARD_MIN_REACH = 12.0 + + +def _plan_circle_curvature(xs, ys, lookahead: float) -> float: + px, py = (0.0, 0.0) + for x, y in zip(xs, ys, strict=False): + try: + x, y = (float(x), float(y)) + except (TypeError, ValueError, OverflowError): + return 0.0 + if not (math.isfinite(x) and math.isfinite(y)): + return 0.0 + px, py = (x, y) + if math.hypot(x, y) >= lookahead: + break + d2 = px * px + py * py + if d2 < 1.0: + return 0.0 + return 2.0 * py / d2 + + +def _plan_dual_probe(model_v2, d_near: float, d_far: float) -> float: + xs, ys = (model_v2.position.x, model_v2.position.y) + near = _plan_circle_curvature(xs, ys, d_near) + far = _plan_circle_curvature(xs, ys, d_far) + if near * far <= 0.0: + return 0.0 + return near if abs(near) < abs(far) else far + + +def get_plan_spatial_curvature(model_v2) -> float: + return _plan_dual_probe(model_v2, CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR, CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR) + + +def get_plan_turn_onset_dist(model_v2) -> float: + xs, ys = (model_v2.position.x, model_v2.position.y) + n = min(len(xs), len(ys)) + for i in range(2, n): + dx = xs[i] - xs[i - 1] + dy = ys[i] - ys[i - 1] + if abs(dx) < 0.001 and abs(dy) < 0.001: + continue + if abs(math.degrees(math.atan2(dy, dx))) > CURVATURE_HOLD_ONSET_HEADING: + return math.hypot(xs[i], ys[i]) + return CURVATURE_HOLD_ONSET_FAR + + +def get_plan_reach(model_v2) -> float: + try: + xs = model_v2.position.x + return float(xs[-1]) if len(xs) else 0.0 + except (AttributeError, IndexError, TypeError, ValueError, OverflowError): + return 0.0 + + +def _plan_positions_are_finite(model_v2) -> bool: + try: + xs, ys = (model_v2.position.x, model_v2.position.y) + return len(xs) == len(ys) and all((math.isfinite(float(x)) and math.isfinite(float(y)) for x, y in zip(xs, ys, strict=True))) + except (AttributeError, TypeError, ValueError, OverflowError): + return False + + +def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float: + if not (math.isfinite(curvature) and math.isfinite(v_ego)): + return curvature + if v_ego >= TWITCH_GUARD_MAX_SPEED or curvature == 0.0: + return curvature + if not _plan_positions_are_finite(model_v2): + return curvature + reach = get_plan_reach(model_v2) + if not math.isfinite(reach) or reach < TWITCH_GUARD_MIN_REACH: + return curvature + plan = abs(_plan_circle_curvature(model_v2.position.x, model_v2.position.y, CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR)) + if not math.isfinite(plan): + return curvature + straightness = (plan - TWITCH_GUARD_STRAIGHT_LO) / (TWITCH_GUARD_STRAIGHT_HI - TWITCH_GUARD_STRAIGHT_LO) + limit = max(TWITCH_GUARD_PLAN_RATIO * plan * min(max(straightness, 0.0), 1.0), TWITCH_GUARD_FLOOR) + if abs(curvature) <= limit: + return curvature + fade = (TWITCH_GUARD_MAX_SPEED - v_ego) / (TWITCH_GUARD_MAX_SPEED - TWITCH_GUARD_FADE_SPEED) + fade = min(max(fade, 0.0), 1.0) + return curvature + (math.copysign(limit, curvature) - curvature) * fade + + +def update_twitch_guard(remaining: float, v_ego: float, standstill: bool) -> float: + if not (math.isfinite(remaining) and math.isfinite(v_ego)): + return 0.0 + if standstill or abs(v_ego) <= 0.3: + return TWITCH_GUARD_DURATION + return max(remaining - DT_CTRL, 0.0) +TURN_LEAD_T = 1.3 +TURN_LEAD_MIN_M = 4.0 +TURN_LEAD_MAX_M = 14.0 +TURN_LEAD_MIN_SPEED = 3.0 +TURN_LEAD_FULL_SPEED = 4.0 +TURN_LEAD_MAX_SPEED = 7.0 +TURN_LEAD_SCALE = 0.85 +TURN_LEAD_CAP = 0.12 +TURN_LEAD_ENGAGED_FRAC = 0.5 +TURN_LEAD_MODEL_OPPOSE = 0.003 +TURN_LEAD_STOP_MARGIN = 1.5 +TURN_LEAD_DECEL_GATE = -0.5 + + +class ModelTurnAssist: + + def __init__(self): + self.reset() + + def reset(self): + self.turn_hold_curvature = 0.0 + self.turn_hold_standstill_t = 0.0 + self.turn_hold_swept = 0.0 + self.turn_hold_handoff_t = 0.0 + self.turn_blinker_swept = 0.0 + self.twitch_guard_remaining = 0.0 + self.curvature = 0.0 + self.turn_hold_done = False + + def update(self, CS, model_v2, CC, new_desired_curvature): + blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker) + if CC.latActive and self.twitch_guard_remaining > 0.0 and (blinker_dir == 0.0) and (self.turn_hold_curvature == 0.0): + new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo) + if blinker_dir == 0.0: + self.turn_blinker_swept = 0.0 + else: + self.turn_blinker_swept += max(CS.vEgo * self.curvature * blinker_dir, 0.0) * DT_CTRL + if CS.vEgo >= CURVATURE_HOLD_RELEASE_SPEED: + self.turn_hold_curvature = 0.0 + self.turn_hold_standstill_t = 0.0 + self.turn_hold_swept = 0.0 + self.turn_hold_handoff_t = 0.0 + self.turn_hold_done = False + else: + if self.turn_hold_curvature == 0.0: + self.turn_hold_swept = 0.0 + else: + self.turn_hold_swept += max(CS.vEgo * self.curvature * math.copysign(1.0, self.turn_hold_curvature), 0.0) * DT_CTRL + turn_exiting = self.turn_hold_swept > CURVATURE_HOLD_SWEPT_EXIT + if (CS.vEgo > CURVATURE_HOLD_HARD_SPEED or turn_exiting) and CC.latActive and (self.turn_hold_curvature != 0.0): + hold_dir = math.copysign(1.0, self.turn_hold_curvature) + model_mag = max(new_desired_curvature * hold_dir, 0.0) + if model_mag < abs(self.turn_hold_curvature): + decay_tau = CURVATURE_HOLD_EXIT_DECAY_TAU if turn_exiting else CURVATURE_HOLD_DECAY_TAU + decayed = abs(self.turn_hold_curvature) + (model_mag - abs(self.turn_hold_curvature)) * (DT_CTRL / decay_tau) + self.turn_hold_curvature = math.copysign(decayed, self.turn_hold_curvature) + if CS.vEgo < 0.5: + self.turn_hold_standstill_t += DT_CTRL + if self.turn_hold_standstill_t > CURVATURE_HOLD_STANDSTILL_TIMEOUT: + self.turn_hold_curvature = 0.0 + self.turn_hold_done = False + else: + self.turn_hold_standstill_t = 0.0 + if CC.latActive and self.turn_hold_curvature != 0.0 and ( + new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) < -CURVATURE_HOLD_OPPOSITE_RELEASE): + self.turn_hold_curvature = 0.0 + self.turn_hold_done = True + if CC.latActive and self.turn_hold_curvature != 0.0 and ( + new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) >= CURVATURE_HOLD_HANDOFF_FRAC * abs(self.turn_hold_curvature)): + self.turn_hold_handoff_t += DT_CTRL + if self.turn_hold_handoff_t > CURVATURE_HOLD_HANDOFF_TIME: + self.turn_hold_curvature = 0.0 + self.turn_hold_done = True + else: + self.turn_hold_handoff_t = 0.0 + if blinker_dir == 0.0: + self.turn_hold_done = False + elif (CC.latActive and CS.steeringPressed and (CS.steeringTorque * blinker_dir < 0.0) and + (self.curvature * blinker_dir > CURVATURE_HOLD_CONFIRM_MIN) and (self.turn_blinker_swept < CURVATURE_HOLD_CONFIRM_SWEPT)): + self.turn_hold_done = False + if blinker_dir != 0.0 and (not self.turn_hold_done): + turn_candidate = new_desired_curvature if CC.latActive else 0.0 + if CC.latActive and CS.vEgo < CURVATURE_HOLD_PLAN_SOURCE_SPEED: + plan_curvature = get_plan_spatial_curvature(model_v2) * CURVATURE_HOLD_PLAN_SCALE + plan_curvature = max(min(plan_curvature, CURVATURE_HOLD_PLAN_CAP), -CURVATURE_HOLD_PLAN_CAP) + onset = get_plan_turn_onset_dist(model_v2) + onset_w = min(max((CURVATURE_HOLD_ONSET_FAR_GATE - onset) / (CURVATURE_HOLD_ONSET_FAR_GATE - CURVATURE_HOLD_ONSET_NEAR), 0.0), 1.0) + reach = get_plan_reach(model_v2) + reach_w = min(max((reach - CURVATURE_HOLD_REACH_MIN) / (CURVATURE_HOLD_REACH_FULL - CURVATURE_HOLD_REACH_MIN), 0.0), 1.0) + plan_curvature *= onset_w * reach_w + if plan_curvature * blinker_dir > turn_candidate * blinker_dir: + turn_candidate = plan_curvature + driver_confirmed = False + if CC.latActive and CS.steeringPressed and (CS.steeringTorque * blinker_dir < 0.0) and (self.curvature * blinker_dir > CURVATURE_HOLD_CONFIRM_MIN): + wound_curvature = max(min(self.curvature, CURVATURE_HOLD_PLAN_CAP), -CURVATURE_HOLD_PLAN_CAP) + if wound_curvature * blinker_dir > turn_candidate * blinker_dir: + turn_candidate = wound_curvature + driver_confirmed = True + if turn_candidate * blinker_dir > abs(self.turn_hold_curvature): + new_mag = turn_candidate * blinker_dir + if CS.vEgo > CURVATURE_HOLD_PLAN_SOURCE_SPEED and (not driver_confirmed): + new_mag = min(new_mag, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL) + self.turn_hold_curvature = math.copysign(new_mag, turn_candidate) + elif self.turn_hold_curvature * blinker_dir < 0.0: + self.turn_hold_curvature = 0.0 + if CC.latActive and self.turn_hold_curvature != 0.0: + hold_dir = math.copysign(1.0, self.turn_hold_curvature) + if new_desired_curvature * hold_dir < abs(self.turn_hold_curvature): + new_desired_curvature = self.turn_hold_curvature + if (CC.latActive and (blinker_dir != 0.0) and (model_v2.meta.laneChangeState == 0) and + (TURN_LEAD_MIN_SPEED <= CS.vEgo < TURN_LEAD_MAX_SPEED) and (new_desired_curvature * blinker_dir > -TURN_LEAD_MODEL_OPPOSE)): + d_near = max(min(TURN_LEAD_T * CS.vEgo, TURN_LEAD_MAX_M), TURN_LEAD_MIN_M) + stopping_short = CS.aEgo < TURN_LEAD_DECEL_GATE and CS.vEgo ** 2 / (2.0 * -CS.aEgo) < TURN_LEAD_STOP_MARGIN * d_near + lead_curvature = 0.0 if stopping_short else _plan_dual_probe(model_v2, d_near, d_near + 3.0) * TURN_LEAD_SCALE + lead_curvature = max(min(lead_curvature, TURN_LEAD_CAP), -TURN_LEAD_CAP) + if lead_curvature * blinker_dir > 0.0: + speed_w = min(max((CS.vEgo - TURN_LEAD_MIN_SPEED) / (TURN_LEAD_FULL_SPEED - TURN_LEAD_MIN_SPEED), 0.0), 1.0) + engaged_ratio = abs(self.curvature) / abs(lead_curvature) + engage_w = min(max((1.0 - engaged_ratio) / (1.0 - TURN_LEAD_ENGAGED_FRAC), 0.0), 1.0) + lead_curvature *= speed_w * engage_w + if lead_curvature * blinker_dir > max(new_desired_curvature * blinker_dir, 0.0): + new_desired_curvature = lead_curvature + if CS.vEgo < CURVATURE_HOLD_RELEASE_SPEED and (not self.turn_hold_done) and (lead_curvature * blinker_dir > abs(self.turn_hold_curvature)): + held_mag = min(lead_curvature * blinker_dir, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL) + self.turn_hold_curvature = math.copysign(held_mag, lead_curvature) + return new_desired_curvature + + +def bind_turn_assist(controller): + """Bind policy ownership once while observing later policy replacements.""" + if hasattr(controller, "starpilot_extension"): + return lambda: bool(getattr(getattr(getattr(controller, "starpilot_extension", None), "policy", None), "turn_assist", False)) + return lambda: bool(getattr(getattr(controller, "ioniq6_policy", None), "turn_assist", False)) diff --git a/openpilot/starpilot/lateral/ordinary_cc_policy.py b/openpilot/starpilot/lateral/ordinary_cc_policy.py new file mode 100644 index 0000000000..eef1fcc18a --- /dev/null +++ b/openpilot/starpilot/lateral/ordinary_cc_policy.py @@ -0,0 +1,36 @@ +"""Default torque law for exact conventional-cruise GM owners.""" + +import math +import numpy as np + +from opendbc.car.gm.values import CAR, is_ordinary_cc_profile, is_conventional_cc_pedal_profile, malibu_hybrid_profile +from opendbc.car.structs import CarParams +from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy + + +def supported_cp(cp): + if (not (is_ordinary_cc_profile(cp) or is_conventional_cc_pedal_profile(cp) or malibu_hybrid_profile(cp) is not None) or + cp.steerControlType != CarParams.SteerControlType.torque or cp.lateralTuning.which() != 'torque'): + return False + tune = cp.lateralTuning.torque + return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and + math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0) + + +class OrdinaryCcTorquePolicy(GMOrdinaryTorquePolicy): + def __init__(self, parent, cp): + if not supported_cp(cp): + raise ValueError('Unsupported conventional-cruise GM torque profile') + self.yukon = cp.carFingerprint == CAR.GMC_YUKON_CC + super().__init__(parent, cp) + + def feedforward(self, value, setpoint, jerk, speed): + if not self.yukon: + return value + phase = math.tanh((setpoint * jerk) / .14) + speed_weight = float(np.interp(speed, [12.0, 30.0], [0.0, 1.0])) + x = (abs(setpoint) - .35) / .18 + z = math.exp(-x) if x >= 0.0 else math.exp(x) + lat_weight = 1.0 / (1.0 + z) if x >= 0.0 else z / (1.0 + z) + scale = 1.0 + ((.08 * max(phase, 0.0) - .12 * max(-phase, 0.0)) * speed_weight * lat_weight) + return value * scale diff --git a/openpilot/starpilot/lateral/palisade_policy.py b/openpilot/starpilot/lateral/palisade_policy.py new file mode 100644 index 0000000000..5ab183f622 --- /dev/null +++ b/openpilot/starpilot/lateral/palisade_policy.py @@ -0,0 +1,26 @@ +from opendbc.car.hyundai.values import CAR +from openpilot.starpilot.lateral import palisade_shaping as shaping +from openpilot.starpilot.lateral.hkg_torque_policy import HKGShapedTorquePolicy, supported_torque_cp + + +PALISADE_CARS = frozenset((CAR.HYUNDAI_PALISADE, CAR.HYUNDAI_PALISADE_2023,)) + + +def supported_cp(cp): + return supported_torque_cp(cp, PALISADE_CARS) + + +class PalisadeTorquePolicy(HKGShapedTorquePolicy): + FACTOR_MULT = shaping.PALISADE_BASE_LAT_ACCEL_FACTOR_MULT + supported_cp = staticmethod(supported_cp) + + def feedforward(self, value, setpoint, jerk, speed): + return value * shaping.get_palisade_ff_scale(setpoint, jerk, speed) * shaping.get_palisade_center_taper_scale(setpoint, speed) + + def friction(self, setpoint, jerk, speed): + taper = shaping.get_palisade_center_taper_scale(setpoint, speed) + return (shaping.get_palisade_friction_threshold(speed, setpoint, jerk), + 1.0 + (shaping.get_palisade_friction_scale(speed, setpoint, jerk) - 1.0) * taper) + + def output(self, value, setpoint, speed): + return value * shaping.get_palisade_center_output_scale(setpoint, speed) diff --git a/openpilot/starpilot/lateral/palisade_shaping.py b/openpilot/starpilot/lateral/palisade_shaping.py new file mode 100644 index 0000000000..2525b5d2cc --- /dev/null +++ b/openpilot/starpilot/lateral/palisade_shaping.py @@ -0,0 +1,154 @@ +import math + +from openpilot.starpilot.lateral.bolt_shaping import get_gm_base_friction_threshold + + +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.44 + +PALISADE_TURN_IN_BOOST_RIGHT = 0.34 + +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 + +PALISADE_CENTER_TAPER_MAX = 0.12 + +PALISADE_CENTER_TAPER_LAT = 0.28 + +PALISADE_CENTER_TAPER_LAT_WIDTH = 0.055 + +PALISADE_CENTER_TAPER_SPEED = 12.0 + +PALISADE_CENTER_TAPER_SPEED_WIDTH = 2.5 + +PALISADE_CENTER_OUTPUT_TAPER_MAX = 0.18 + +PALISADE_CENTER_OUTPUT_TAPER_LAT = 0.28 + +PALISADE_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.055 + +PALISADE_CENTER_OUTPUT_TAPER_SPEED = 15.0 + +PALISADE_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 3.0 + +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 _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_gm_base_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 get_palisade_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _palisade_sigmoid((v_ego - PALISADE_CENTER_TAPER_SPEED) / PALISADE_CENTER_TAPER_SPEED_WIDTH) + center_weight = _palisade_sigmoid((PALISADE_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + PALISADE_CENTER_TAPER_LAT_WIDTH) + return 1.0 - (PALISADE_CENTER_TAPER_MAX * speed_weight * center_weight) + +def get_palisade_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + """Reduce high-speed center corrections without reducing normal turn authority.""" + speed_weight = _palisade_sigmoid((v_ego - PALISADE_CENTER_OUTPUT_TAPER_SPEED) / + PALISADE_CENTER_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _palisade_sigmoid((PALISADE_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + PALISADE_CENTER_OUTPUT_TAPER_LAT_WIDTH) + return 1.0 - (PALISADE_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight) diff --git a/openpilot/starpilot/lateral/pause.py b/openpilot/starpilot/lateral/pause.py new file mode 100644 index 0000000000..ef25bc8c26 --- /dev/null +++ b/openpilot/starpilot/lateral/pause.py @@ -0,0 +1,90 @@ +"""Temporary steering output pauses, independent of driver authorization.""" + +from dataclasses import dataclass +import math + +from openpilot.common.constants import CV +from openpilot.starpilot.saved_source import read_saved + +KEYS = ('PauseLateralSpeed', 'PauseLateralOnSignal', 'LateralResumeDelay') + + +@dataclass(frozen=True) +class PauseSettings: + speed_mps: float = 0.0 + signal_only: bool = False + delay_s: float = 0.0 + + +def read_settings(params) -> PauseSettings | None: + values = {key: read_saved(params, key, 128) for key in (*KEYS, 'IsMetric')} + if not all(readable for _, readable in values.values()): + return None + try: + speed_raw = values[KEYS[0]][0] + speed = float(b'0' if speed_raw is None else speed_raw) + signal = values[KEYS[1]][0] + delay_raw = values[KEYS[2]][0] + delay = float(b'0' if delay_raw is None else delay_raw) + metric = values['IsMetric'][0] + if (signal not in (None, b'0', b'1') or metric not in (None, b'0', b'1') or + not math.isfinite(speed) or not 0 <= speed <= 100 or not math.isfinite(delay) or not 0 <= delay <= 5): + return None + return PauseSettings(speed * (CV.KPH_TO_MS if metric == b'1' else CV.MPH_TO_MS), signal == b'1', delay) + except (ValueError, OverflowError): + return None + + +class LateralPause: + def __init__(self, settings: PauseSettings | None, params=None): + self.settings = settings or PauseSettings() + self.params = params + self._refresh_ns = 0 + self._drive_id = 0 + self.reset() + + def reset(self) -> None: + self._blinker = False + self._minimum = math.inf + self._release_ns = 0 + self._last_ns = 0 + + def refresh(self, now_ns: int) -> None: + if self.params is not None and (now_ns < self._refresh_ns or now_ns - self._refresh_ns >= 1_000_000_000): + settings = read_settings(self.params) + if settings is not None: + if settings != self.settings: + self.reset() + self.settings = settings + self._refresh_ns = now_ns + + def allowed(self, cs, *, now_ns: int, source_ns: int, drive_id: int | None = None) -> bool: + self.refresh(now_ns) + if self.settings.speed_mps == 0: + self.reset() + return True + if now_ns < self._last_ns: + self.reset() + return False + self._last_ns = now_ns + if drive_id is not None: + if drive_id <= 0: + return False + if drive_id != self._drive_id: + self.reset() + self._drive_id = drive_id + self._last_ns = now_ns + if (not cs.canValid or cs.canTimeout or not math.isfinite(cs.vEgo) or + not 0 < source_ns <= now_ns or now_ns - source_ns > 250_000_000): + return False + signal = bool(cs.leftBlinker or cs.rightBlinker) + if signal: + self._minimum = min(self._minimum, cs.vEgo) if self._blinker else cs.vEgo + self._release_ns = 0 + elif self._blinker: + self._release_ns = now_ns + self._blinker = signal + delayed = (self.settings.signal_only and self._release_ns > 0 and + self._minimum < self.settings.speed_mps / 2 and + now_ns - self._release_ns < self.settings.delay_s * 1e9) + return bool((cs.vEgo >= self.settings.speed_mps or self.settings.signal_only and not signal) and not delayed) diff --git a/openpilot/starpilot/lateral/sdgm_policy.py b/openpilot/starpilot/lateral/sdgm_policy.py new file mode 100644 index 0000000000..9488779d57 --- /dev/null +++ b/openpilot/starpilot/lateral/sdgm_policy.py @@ -0,0 +1,34 @@ +"""Ordinary SDGM torque selection; factory PID identity stays on PID.""" + +import math +import numpy as np + +from opendbc.car.structs import CarParams +from opendbc.car.gm.values import CAR, is_ordinary_sdgm_profile +from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy + + +def supported_cp(cp): + if not (is_ordinary_sdgm_profile(cp, longitudinal=False) or is_ordinary_sdgm_profile(cp, longitudinal=True)): + return False + if cp.steerControlType != CarParams.SteerControlType.torque or cp.lateralTuning.which() != "torque": + return False + tune = cp.lateralTuning.torque + return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and + math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0) + + +class SdgmTorquePolicy(GMOrdinaryTorquePolicy): + def __init__(self, parent, cp): + if not supported_cp(cp): + raise ValueError("Unsupported ordinary SDGM torque profile") + if cp.carFingerprint == CAR.CADILLAC_XT4: + lateral_values = np.arange(-5.0, 5.0, 0.01) + torque_values = [] + for value in lateral_values: + sig_input = 2.4 * value + sigmoid = np.sign(sig_input) * (1 / (1 + math.exp(-abs(sig_input))) - 0.5) + torque_values.append(float(sigmoid * .95 + value * .28)) + parent.torque_from_lateral_accel = lambda value, tune: np.interp(value, lateral_values, torque_values) + parent.lateral_accel_from_torque = lambda value, tune: np.interp(value, torque_values, lateral_values) + super().__init__(parent, cp) diff --git a/openpilot/starpilot/lateral/sonata_hybrid_policy.py b/openpilot/starpilot/lateral/sonata_hybrid_policy.py new file mode 100644 index 0000000000..ed7b9b7ba6 --- /dev/null +++ b/openpilot/starpilot/lateral/sonata_hybrid_policy.py @@ -0,0 +1,24 @@ +from opendbc.car.hyundai.values import CAR +from openpilot.starpilot.lateral import sonata_hybrid_shaping as shaping +from openpilot.starpilot.lateral.hkg_torque_policy import HKGShapedTorquePolicy, supported_torque_cp + + +SONATA_HYBRID_CARS = frozenset((CAR.HYUNDAI_SONATA_HYBRID,)) + + +def supported_cp(cp): + return supported_torque_cp(cp, SONATA_HYBRID_CARS) + + +class SonataHybridTorquePolicy(HKGShapedTorquePolicy): + FACTOR_MULT = shaping.SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT + supported_cp = staticmethod(supported_cp) + + def feedforward(self, value, setpoint, jerk, speed): + return value * shaping.get_sonata_hybrid_ff_scale(setpoint, jerk, speed) * shaping.get_sonata_hybrid_center_taper_scale(setpoint, speed) + + def friction(self, setpoint, jerk, speed): + return shaping.get_sonata_hybrid_friction_threshold(speed, setpoint), 1.0 + + def output(self, value, setpoint, speed): + return value * shaping.get_sonata_hybrid_center_taper_scale(setpoint, speed) * shaping.get_sonata_hybrid_center_output_scale(setpoint, speed) diff --git a/openpilot/starpilot/lateral/sonata_hybrid_shaping.py b/openpilot/starpilot/lateral/sonata_hybrid_shaping.py new file mode 100644 index 0000000000..c1bef8bd13 --- /dev/null +++ b/openpilot/starpilot/lateral/sonata_hybrid_shaping.py @@ -0,0 +1,142 @@ +import math + +import numpy as np + +from openpilot.starpilot.lateral.torque_shaping import get_standard_friction_threshold + + +SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.05 + +SONATA_HYBRID_FF_REDUCTION_LEFT = 0.09 + +SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.26 + +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.10 + +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_HYBRID_CENTER_OUTPUT_TAPER_MAX = 0.14 + +SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT = 0.18 + +SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.05 + +SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED = 12.5 + +SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.5 + +SONATA_HYBRID_CHATTER_THRESHOLD_SPEED_BP = [0.0, 4.5, 7.5, 11.0, 20.0] + +SONATA_HYBRID_CHATTER_THRESHOLD_BUMP = [0.02, 0.04, 0.04, 0.02, 0.0] + +SONATA_HYBRID_CHATTER_THRESHOLD_CENTER = 0.20 + +SONATA_HYBRID_CHATTER_THRESHOLD_CENTER_WIDTH = 0.05 + +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 _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 get_sonata_hybrid_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sonata_hybrid_sigmoid((v_ego - SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED) / + SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT_WIDTH) + return 1.0 - SONATA_HYBRID_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight + +def get_sonata_hybrid_friction_threshold(v_ego: float, desired_lateral_accel: float) -> float: + base_threshold = get_standard_friction_threshold(v_ego) + speed_bump = np.interp(max(v_ego, 0.0), SONATA_HYBRID_CHATTER_THRESHOLD_SPEED_BP, + SONATA_HYBRID_CHATTER_THRESHOLD_BUMP) + center_weight = _sonata_hybrid_sigmoid( + (SONATA_HYBRID_CHATTER_THRESHOLD_CENTER - abs(desired_lateral_accel)) / + SONATA_HYBRID_CHATTER_THRESHOLD_CENTER_WIDTH + ) + return float(base_threshold + speed_bump * center_weight) diff --git a/openpilot/starpilot/lateral/suburban_policy.py b/openpilot/starpilot/lateral/suburban_policy.py new file mode 100644 index 0000000000..89d1cb917c --- /dev/null +++ b/openpilot/starpilot/lateral/suburban_policy.py @@ -0,0 +1,24 @@ +"""Ordinary Suburban torque selection.""" + +import math + +from opendbc.car.structs import CarParams +from opendbc.car.gm.suburban import supported_cp as supported_profile +from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy + + +def supported_cp(cp): + if not supported_profile(cp): + return False + if cp.steerControlType != CarParams.SteerControlType.torque or cp.lateralTuning.which() != "torque": + return False + tune = cp.lateralTuning.torque + return (math.isfinite(tune.latAccelFactor) and tune.latAccelFactor > 0 and + math.isfinite(tune.latAccelOffset) and math.isfinite(tune.friction) and tune.friction >= 0) + + +class SuburbanTorquePolicy(GMOrdinaryTorquePolicy): + def __init__(self, parent, cp): + if not supported_cp(cp): + raise ValueError("Unsupported ordinary Suburban torque profile") + super().__init__(parent, cp) diff --git a/openpilot/starpilot/lateral/tests/__init__.py b/openpilot/starpilot/lateral/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/starpilot/lateral/tests/fixtures/adjacent_lane_geometry.json b/openpilot/starpilot/lateral/tests/fixtures/adjacent_lane_geometry.json new file mode 100644 index 0000000000..cf37cf779e --- /dev/null +++ b/openpilot/starpilot/lateral/tests/fixtures/adjacent_lane_geometry.json @@ -0,0 +1,39 @@ +{ + "model_valid": true, + "model": { + "frameId": 6144, + "frameAge": 0, + "timestampEof": 401368071578, + "laneLineProbs": [0.8565725088119507, 0.983020007610321, 0.05520869046449661, 0.00023782203788869083], + "laneLineStds": [0.23406687378883362, 0.04350994899868965, 0.2947346866130829, 1.1171478033065796], + "roadEdgeStds": [1.0554050207138062, 0.177374467253685], + "laneLines": [ + { + "x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0], + "y": [-4.328125, -4.32421875, -4.3203125, -4.33203125, -4.328125, -4.3359375, -4.3359375, -4.3359375, -4.32421875, -4.32421875, -4.328125, -4.34375, -4.3515625, -4.35546875, -4.3359375, -4.32421875, -4.3125, -4.3125, -4.29296875, -4.265625, -4.2421875, -4.21484375, -4.1953125, -4.203125, -4.2109375, -4.203125, -4.2109375, -4.1875, -4.1640625, -4.10546875, -4.0390625, -3.953125, -3.8359375] + }, + { + "x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0], + "y": [-1.2509765625, -1.25390625, -1.251953125, -1.25390625, -1.255859375, -1.2578125, -1.2509765625, -1.2529296875, -1.255859375, -1.2568359375, -1.259765625, -1.2587890625, -1.2529296875, -1.244140625, -1.2373046875, -1.2255859375, -1.21484375, -1.201171875, -1.185546875, -1.1669921875, -1.15234375, -1.138671875, -1.12890625, -1.123046875, -1.1142578125, -1.1044921875, -1.083984375, -1.0625, -1.021484375, -0.95263671875, -0.87548828125, -0.783203125, -0.66748046875] + }, + { + "x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0], + "y": [1.974609375, 1.9736328125, 1.9765625, 1.9765625, 1.97265625, 1.9765625, 1.970703125, 1.970703125, 1.966796875, 1.970703125, 1.97265625, 1.9716796875, 1.9775390625, 1.9853515625, 1.984375, 2.0, 2.017578125, 2.03125, 2.04296875, 2.0546875, 2.072265625, 2.078125, 2.08984375, 2.109375, 2.125, 2.138671875, 2.16796875, 2.203125, 2.2578125, 2.326171875, 2.421875, 2.533203125, 2.669921875] + }, + { + "x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0], + "y": [4.14453125, 4.14453125, 4.16015625, 4.16015625, 4.16796875, 4.171875, 4.17578125, 4.1796875, 4.1796875, 4.19140625, 4.1875, 4.203125, 4.1953125, 4.203125, 4.19921875, 4.22265625, 4.23828125, 4.26171875, 4.265625, 4.2734375, 4.2578125, 4.2578125, 4.2578125, 4.25390625, 4.2578125, 4.265625, 4.2734375, 4.3125, 4.3359375, 4.390625, 4.4609375, 4.5703125, 4.6640625] + } + ], + "roadEdges": [ + { + "x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0], + "y": [-8.1875, -8.203125, -8.171875, -8.203125, -8.203125, -8.1875, -8.1953125, -8.2109375, -8.2109375, -8.21875, -8.2265625, -8.2265625, -8.25, -8.2578125, -8.2890625, -8.296875, -8.28125, -8.2734375, -8.2578125, -8.2265625, -8.171875, -8.1484375, -8.125, -8.1328125, -8.109375, -8.109375, -8.1015625, -8.078125, -8.0546875, -7.99609375, -7.91015625, -7.80859375, -7.70703125] + }, + { + "x": [0.0, 0.1875, 0.75, 1.6875, 3.0, 4.6875, 6.75, 9.1875, 12.0, 15.1875, 18.75, 22.6875, 27.0, 31.6875, 36.75, 42.1875, 48.0, 54.1875, 60.75, 67.6875, 75.0, 82.6875, 90.75, 99.1875, 108.0, 117.1875, 126.75, 136.6875, 147.0, 157.6875, 168.75, 180.1875, 192.0], + "y": [2.23828125, 2.2421875, 2.2421875, 2.24609375, 2.2421875, 2.251953125, 2.2421875, 2.2578125, 2.263671875, 2.2734375, 2.291015625, 2.318359375, 2.375, 2.4296875, 2.5078125, 2.580078125, 2.669921875, 2.744140625, 2.83203125, 2.876953125, 2.919921875, 2.955078125, 2.98046875, 3.021484375, 3.048828125, 3.107421875, 3.15234375, 3.224609375, 3.30078125, 3.400390625, 3.50390625, 3.623046875, 3.751953125] + } + ] + } +} diff --git a/openpilot/starpilot/lateral/tests/fixtures/corolla_tss2_ba901b5f.json b/openpilot/starpilot/lateral/tests/fixtures/corolla_tss2_ba901b5f.json new file mode 100644 index 0000000000..10e2c08eb2 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/fixtures/corolla_tss2_ba901b5f.json @@ -0,0 +1 @@ +{"source_commit":"ba901b5f551984c331db3dadab9b70af75597d35","source_sha256":{"selfdrive/controls/lib/latcontrol_torque.py":"3561d5ac43a72f9b68bc9e44042b8f5cbdca406dec940e7648c64607862a60f7","selfdrive/controls/lib/latcontrol_vehicle_tunes.py":"f7096ba7208c9d49e410a904909d0ccb1846494b9dc6f413293c6c8096113321","common/pid.py":"67ae4b84e38a219a8997654c16832c2863d56c9458832678dee76497ba36145a","opendbc/car/lateral.py":"8c9fb186ed9a43487478a76e0d1ec2b311d76d44c09e99889a0950cd5f6a2528"},"oracle_boundary":"Frozen original update + PID + friction + default family helpers; current matched CP, VM and torque conversion callbacks; no FLM, trailer or deprecated deadzone boost.","car_params":{"carFingerprint":"TOYOTA_COROLLA_TSS2","mass":1523.9915771484375,"wheelbase":2.6700000762939453,"centerToFront":1.1748000383377075,"steerRatio":13.899999618530273,"tireStiffnessFront":83003.296875,"tireStiffnessRear":103094.59375,"minSteerSpeed":0.0,"lateralTuning":{"torque":{"deprecated":{"useSteeringAngle":false,"kp":0.0,"ki":0.0,"kf":0.0,"kd":0.0},"friction":0.19570063054561615,"steeringAngleDeadzoneDeg":0.0,"latAccelFactor":1.991132378578186,"latAccelOffset":0.0}}},"log_fields":["error","p","i","d","f","output","actualLateralAccel","desiredLateralAccel","desiredLateralJerk"],"cases":[{"name":"pull_away","frames":[{"input":[false,0.0,2.5168921696727433,false,false,-0.0013323183357528668,true,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.4540018440435785,false,false,-0.001291240963619141,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.388915147095227,false,false,-0.0012487290121424167,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.321704390683941,false,false,-0.0012048297124508391,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.252444246513523,false,false,-0.0011595918370278437,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.181211663174453,false,false,-0.0011130656455255266,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.1080857806533686,false,false,-0.0010653028289257553,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.0331478424078884,false,false,-0.0010163564521110636,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.9564811051044617,false,false,-0.0009662808949091303,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.8781707461195272,false,false,-0.000915131791676342,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.798303768906744,false,false,-0.0008629659694875654,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.7169689063354427,false,false,-0.0008098413850007994,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.6342565221076721,false,false,-0.0007558170600668505,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.5502585103633846,false,false,-0.000700953016155571,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.4650681935852896,false,false,-0.0006453102076715109,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.3787802189168077,false,false,-0.000588950454233074,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.2914904530083127,false,false,-0.0005319363719904111,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.2032958755084968,false,false,-0.0004743313040583631,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.114294471319179,false,false,-0.0004161992501417374,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.0245851217332744,false,false,-0.00035760479543111017,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,0.0,0.9342674945768639,false,false,-0.000298613038848147,false,0.2],"torque":5.188903418059707e-18,"active":true,"saturated":false,"log":[0.0,0.0,0.0,-0.0,-1.393409068338849e-17,5.188903568954836e-18,-0.0,0.0,0.0]},{"input":[true,0.045,0.8434429831208166,false,false,-0.00023928952072016532,false,0.2],"torque":0.00031236196521196617,"active":true,"saturated":false,"log":[-3.3342744245601352e-06,-0.0008335686288774014,0.0,0.0,-4.814291514776414e-06,0.0003123619535472244,-3.4888410027633654e-07,-2.465410489094211e-06,7.46762566450343e-07]},{"input":[true,0.09,0.7522124993997793,false,false,-0.00017970014996429066,false,0.2],"torque":0.0012279546642043498,"active":true,"saturated":false,"log":[-1.3102652701491024e-05,-0.003275663126260042,0.0,0.0,-1.84699529199861e-05,0.0012279546353965998,-1.048011199600296e-06,-9.368715836899355e-06,3.7299475934560178e-06]},{"input":[true,0.135,0.6606758273549067,false,false,-0.00011991113086210739,false,0.2],"torque":0.002701309297378084,"active":true,"saturated":false,"log":[-2.8812437449232675e-05,-0.007203109562397003,0.0,0.0,-3.9599399315193295e-05,0.0027013092767447233,-1.5734736962258467e-06,-1.9878036255249754e-05,1.0399921848147642e-05]},{"input":[true,0.18,0.56893308467758,false,false,-5.998888950615651e-05,false,0.2],"torque":0.00468391447066299,"active":true,"saturated":false,"log":[-4.993898619432002e-05,-0.012484746985137463,0.0,0.0,-6.679086800431833e-05,0.004683914594352245,-1.3994206256029429e-06,-3.3138734579551965e-05,2.2160196749609895e-05]},{"input":[true,0.22499999999999998,0.4770846156639367,false,false,0.0,false,0.2],"torque":0.00712443670956969,"active":true,"saturated":false,"log":[-7.592821930302307e-05,-0.0189820546656847,0.0,-0.0,-9.85943988780491e-05,0.007124436553567648,3.2043286210189925e-13,-4.8276913730660453e-05,4.03601661673747e-05]},{"input":[true,0.27,0.3852308838254319,false,false,5.998888950615651e-05,false,0.2],"torque":0.00996894725809459,"active":true,"saturated":false,"log":[-0.00010619919339660555,-0.026549797505140305,0.0,-0.0,-0.00013352782116271555,0.009968947619199753,3.1486972602579044e-06,-6.440294237108901e-05,6.628815754083917e-05]},{"input":[true,0.315,0.29347236436760493,false,false,0.00011991113086210739,false,0.2],"torque":0.013161328232193435,"active":true,"saturated":false,"log":[-0.00014014662883710116,-0.03503665700554848,-4.905131731902657e-07,-0.0,-0.00017008282884489745,0.013161328621208668,8.566691576561425e-06,-8.061500557232648e-05,0.00010116480552824214]},{"input":[true,0.36,0.20190943664935485,false,false,0.00017970014996429066,false,0.2],"torque":0.01664300478370525,"active":true,"saturated":false,"log":[-0.0001771435490809381,-0.044285885989665985,-1.110515654545452e-06,-0.0,-0.00020673099788837135,0.01664300449192524,1.676818101259414e-05,-9.600265184417367e-05,0.00014613675011787564]},{"input":[true,0.40499999999999997,0.1106422767350678,false,false,0.00023928952072016532,false,0.2],"torque":0.020353629925560467,"active":true,"saturated":false,"log":[-0.0002165438636438921,-0.054135967046022415,-1.8684191900320002e-06,-0.0,-0.00024192991259042174,0.02035363018512726,2.825961564667523e-05,-0.0001096503619919531,0.00020227066124789417]},{"input":[true,0.44999999999999996,0.019770750151848926,false,false,0.000298613038848147,false,0.2],"torque":0.024231181865619178,"active":true,"saturated":false,"log":[-0.00025768502382561564,-0.06442125886678696,-2.7703167688741814e-06,-0.0,-0.0002741291536949575,0.024231182411313057,4.35377805843018e-05,-0.00012064108886988834,0.0002705476072151214]},{"input":[true,0.495,-0.07060569503608644,false,false,0.00035760479543111017,false,0.2],"torque":0.028212191897593747,"active":true,"saturated":false,"log":[-0.00029989058384671807,-0.07497264444828033,-3.819933681370458e-06,-0.0,-0.0003017763956449926,0.02821219153702259,6.308792217168957e-05,-0.00012805982260033488,0.00035185780143365264]},{"input":[true,0.54,-0.1603881347241201,false,false,0.0004161992501417374,false,0.2],"torque":0.03267346567998528,"active":true,"saturated":false,"log":[-0.0003424729220569134,-0.08561822772026062,-5.018589035898913e-06,-0.0,-0.00150052341632545,0.032673466950654984,8.738186443224549e-05,-0.00013099714124109596,0.00044699557474814355]},{"input":[true,0.585,-0.24947827472372003,false,false,0.0004743313040583631,false,0.2],"torque":0.037163716938641525,"active":true,"saturated":false,"log":[-0.00038473590393550694,-0.09618397802114487,-6.365164608723717e-06,-0.0,-0.0028387822676450014,0.037163715809583664,0.00011687618098221719,-0.00012855263776145875,0.0005566549953073263]},{"input":[true,0.63,-0.3377785560679362,false,false,0.0005319363719904111,false,0.2],"torque":0.04156144333263071,"active":true,"saturated":false,"log":[-0.0004259773704688996,-0.10649434477090836,-7.8560851761722e-06,-0.0,-0.004167880397289991,0.041561443358659744,0.00015201039786916226,-0.00011983851436525583,0.0006814255029894412]},{"input":[true,0.6749999999999999,-0.4251922595557785,false,false,0.000588950454233074,false,0.2],"torque":0.045799807477312444,"active":true,"saturated":false,"log":[-0.00046549199032597244,-0.11637299507856369,-9.485307600698434e-06,-0.0,-0.005486317910254002,0.04579980671405792,0.00019320519641041756,-0.00010398291487945244,0.0008217881550081074]},{"input":[true,0.72,-0.5116236094261108,false,false,0.0006453102076715109,false,0.2],"torque":0.04981170746880695,"active":true,"saturated":false,"log":[-0.0005025736754760146,-0.12564341723918915,-1.1244314919167664e-05,-0.0,-0.006792622618377209,0.0498117059469223,0.00024086075427476317,-8.013338810997084e-05,0.0009781121043488383]},{"input":[true,0.765,-0.5969778760525832,false,false,0.000700953016155571,false,0.2],"torque":0.053530023363587774,"active":true,"saturated":false,"log":[-0.0005365182878449559,-0.1341295689344406,-1.3122129530529492e-05,-0.0,-0.008085355162620544,0.053530022501945496,0.0002953549555968493,-4.7460198402404785e-05,0.0011506517184898257]},{"input":[true,0.8099999999999999,-0.681161477552094,false,false,0.0007558170600668505,false,0.2],"torque":0.05688786190827032,"active":true,"saturated":false,"log":[-0.0005666263168677688,-0.14165657758712769,-1.5105321836017538e-05,-0.0,-0.009363114833831787,0.05688786134123802,0.0003570419503375888,-5.159617558092577e-06,0.0013395434943959117]},{"input":[true,0.855,-0.7640820802003787,false,false,0.0008098413850007994,false,0.2],"torque":0.05982102236398815,"active":true,"saturated":false,"log":[-0.0005922052077949047,-0.14805130660533905,-1.71780393429799e-05,-0.0,-0.010624547488987446,0.05982102081179619,0.00042625030619092286,4.7542846004944295e-05,0.001544804428704083]},{"input":[true,0.8999999999999999,-0.8456486975495201,false,false,0.0008629659694875654,false,0.2],"torque":0.06226246230361102,"active":true,"saturated":false,"log":[-0.0006125721265561879,-0.1531430333852768,-1.9322042135172524e-05,-0.0,-0.011868346482515335,0.062262460589408875,0.0005032817716710269,0.00011138931586174294,0.0017663293983787298]},{"input":[true,0.945,-0.9257717881434888,false,false,0.000915131791676342,false,0.2],"torque":0.06414711171374053,"active":true,"saturated":false,"log":[-0.0006270563462749124,-0.15676409006118774,-2.151674016204197e-05,-0.0,-0.013093261979520321,0.06414711475372314,0.0005884096026420593,0.00018708647985477,0.002003889996558428]},{"input":[true,0.99,-1.0043633517292305,false,false,0.0009662808949091303,false,0.2],"torque":0.06541122129753692,"active":true,"saturated":false,"log":[-0.0006350015755742788,-0.15875039994716644,-2.373924507992342e-05,-0.0,-0.01429810468107462,0.06541122496128082,0.0006818773690611124,0.00027530259103514254,0.002257133601233363]},{"input":[true,1.035,-1.0813370238623508,false,false,0.0010163564521110636,false,0.2],"torque":0.06314996110884494,"active":true,"saturated":false,"log":[-0.0006286325515247881,-0.15143758058547974,-2.5939458282664418e-05,-0.0,-0.01547248288989067,0.06314995884895325,0.0007838974706828594,0.0003766645968426019,0.002525581978261471]},{"input":[true,1.08,-1.1566081688080532,false,false,0.0010653028289257553,false,0.2],"torque":0.05959944861795032,"active":true,"saturated":false,"log":[-0.0006140905315987766,-0.14074954390525818,-2.8088776161894202e-05,-0.0,-0.016624122858047485,0.05959944799542427,0.0008946498273871839,0.0004917553742416203,0.0028086320962756872]},{"input":[true,1.125,-1.2300939706397322,false,false,0.0011130656455255266,false,0.2],"torque":0.0556331644928744,"active":true,"saturated":false,"log":[-0.0005930997431278229,-0.12899918854236603,-3.016462505911477e-05,-0.0,-0.01775505766272545,0.05563316494226456,0.0010142810642719269,0.0006211110157892108,0.003105555195361376]},{"input":[true,1.17,-1.301713522439424,false,false,0.0011595918370278437,false,0.2],"torque":0.05129387805168514,"active":true,"saturated":false,"log":[-0.00056513148592785,-0.11630406230688095,-3.214258322259411e-05,-0.0,-0.018864447250962257,0.05129387974739075,0.0011429029982537031,0.0007652182248421013,0.003415497252717614]},{"input":[true,1.2149999999999999,-1.3713879135062645,false,false,0.0012048297124508391,false,0.2],"torque":0.04663228164508153,"active":true,"saturated":false,"log":[-0.0005296355811879039,-0.10280226916074753,-3.399630804779008e-05,-0.0,-0.01995147205889225,0.04663228243589401,0.0012805918231606483,0.0009245119872502983,0.0037374808453023434]},{"input":[true,1.26,-1.4390403144811001,false,false,0.0012487290121424167,false,0.2],"torque":0.041706785277694024,"active":true,"saturated":false,"log":[-0.000486029835883528,-0.08865184336900711,-3.569741238607094e-05,-0.0,-0.021015319973230362,0.041706785559654236,0.0014273872366175056,0.0010993729811161757,0.004070404917001724]},{"input":[true,1.305,-1.5045960602975343,false,false,0.001291240963619141,false,0.2],"torque":0.036583271155382735,"active":true,"saturated":false,"log":[-0.00043368455953896046,-0.07402995228767395,-3.7215308111626655e-05,-0.0,-0.02205517143011093,0.036583270877599716,0.0015832912176847458,0.001290125772356987,0.0044130487367510796]},{"input":[true,1.3499999999999999,-1.567982730871861,false,false,0.0013323183357528668,false,0.2],"torque":0.031334784564336596,"active":true,"saturated":false,"log":[-0.0003718992229551077,-0.05913197621703148,-3.8516958738910034e-05,-0.0,-0.023070169612765312,0.031334783881902695,0.00174826814327389,0.001497036311775446,0.004764070268720388]},{"input":[true,1.395,-1.6291302294466827,false,false,0.0013719154912449113,false,0.2],"torque":0.0260411978022802,"active":true,"saturated":false,"log":[-0.00029986759182065725,-0.04417049512267113,-3.956649379688315e-05,-0.0,-0.024059368297457695,0.026041198521852493,0.0019222429255023599,0.0017203104216605425,0.005122012458741665]},{"input":[true,1.44,-1.6879708585053412,false,false,0.0014099884373294702,false,0.2],"torque":0.020788823893570087,"active":true,"saturated":false,"log":[-0.00021662400104105473,-0.02937421388924122,-4.032467768411152e-05,-0.0,-0.02502167597413063,0.020788824185729027,0.002105101477354765,0.0019600915256887674,0.005485304165631533]},{"input":[true,1.4849999999999999,-1.744439393176785,false,false,0.0014464948746499401,true,0.2],"torque":0.01566998542441227,"active":true,"saturated":false,"log":[-0.000120958560728468,-0.014986765570938587,-4.074803291587159e-05,-0.0,-0.02595573477447033,0.015669984742999077,0.0022966896649450064,0.0022164597176015377,0.005852262023836374]},{"input":[true,1.53,-1.7984731520530517,false,false,0.0014813942442538548,false,0.2],"torque":0.010795160948811012,"active":true,"saturated":false,"log":[-1.1128190635645296e-05,-0.0012986598303541541,-4.0786981116980314e-05,-0.0,-0.026859553530812263,0.010795161128044128,0.0024968129582703114,0.00248942943289876,0.006221097428351641]},{"input":[true,1.575,-1.8500120653441623,false,false,0.0015146477726542137,false,0.2],"torque":0.005913382282392208,"active":true,"saturated":false,"log":[0.00011050974717363715,0.012349463999271393,-4.040019848616794e-05,-0.0,-0.027736518532037735,0.005913382396101952,0.002705236664041877,0.002778948750346899,0.006589917466044426]},{"input":[true,1.6199999999999999,-1.8989987402979225,false,false,0.0015462185149071458,false,0.2],"torque":0.000997618997715954,"active":true,"saturated":false,"log":[0.0002437036018818617,0.02602754533290863,-3.954723433707841e-05,-0.0,-0.02858734130859375,0.000997619004920125,0.0029216851107776165,0.003084897994995117,0.006956728640943766]},{"input":[true,1.665,-1.9453785238149528,false,false,0.0015760713956580457,false,0.2],"torque":-0.003908069497499577,"active":true,"saturated":false,"log":[0.00038899542414583266,0.03961918503046036,-3.818574987235479e-05,-0.0,-0.029411757364869118,-0.003908069338649511,0.003145841648802161,0.0034070885740220547,0.007319445721805096]},{"input":[true,1.71,-1.9890995621920713,false,false,0.0016041732481105837,false,0.2],"torque":-0.008757394033262847,"active":true,"saturated":false,"log":[0.000546984258107841,0.053002774715423584,-3.627130718086846e-05,-0.0,-0.030209485441446304,-0.008757393807172775,0.0033773493487387896,0.0037452627439051867,0.007675890810787678]},{"input":[true,1.755,-2.0301128579301215,false,false,0.001630492850875286,false,0.2],"torque":-0.01350205986679513,"active":true,"saturated":false,"log":[0.00071834831032902,0.06605212390422821,-3.375708547537215e-05,-0.0,-0.030980199575424194,-0.013502059504389763,0.003615810303017497,0.004099092446267605,0.008023803122341633]},{"input":[true,1.7999999999999998,-2.06837232354531,false,false,0.0016550009626567592,false,0.2],"torque":-0.018092029430263147,"active":true,"saturated":false,"log":[0.0009038756834343076,0.07863718271255493,-3.059352093259804e-05,-0.0,-0.03172348812222481,-0.01809202879667282,0.003860786324366927,0.004468178376555443,0.008360842242836952]},{"input":[true,1.845,-2.103834832326184,false,false,0.0016776703547410072,false,0.2],"torque":-0.022475769124297895,"active":true,"saturated":false,"log":[0.0011045054998248816,0.09062467515468597,-2.6727751901489682e-05,-0.0,-0.032438792288303375,-0.022475769743323326,0.004111798945814371,0.0048520504496991634,0.008684595115482807]},{"input":[true,1.89,-2.136460265981483,false,false,0.0016984758412467596,false,0.2],"torque":-0.02660053886409502,"active":true,"saturated":false,"log":[0.001321385963819921,0.10187885910272598,-2.2102902221377008e-05,-0.0,-0.033125344663858414,-0.026600539684295654,0.004368330352008343,0.005250168032944202,0.008992581628262997]},{"input":[true,1.9349999999999998,-2.1662115591273006,false,false,0.0017173943071071896,false,0.2],"torque":-0.030412712346218448,"active":true,"saturated":false,"log":[0.0015559569001197815,0.1122622936964035,-1.665705167397391e-05,-0.0,-0.033782053738832474,-0.030412713065743446,0.004629823844879866,0.00566191878169775,0.009282258339226246]},{"input":[true,1.98,-2.1930547405651954,false,false,0.0017344047337509473,false,0.2],"torque":-0.033858090420437005,"active":true,"saturated":false,"log":[0.0018100676825270057,0.12163654714822769,-1.0321815352654085e-05,-0.0,-0.03440733999013901,-0.03385809063911438,0.004895683377981186,0.006086619105190039,0.009551030583679676]},{"input":[true,2.025,-2.2169589713061777,false,false,0.0017494882224539628,false,0.2],"torque":-0.037861692061569974,"active":true,"saturated":false,"log":[0.0020648695062845945,0.13240975141525269,-3.0947721825214103e-06,-0.0,-0.03502655774354935,-0.037861693650484085,0.00516527658328414,0.006523516029119492,0.009796255268156528]},{"input":[true,2.07,-2.237896579298861,false,false,0.0017626280153360862,false,0.2],"torque":-0.04253076849327863,"active":true,"saturated":false,"log":[0.002315852791070938,0.14485658705234528,5.010712357034208e-06,-0.0,-0.03564649075269699,-0.04253076761960983,0.005437932908535004,0.006971784867346287,0.010015249252319336]},{"input":[true,2.1149999999999998,-2.255843090823365,false,false,0.0017738095139792282,false,0.2],"torque":-0.04715775779542637,"active":true,"saturated":false,"log":[0.00257706712000072,0.15713666379451752,1.4030447346158326e-05,-0.0,-0.03624987602233887,-0.047157756984233856,0.005712947342544794,0.00743053387850523,0.01020529679954052]},{"input":[true,2.16,-2.2707772585160746,false,false,0.0017830202956463255,false,0.2],"torque":-0.05172100244308219,"active":true,"saturated":false,"log":[0.0028484060894697905,0.16919532418251038,2.3999868062674068e-05,-0.0,-0.03683788701891899,-0.0517210029065609,0.005989579018205404,0.007898801006376743,0.010363655164837837]},{"input":[true,2.205,-2.2826810859937297,false,false,0.0017902501270831014,false,0.2],"torque":-0.05619850998389153,"active":true,"saturated":false,"log":[0.0031297453679144382,0.18097752332687378,3.495397686492652e-05,-0.0,-0.03741176053881645,-0.05619851127266884,0.0062670535407960415,0.008375559002161026,0.010487564839422703]},{"input":[true,2.25,-2.291539849048848,false,false,0.001795490975887298,false,0.2],"torque":-0.060568016466401454,"active":true,"saturated":false,"log":[0.003420943394303322,0.19242806732654572,4.6927278162911534e-05,-0.0,-0.03797280043363571,-0.06056801602244377,0.006544564850628376,0.008859713561832905,0.01057425793260336]},{"input":[true,2.295,-2.2973421133920016,false,false,0.0017987370194327387,false,0.2],"torque":-0.06480713341618516,"active":true,"saturated":false,"log":[0.0037218460347503424,0.2034919261932373,5.995374158374034e-05,-0.0,-0.03852237015962601,-0.06480713188648224,0.006821274757385254,0.009350105188786983,0.010620961897075176]},{"input":[true,2.34,-2.3000797489200013,false,false,0.0017999846513383159,false,0.2],"torque":-0.06889341642590807,"active":true,"saturated":false,"log":[0.004032288212329149,0.2141145020723343,7.406675285892561e-05,-0.0,-0.03906189277768135,-0.0688934177160263,0.007096317131072283,0.009845515713095665,0.01062491349875927]},{"input":[true,2.385,-2.299747940492644,false,false,0.001799232485474709,false,0.2],"torque":-0.07280447643557783,"active":true,"saturated":false,"log":[0.004352097865194082,0.22424183785915375,8.929909381549805e-05,-0.0,-0.03959283605217934,-0.07280447334051132,0.007368796039372683,0.010344660840928555,0.010583360679447651]},{"input":[true,2.4299999999999997,-2.2963451952042293,false,false,0.0017964813575043867,false,0.2],"torque":-0.07651811046885518,"active":true,"saturated":false,"log":[0.004681101534515619,0.23382101953029633,0.00010568294965196401,-0.0,-0.040116701275110245,-0.07651811093091965,0.007637790869921446,0.01084620039910078,0.01049357745796442]},{"input":[true,2.475,-2.289873346139702,false,false,0.001791734323953177,false,0.2],"torque":-0.08001237384722058,"active":true,"saturated":false,"log":[0.005019127856940031,0.24280031025409698,0.00012324989074841142,-0.0,-0.04063502699136734,-0.08001237362623215,0.007902354933321476,0.01134873740375042,0.010352867655456066]},{"input":[true,2.52,-2.280337552608878,false,false,0.0017849966588144434,false,0.2],"torque":-0.08349898274954289,"active":true,"saturated":false,"log":[0.005366016179323196,0.2511295676231384,0.00014203095634002239,-0.0,-0.0405607745051384,-0.0834989845752716,0.008161518722772598,0.011850820854306221,0.01015857420861721]},{"input":[true,2.565,-2.26774629685586,false,false,0.0017762758476896352,false,0.2],"torque":-0.08701666113577945,"active":true,"saturated":false,"log":[0.005721623543649912,0.2587604224681854,0.00016205663268920034,-0.0,-0.03974834829568863,-0.08701666444540024,0.008414293639361858,0.01235094666481018,0.009908091276884079]},{"input":[true,2.61,-2.2521113772443626,false,false,0.0017655815794717213,false,0.2],"torque":-0.09025401947947889,"active":true,"saturated":false,"log":[0.006085832603275776,0.2656466066837311,0.000183357042260468,-0.0,-0.03893505409359932,-0.09025401622056961,0.008659669198095798,0.012847562320530415,0.009598866105079651]},{"input":[true,2.655,-2.233447897923341,false,false,0.0017529257355807513,false,0.2],"torque":-0.0931914654785861,"active":true,"saturated":false,"log":[0.006458566524088383,0.2717441916465759,0.00020596203103195876,-0.0,-0.038122422993183136,-0.0931914672255516,0.008896620944142342,0.013339067809283733,0.009228415787220001]},{"input":[true,2.6999999999999997,-2.2117742549809565,false,false,0.0017383223767634996,false,0.2],"torque":-0.0958102413670491,"active":true,"saturated":false,"log":[0.006839799229055643,0.2770118713378906,0.00022990132856648415,-0.0,-0.03731193393468857,-0.09581024199724197,0.009124106727540493,0.01382382307201624,0.008794328197836876]},{"input":[true,2.745,-2.1871121190985012,false,false,0.00172178772747186,false,0.2],"torque":-0.09809258649124042,"active":true,"saturated":false,"log":[0.007229574955999851,0.28141120076179504,0.0002552048536017537,-0.0,-0.036504991352558136,-0.09809258580207825,0.009341074153780937,0.014300143346190453,0.008294275961816311]},{"input":[true,2.79,-2.1594864147196176,false,false,0.001703340157837346,false,0.2],"torque":-0.10002187260385637,"active":true,"saturated":false,"log":[0.007628030609339476,0.28490695357322693,0.0002819029614329338,-0.0,-0.0357029028236866,-0.10002186894416809,0.009546458721160889,0.014766310341656208,0.007726022507995367]},{"input":[true,2.835,-2.1289252957536764,false,false,0.001683000163261724,false,0.2],"torque":-0.1015826931072932,"active":true,"saturated":false,"log":[0.008035420440137386,0.2874671518802643,0.0003100269241258502,-0.0,-0.034906819462776184,-0.10158269107341766,0.009739188477396965,0.015220570378005505,0.007087429985404015]},{"input":[true,2.88,-2.0954601178358474,false,false,0.0016607903416464523,false,0.2],"torque":-0.10276101797929663,"active":true,"saturated":false,"log":[0.008452151902019978,0.28906360268592834,0.00033960945438593626,-0.0,-0.034117717295885086,-0.10276101529598236,0.00991818681359291,0.015661140903830528,0.006376469507813454]},{"input":[true,2.925,-2.059125407169935,false,false,0.001636735368286227,false,0.2],"torque":-0.10354431784116053,"active":true,"saturated":false,"log":[0.008878828957676888,0.28967180848121643,0.00037068536039441824,-0.0,-0.03333631530404091,-0.10354431718587875,0.010082371532917023,0.016086211428046227,0.005591228138655424]},{"input":[true,2.9699999999999998,-2.019958825983635,false,false,0.0016108619684545263,false,0.2],"torque":-0.10392173049169011,"active":true,"saturated":false,"log":[0.009316311217844486,0.2892714738845825,0.00040329244802705944,-0.0,-0.03256300464272499,-0.10392173379659653,0.010230661369860172,0.01649394817650318,0.004729917738586664]},{"input":[true,3.0149999999999997,-1.9780011346293873,false,false,0.001583198887711611,false,0.2],"torque":-0.10488210524934635,"active":true,"saturated":false,"log":[0.009723806753754616,0.2903650104999542,0.00043732576887123287,-0.0,-0.03185220807790756,-0.10488210618495941,0.01036197692155838,0.016882503405213356,0.0037908819504082203]},{"input":[true,3.06,-1.9332961503675115,false,false,0.0015537768599679727,false,0.2],"torque":-0.10757697584192025,"active":true,"saturated":false,"log":[0.010047625750303268,0.29585233330726624,0.0004724924510810524,-0.0,-0.031275082379579544,-0.10757697373628616,0.010475240647792816,0.01725000888109207,0.0027726045809686184]},{"input":[true,3.105,-1.8858907028717764,false,false,0.0015226285733387025,false,0.2],"torque":-0.11008135305577267,"active":true,"saturated":false,"log":[0.010364742018282413,0.3008755147457123,0.0005087690660730004,-0.0,-0.03073323518037796,-0.11008135229349136,0.010569384321570396,0.017594588920474052,0.0016737171681597829]},{"input":[true,3.15,-1.8358345865009793,true,false,0.0014897886338267289,false,0.2],"torque":-0.11237012249531082,"active":true,"saturated":false,"log":[0.010674218647181988,0.305416077375412,0.0005087690660730004,-0.0,-0.0302293561398983,-0.11237012594938278,0.010643348097801208,0.017914360389113426,0.0004930063150823116]},{"input":[true,3.195,-1.7831805093835322,true,false,0.001455293526875262,false,0.2],"torque":-0.1144484797551514,"active":true,"saturated":false,"log":[0.010975101962685585,0.3094567060470581,0.0005087690660730004,-0.0,-0.029766114428639412,-0.11444848030805588,0.01069608423858881,0.018207436427474022,-0.000770578917581588]},{"input":[true,3.2399999999999998,-1.72798403936535,true,false,0.0014191815768321676,false,0.2],"torque":-0.11630732335022863,"active":true,"saturated":false,"log":[0.011266428045928478,0.31298136711120605,0.0005087690660730004,-0.0,-0.029346195980906487,-0.1163073256611824,0.010726560838520527,0.018471933901309967,-0.002117920434102416]},{"input":[true,3.2849999999999997,-1.6703035468746825,true,false,0.0013814929043712983,false,0.2],"torque":-0.11793791956376336,"active":true,"saturated":false,"log":[0.011547215282917023,0.3159750998020172,0.0005087690660730004,-0.0,-0.02897229976952076,-0.11793792247772217,0.01073376089334488,0.018705977126955986,-0.0035497238859534264]},{"input":[true,3.33,-1.6102001447607586,true,false,0.0013422693819180966,false,0.2],"torque":-0.11933207160624558,"active":true,"saturated":false,"log":[0.011816476471722126,0.3184244930744171,0.0005087690660730004,-0.0,-0.02864708937704563,-0.11933207511901855,0.010716689750552177,0.01890769973397255,-0.005066513083875179]},{"input":[true,3.375,-1.5477376251663058,true,false,0.0013015545871289842,false,0.2],"torque":-0.12048206804142493,"active":true,"saturated":false,"log":[0.0120732132345438,0.32031744718551636,0.0005087690660730004,-0.0,-0.02837321162223816,-0.12048206478357315,0.010674374178051949,0.019075248390436172,-0.00666862353682518]},{"input":[true,3.42,-1.482982393497155,true,false,0.0012593937544762292,false,0.2],"torque":-0.12138073303453938,"active":true,"saturated":false,"log":[0.012316418811678886,0.32164326310157776,0.0005087690660730004,-0.0,-0.028153281658887863,-0.12138073146343231,0.010605868883430958,0.01920679584145546,-0.008356194943189621]},{"input":[true,3.465,-1.4160033995551988,true,false,0.001215833724992072,false,0.2],"torque":-0.12202148433297413,"active":true,"saturated":false,"log":[0.012545081786811352,0.32239291071891785,0.0005087690660730004,-0.0,-0.027989890426397324,-0.12202148139476776,0.01051025278866291,0.019300533458590508,-0.010129169560968876]},{"input":[true,3.51,-1.3468720659040154,true,false,0.0011709228942279536,false,0.2],"torque":-0.12239836862693919,"active":true,"saturated":false,"log":[0.01275818794965744,0.3225588798522949,0.0005087690660730004,-0.0,-0.027885587885975838,-0.12239836901426315,0.010386639274656773,0.01935468427836895,-0.01198728196322918]},{"input":[true,3.5549999999999997,-1.2756622135393854,true,false,0.0011247111584866602,false,0.2],"torque":-0.1225060568982396,"active":true,"saturated":false,"log":[0.012954718433320522,0.3221352696418762,0.0005087690660730004,-0.0,-0.027842877432703972,-0.12250605970621109,0.010234172455966473,0.019367506727576256,-0.013930060900747776]},{"input":[true,3.5999999999999996,-1.202449984939821,true,false,0.0010772498593871215,false,0.2],"torque":-0.1223399079270997,"active":true,"saturated":false,"log":[0.013133654370903969,0.3211178481578827,0.0005087690660730004,0.0,-0.02786421775817871,-0.12233990430831909,0.010052033700048923,0.01933729462325573,-0.01595681719481945]},{"input":[true,3.645,-1.12731376457502,true,false,0.0010285917268234453,false,0.2],"torque":-0.12189599959612443,"active":true,"saturated":false,"log":[0.0132939787581563,0.31950417160987854,0.0005087690660730004,0.0,-0.027952009811997414,-0.12189599871635437,0.009839444421231747,0.019262386485934258,-0.018066642805933952]},{"input":[true,3.69,-1.0503340969528967,true,false,0.0009787908203815724,false,0.2],"torque":-0.12117114005168951,"active":true,"saturated":false,"log":[0.013434676453471184,0.3172934651374817,0.0005087690660730004,0.0,-0.028108594939112663,-0.12117113918066025,0.009595666080713272,0.019141167402267456,-0.02025841549038887]},{"input":[true,3.735,-0.9715936022884895,true,false,0.0009279024692786355,false,0.2],"torque":-0.1201628968718934,"active":true,"saturated":false,"log":[0.013554736040532589,0.3144868314266205,0.0005087690660730004,0.0,-0.02833624556660652,-0.12016289681196213,0.009320002980530262,0.01897207647562027,-0.022530777379870415]},{"input":[true,3.78,-0.891176889880571,true,false,0.0008759832108917369,false,0.2],"torque":-0.11886963327679673,"active":true,"saturated":false,"log":[0.013653152622282505,0.31108707189559937,0.0005087690660730004,0.0,-0.02863716334104538,-0.11886963248252869,0.009011806920170784,0.01875360682606697,-0.024882148951292038]},{"input":[true,3.8249999999999997,-0.8091704692843487,true,false,0.0008230907279444615,false,0.2],"torque":-0.11729052399177642,"active":true,"saturated":false,"log":[0.013728928752243519,0.30709898471832275,0.0005087690660730004,0.0,-0.02901347726583481,-0.11729052662849426,0.00867047905921936,0.018484316766262054,-0.027310719713568687]},{"input":[true,3.8699999999999997,-0.7256626593709485,false,false,0.0007692837844208935,true,0.2],"torque":-0.11540297139566277,"active":true,"saturated":false,"log":[0.01378107350319624,0.3025290071964264,0.0004552489845082164,0.0,-0.029467225074768066,-0.11540297418832779,0.008295469917356968,0.01816282980144024,-0.029814448207616806]},{"input":[true,3.915,-0.6407434953667595,false,false,0.0007146221602783498,false,0.2],"torque":-0.11327339191048202,"active":true,"saturated":false,"log":[0.013808609917759895,0.29738566279411316,0.0005035791546106339,0.0,-0.030000366270542145,-0.11327338963747025,0.00788628589361906,0.017787834629416466,-0.032391052693128586]},{"input":[true,3.96,-0.5545046339678528,false,false,0.0006591665850313498,false,0.2],"torque":-0.110860592646915,"active":true,"saturated":false,"log":[0.013810567557811737,0.29167917370796204,0.0005519161350093782,0.0,-0.030614759773015976,-0.11086059361696243,0.007442486472427845,0.017358101904392242,-0.03503802791237831]},{"input":[true,4.005,-0.46703925662689133,false,false,0.0006029786702806292,false,0.2],"torque":-0.10816712263104508,"active":true,"saturated":false,"log":[0.013785991817712784,0.2854217290878296,0.0006001670844852924,0.0,-0.03131217509508133,-0.1081671193242073,0.006963691208511591,0.016872476786375046,-0.03775262087583542]},{"input":[true,4.05,-0.3784419711118911,false,false,0.000546120841262141,false,0.2],"torque":-0.10519639938346773,"active":true,"saturated":false,"log":[0.013733940199017525,0.27862730622291565,0.0006482359021902084,0.0,-0.03209427744150162,-0.10519640147686005,0.006449577864259481,0.016329888254404068,-0.04053185135126114]},{"input":[true,4.095,-0.2888087114381786,false,false,0.0004886562674921086,false,0.2],"torque":-0.10195272510663693,"active":true,"saturated":false,"log":[0.013653484173119068,0.27131178975105286,0.0006960230530239642,0.0,-0.0329626239836216,-0.10195272415876389,0.005899888463318348,0.015729352831840515,-0.0433725044131279]},{"input":[true,4.14,-0.19823663627666316,false,false,0.00043064879258516834,false,0.2],"torque":-0.09844130060701749,"active":true,"saturated":false,"log":[0.013543711043894291,0.2634929120540619,0.0007434260915033519,0.0,-0.03391867130994797,-0.09844130277633667,0.005314426496624947,0.01506997924298048,-0.046271126717329025]},{"input":[true,4.185,-0.10682402594330465,false,false,0.0003721628633235905,false,0.2],"torque":-0.09466822172090987,"active":true,"saturated":false,"log":[0.013403723947703838,0.2551901638507843,0.0007903390796855092,0.0,-0.034963760524988174,-0.09466822445392609,0.004693063907325268,0.014350971207022667,-0.04922403022646904]},{"input":[true,4.2299999999999995,-0.014670178076241769,false,false,0.0003132634580563725,false,0.2],"torque":-0.09064045286082736,"active":true,"saturated":false,"log":[0.0132326390594244,0.2464248239994049,0.0008366533438675106,0.0,-0.036099109798669815,-0.09064045548439026,0.004035738296806812,0.013571630232036114,-0.05222730711102486]},{"input":[true,4.2749999999999995,0.0781246978914267,false,false,0.00025401601450776096,false,0.2],"torque":-0.08636586108775375,"active":true,"saturated":false,"log":[0.013029590249061584,0.23721997439861298,0.0008822568925097585,0.0,-0.037325840443372726,-0.08636586368083954,0.003342457115650177,0.012731361202895641,-0.05527681112289429]},{"input":[true,4.32,0.17145958735384198,false,false,0.00019448635707539666,false,0.2],"torque":-0.08185318797239788,"active":true,"saturated":false,"log":[0.012793726287782192,0.22760039567947388,0.0009270349401049316,0.0,-0.0386449359357357,-0.08185318857431412,0.0026132992934435606,0.011829675175249577,-0.058368172496557236]},{"input":[true,4.365,0.2652327781388689,false,false,0.00013474062369886897,false,0.2],"torque":-0.077112044583967,"active":true,"saturated":false,"log":[0.012524210847914219,0.2175925076007843,0.0009708696743473411,0.0,-0.04005727171897888,-0.07711204141378403,0.0018484153551980853,0.010866193100810051,-0.061496805399656296]},{"input":[true,4.41,0.3593419689870246,false,false,7.484519237992288e-05,false,0.2],"torque":-0.07215287856182646,"active":true,"saturated":false,"log":[0.012220218777656555,0.20722435414791107,0.0010136404307559133,0.0,-0.041563600301742554,-0.07215287536382675,0.0010480297496542335,0.009840646758675575,-0.06465789675712585]},{"input":[true,4.455,0.4536843787899447,false,false,1.4866607435947705e-05,false,0.2],"torque":-0.06698699107821103,"active":true,"saturated":false,"log":[0.011880938895046711,0.19652557373046875,0.0010552237508818507,0.0,-0.04316455125808716,-0.06698699295520782,0.00021244182426016778,0.008752886205911636,-0.06784641742706299]},{"input":[true,4.5,0.5481568564744977,false,false,-4.5128494431304356e-05,false,0.2],"torque":-0.06162648515544732,"active":true,"saturated":false,"log":[0.011505567468702793,0.1855272799730301,0.0010954932076856494,0.0,-0.04486064985394478,-0.06162648648023605,-0.0006579733453691006,0.00760287931188941,-0.07105714827775955]},{"input":[true,4.545,0.6426559914178609,false,false,-0.00010507345816964415,false,0.2],"torque":-0.0560842644704105,"active":true,"saturated":false,"log":[0.011093308217823505,0.17426200211048126,0.001134319812990725,0.0,-0.04665228724479675,-0.05608426406979561,-0.0015627634711563587,0.006390714086592197,-0.07428465783596039]},{"input":[true,4.59,0.7370782242781213,false,false,-0.0001649016844307542,false,0.2],"torque":-0.05037398800154973,"active":true,"saturated":false,"log":[0.010643365792930126,0.16276367008686066,0.001171571551822126,0.0,-0.04853975772857666,-0.05037398636341095,-0.002501398790627718,0.005116603337228298,-0.07752329856157303]},{"input":[true,4.635,0.8313199581243969,false,false,-0.0002245467035626178,false,0.2],"torque":-0.04451006085044558,"active":true,"saturated":false,"log":[0.01015494391322136,0.15106748044490814,0.00120711384806782,0.0,-0.05052324756979942,-0.04451005905866623,-0.003473270684480667,0.003780885599553585,-0.08076725900173187]},{"input":[true,4.68,0.9252776697499385,false,false,-0.00028394224945784677,false,0.2],"torque":-0.03850757897819877,"active":true,"saturated":false,"log":[0.009627236053347588,0.1392098367214203,0.00124080921523273,0.0,-0.05260283499956131,-0.03850758075714111,-0.00447769183665514,0.002384028397500515,-0.0840105339884758]},{"input":[true,4.725,1.0188480210513646,false,true,-0.0003430223331758737,false,0.2],"torque":-0.03236838939310086,"active":true,"saturated":false,"log":[0.009059421718120575,0.1272282600402832,0.00124080921523273,0.0,-0.05477852001786232,-0.032368388026952744,-0.0055138953030109406,0.0009266287670470774,-0.08724693208932877]},{"input":[true,4.77,1.1119279703568874,false,true,-0.00040172131625720533,false,0.2],"torque":-0.026128900140426233,"active":true,"saturated":false,"log":[0.008450653403997421,0.11516127735376358,0.00124080921523273,0.0,-0.057050153613090515,-0.026128899306058884,-0.006581034045666456,-0.0005905844154767692,-0.09047010540962219]},{"input":[true,4.8149999999999995,1.2044148835863053,false,true,-0.0004599739836482962,false,0.2],"torque":-0.019804768189513825,"active":true,"saturated":false,"log":[0.007800048217177391,0.10304838418960571,0.00124080921523273,0.0,-0.059417545795440674,-0.01980476826429367,-0.007678180932998657,-0.002166748745366931,-0.09367354959249496]},{"input":[true,4.859999999999999,1.2962066451254834,false,true,-0.000517715616156007,false,0.2],"torque":-0.013403120891759315,"active":true,"saturated":false,"log":[0.00710667297244072,0.09092988073825836,0.00124080921523273,0.0,-0.06188060715794563,-0.013403120450675488,-0.008804329670965672,-0.00380086712539196,-0.09685058891773224]},{"input":[true,4.905,1.3872017682982085,false,true,-0.0005748820623511629,false,0.2],"torque":-0.006941941406036011,"active":true,"saturated":false,"log":[0.00636952742934227,0.07884678989648819,0.00124080921523273,0.0,-0.06443912535905838,-0.006941941566765308,-0.009958392940461636,-0.0054918075911700726,-0.09999442845582962]},{"input":[true,4.95,1.4772995053184916,false,true,-0.0006314098098413157,false,0.2],"torque":-0.0004397090793426061,"active":true,"saturated":false,"log":[0.005587524268776178,0.066840760409832,0.00124080921523273,0.0,-0.0670928806066513,-0.0004397090815473348,-0.011139205656945705,-0.007238300051540136,-0.10309813171625137]},{"input":[true,4.995,1.5663999566068048,false,true,-0.0006872360558335404,false,0.2],"torque":0.006084670653240412,"active":true,"saturated":false,"log":[0.004759462084621191,0.05495394021272659,0.00124080921523273,0.0,-0.06984171271324158,0.006084670778363943,-0.012345520779490471,-0.009038940072059631,-0.10615462809801102]},{"input":[true,5.04,1.6544041793541437,false,false,-0.0007422987769088523,false,0.2],"torque":0.012293894395048044,"active":true,"saturated":false,"log":[0.0038551599718630314,0.04396424442529678,0.001254302216693759,0.0,-0.07272286713123322,0.012293894775211811,-0.013576014898717403,-0.010892183519899845,-0.10915675759315491]},{"input":[true,5.085,1.7412142952184702,false,false,-0.0007965367979307344,false,0.2],"torque":0.018609207351630207,"active":true,"saturated":false,"log":[0.0029118317179381847,0.032892052084207535,0.0012644936796277761,0.0,-0.07568617910146713,0.01860920712351799,-0.014829286374151707,-0.012796349823474884,-0.11209724843502045]},{"input":[true,5.13,1.8267335970387706,false,false,-0.000849889860011222,false,0.2],"torque":0.025070412857030388,"active":true,"saturated":false,"log":[0.001934335450641811,0.02164134569466114,0.0012712638126686215,0.0,-0.07872331887483597,0.02507041208446026,-0.01610385626554489,-0.014749621972441673,-0.11496872454881668]},{"input":[true,5.175,1.910866654452805,false,false,-0.0009022986874590205,false,0.2],"torque":0.03166886964883861,"active":true,"saturated":false,"log":[0.0009232740267179906,0.010229876264929771,0.0012744952691718936,0.0,-0.08183132857084274,0.03166886791586876,-0.0173981674015522,-0.016750046983361244,-0.11776376515626907]},{"input":[true,5.22,1.993519418305584,false,false,-0.0009537050536352881,false,0.2],"torque":0.03839549958222109,"active":true,"saturated":false,"log":[-0.00012068203068338335,-0.0013241232372820377,0.0012740729143843055,0.0,-0.08500707894563675,0.03839549794793129,-0.01871059462428093,-0.01879553124308586,-0.12047486752271652]},{"input":[true,5.265,2.074599323736675,false,false,-0.001004051845643908,false,0.2],"torque":0.04534692216022512,"active":true,"saturated":false,"log":[-0.0011967939790338278,-0.013001970015466213,0.0012698841746896505,0.0,-0.08848178386688232,0.04534692317247391,-0.020039431750774384,-0.020883850753307343,-0.12309446185827255]},{"input":[true,5.31,2.154015391835651,false,false,-0.0010532831277843902,false,0.2],"torque":0.05257713790020507,"active":true,"saturated":false,"log":[-0.002304254798218608,-0.024784564971923828,0.001261819270439446,0.0,-0.09239115566015244,0.05257713794708252,-0.021382903680205345,-0.02301264926791191,-0.1256149560213089]},{"input":[true,5.3549999999999995,2.2316783297562597,false,false,-0.001101344203696894,false,0.2],"torque":0.05989325408315611,"active":true,"saturated":false,"log":[-0.003442189423367381,-0.03665243461728096,0.0012497715651988983,0.0,-0.0963272824883461,0.059893254190683365,-0.022739164531230927,-0.025179430842399597,-0.1280287355184555]},{"input":[true,5.3999999999999995,1.2741779894635443,false,false,-0.0005,false,0.2],"torque":0.15104854301900952,"active":true,"saturated":false,"log":[-0.021897608414292336,-0.2308007925748825,0.001173130003735423,-0.0,-0.10188137739896774,0.15104854106903076,-0.010497599840164185,-0.02605639398097992,-0.12370223551988602]},{"input":[true,5.444999999999999,1.2729347258380057,false,false,-0.0004988893003505612,false,0.2],"torque":0.1556295783911376,"active":true,"saturated":false,"log":[-0.022877240553498268,-0.23865537345409393,0.0010930596617981791,-0.0,-0.10328049212694168,0.15562957525253296,-0.01064957957714796,-0.02693968452513218,-0.11912231892347336]},{"input":[true,5.49,1.2681553165532318,false,false,-0.0004955621360170897,false,0.2],"torque":0.16047587963426183,"active":true,"saturated":false,"log":[-0.023920947685837746,-0.2469598650932312,0.0010093363234773278,-0.0,-0.10477429628372192,0.16047587990760803,-0.010754129849374294,-0.027822984382510185,-0.11426415294408798]},{"input":[true,5.535,1.2598561987349952,false,false,-0.0004900332889206208,false,0.2],"torque":0.1655104509342499,"active":true,"saturated":false,"log":[-0.025022761896252632,-0.25563254952430725,0.0009217566112056375,-0.0,-0.10627418756484985,0.16551044583320618,-0.010809194296598434,-0.028700094670057297,-0.10910414904356003]},{"input":[true,5.58,1.2480693925288153,false,false,-0.00048232732261528193,false,0.2],"torque":0.17069264988143906,"active":true,"saturated":false,"log":[-0.02617669850587845,-0.26459407806396484,0.0008301382185891271,-0.0,-0.10776978731155396,0.17069265246391296,-0.010812913998961449,-0.029564950615167618,-0.10362005978822708]},{"input":[true,5.625,1.2328423812546692,false,false,-0.00047247847315736883,false,0.2],"torque":0.17598261631013007,"active":true,"saturated":false,"log":[-0.02737673930823803,-0.2737673819065094,0.0007343196193687618,-0.0,-0.1092509925365448,0.17598260939121246,-0.010763649828732014,-0.03041163645684719,-0.09779097139835358]},{"input":[true,5.67,1.2142379233428098,false,false,-0.00046053049700144256,false,0.2],"torque":0.18134129257436551,"active":true,"saturated":false,"log":[-0.028616828843951225,-0.2830776572227478,0.0006341607077047229,-0.0,-0.11070795357227325,0.181341290473938,-0.010659994557499886,-0.031234387308359146,-0.09159744530916214]},{"input":[true,5.715,1.192333796779173,false,false,-0.00044653647659921467,false,0.2],"torque":0.18673042968412193,"active":true,"saturated":false,"log":[-0.029890866950154305,-0.29245224595069885,0.000529542681761086,-0.0,-0.11213114112615585,0.18673042953014374,-0.010500787757337093,-0.03202761709690094,-0.08502151817083359]},{"input":[true,5.76,1.1672224770855621,false,false,-0.00043055858456490516,false,0.2],"torque":0.19211263013472255,"active":true,"saturated":false,"log":[-0.031192708760499954,-0.30182063579559326,0.0004203681892249733,-0.0,-0.11351130157709122,0.19211262464523315,-0.01028512790799141,-0.03278592228889465,-0.07804679870605469]},{"input":[true,5.805,1.13901075015246,false,false,-0.0004126678074548392,false,0.2],"torque":0.1974513836065734,"active":true,"saturated":false,"log":[-0.03251616284251213,-0.311114639043808,0.0003065616183448583,-0.0,-0.11483953148126602,0.1974513828754425,-0.010012384504079819,-0.033504094928503036,-0.07065852731466293]},{"input":[true,5.85,1.1078192615296771,false,false,-0.00039294363038847404,false,0.2],"torque":0.20271112848959563,"active":true,"saturated":false,"log":[-0.03385499492287636,-0.3202682435512543,0.00018806914158631116,-0.0,-0.11610724031925201,0.20271113514900208,-0.009682209230959415,-0.03417715057730675,-0.06284363567829132]},{"input":[true,5.895,1.0737820040608128,false,false,-0.00037147368391202205,false,0.2],"torque":0.20785729667480382,"active":true,"saturated":false,"log":[-0.03520292788743973,-0.329217791557312,6.485889025498182e-05,-0.0,-0.11730622500181198,0.20785729587078094,-0.009294545277953148,-0.03480032458901405,-0.05459083616733551]},{"input":[true,5.9399999999999995,1.0370457460204652,false,false,-0.0003483533546735827,false,0.2],"torque":0.21285639857962613,"active":true,"saturated":false,"log":[-0.036553651094436646,-0.3379019498825073,-6.307889270829037e-05,-0.0,-0.11842864006757736,0.21285639703273773,-0.00884963572025299,-0.03536910563707352,-0.04589065536856651]},{"input":[true,5.984999999999999,0.9977694021770787,false,false,-0.0003236853616394762,false,0.2],"torque":0.21767608165265234,"active":true,"saturated":false,"log":[-0.03790082409977913,-0.3462619185447693,-0.00019573177269194275,-0.0,-0.11946704983711243,0.2176760882139206,-0.008348027244210243,-0.0358792319893837,-0.036735523492097855]},{"input":[true,6.029999999999999,0.9561233504580668,false,false,-0.0002975792997345639,false,0.2],"torque":0.22228521876879653,"active":true,"saturated":false,"log":[-0.039238087832927704,-0.3542414605617523,-0.00033306507975794375,-0.0,-0.12041441351175308,0.2222852259874344,-0.00779058039188385,-0.03632672131061554,-0.02711980976164341]},{"input":[true,6.075,0.9122886971363318,false,false,-0.00027015115293406986,false,0.2],"torque":0.22665399276230871,"active":true,"saturated":false,"log":[-0.0405590757727623,-0.36178696155548096,-0.0004750218358822167,-0.0,-0.12126415222883224,0.22665399312973022,-0.0071784695610404015,-0.03670787811279297,-0.01703990250825882]},{"input":[true,6.12,0.8664564936873613,false,false,-0.00024152277897013832,false,0.2],"torque":0.23075396133501305,"active":true,"saturated":false,"log":[-0.04185742139816284,-0.3688476085662842,-0.0006215228349901736,-0.0,-0.12201011925935745,0.23075395822525024,-0.006513185333460569,-0.03701930120587349,-0.006494251545518637]},{"input":[true,6.165,0.8188269086827802,false,false,-0.0002118213679424723,false,0.2],"torque":0.2345589421673938,"active":true,"saturated":false,"log":[-0.043126773089170456,-0.3753754198551178,-0.0007724665338173509,-0.0,-0.12264831364154816,0.23455893993377686,-0.005796534474939108,-0.037257906049489975,0.004516589920967817]},{"input":[true,6.21,0.7696083582885347,false,false,-0.00018117887723833681,false,0.2],"torque":0.23804411482272636,"active":true,"saturated":false,"log":[-0.04436080902814865,-0.38132551312446594,-0.0009277293574996293,-0.0,-0.12317482382059097,0.2380441129207611,-0.005030639935284853,-0.037420932203531265,0.015989912673830986]},{"input":[true,6.255,0.7190165991229259,false,false,-0.00014973144527246395,true,0.2],"torque":0.24118464454743274,"active":true,"saturated":false,"log":[-0.04555324837565422,-0.3866559863090515,-0.0010871656704694033,-0.0,-0.12358274310827255,0.24118465185165405,-0.004217937123030424,-0.03750595450401306,0.027920817956328392]},{"input":[true,6.3,0.6672737874006298,false,false,-0.00011761878665149471,false,0.2],"torque":0.09418695864918389,"active":true,"saturated":false,"log":[-0.01398501917719841,-0.11719445884227753,-0.0010871656704694033,-0.0,-0.08159582316875458,0.09418696165084839,-0.0033611683174967766,-0.013588285073637962,0.03128357604146004]},{"input":[true,6.345,0.6146075084428586,false,false,-8.498357145012052e-05,false,0.2],"torque":0.09552051487166334,"active":true,"saturated":false,"log":[-0.01453317515552044,-0.120218425989151,-0.0011380318319424987,-0.0,-0.08105523884296417,0.0955205112695694,-0.0024633759167045355,-0.01309844572097063,0.03493175283074379]},{"input":[true,6.39,0.5612497807702759,false,false,-5.197079135667549e-05,false,0.2],"torque":0.09615015334595649,"active":true,"saturated":false,"log":[-0.014941688627004623,-0.12198394536972046,-0.0011903278063982725,-0.0,-0.0802818164229393,0.09615015238523483,-0.001527894870378077,-0.012468397617340088,0.038832224905490875]},{"input":[true,6.435,0.5074360391134829,false,false,-1.872711550426666e-05,false,0.2],"torque":0.0960973539525056,"active":true,"saturated":false,"log":[-0.015210747718811035,-0.12253778427839279,-0.0012435653479769826,-0.0,-0.07928086072206497,0.09609735757112503,-0.0005583419697359204,-0.011701640672981739,0.04295129328966141]},{"input":[true,6.4799999999999995,0.4534041007753739,false,false,1.4599761150644409e-05,false,0.2],"torque":0.09538511091299126,"active":true,"saturated":false,"log":[-0.015340792015194893,-0.1219286173582077,-0.0012972581898793578,-0.0,-0.07805841416120529,0.0953851118683815,0.0004413961141835898,-0.010802171193063259,0.04725473001599312]},{"input":[true,6.5249999999999995,0.39939311985988435,false,false,4.786177400718783e-05,false,0.2],"torque":0.09403798960206936,"active":true,"saturated":false,"log":[-0.015332544222474098,-0.12020714581012726,-0.0013509220443665981,-0.0,-0.07662120461463928,0.0940379872918129,0.00146717659663409,-0.009774498641490936,0.05170784145593643]},{"input":[true,6.569999999999999,0.34564253394228717,false,false,8.091114664128603e-05,false,0.2],"torque":0.0920821849682929,"active":true,"saturated":false,"log":[-0.015187045559287071,-0.11742623895406723,-0.0014040766982361674,-0.0,-0.07497672736644745,0.0920821875333786,0.002514615887776017,-0.008623647503554821,0.05627552792429924]},{"input":[true,6.614999999999999,0.2923910077969087,false,false,0.00011360104734654355,false,0.2],"torque":0.08954553315950968,"active":true,"saturated":false,"log":[-0.01490568183362484,-0.1136409193277359,-0.0014562465948984027,-0.0,-0.07313321530818939,0.08954553306102753,0.0035791059490293264,-0.007355168461799622,0.060922376811504364]},{"input":[true,6.66,0.2398753788187306,false,false,0.0001457862414779463,false,0.2],"torque":0.08645752618813483,"active":true,"saturated":false,"log":[-0.014490213245153427,-0.10890844464302063,-0.0015069623477756977,-0.0,-0.07109968364238739,0.08645752817392349,0.004655834287405014,-0.005975136533379555,0.06561274081468582]},{"input":[true,6.705,0.18832960877569604,false,false,0.0001773237367001388,false,0.2],"torque":0.08284927256948323,"active":true,"saturated":false,"log":[-0.01394279208034277,-0.1032882034778595,-0.001555762137286365,-0.0,-0.06888589262962341,0.08284927159547806,0.005739802494645119,-0.0044901492074131966,0.07031083852052689]},{"input":[true,6.75,0.1379837465086033,false,false,0.0002080734182735712,false,0.2],"torque":0.0787534762012561,"active":true,"saturated":false,"log":[-0.013265987858176231,-0.0968417078256607,-0.0016021931078284979,-0.0,-0.06650236994028091,0.07875347882509232,0.006825848948210478,-0.002907318528741598,0.07498087733983994]},{"input":[true,6.795,0.08906290615530407,false,false,0.0002378986715560677,false,0.2],"torque":0.07420436384195671,"active":true,"saturated":false,"log":[-0.012462800368666649,-0.08963245898485184,-0.001645812881179154,-0.0,-0.06396039575338364,0.07420436292886734,0.007908670231699944,-0.0012342615518718958,0.07958715409040451]},{"input":[true,6.84,0.041786265415665436,false,false,0.00026666698895416686,false,0.2],"torque":0.0692418840397082,"active":true,"saturated":false,"log":[-0.011536672711372375,-0.08172579109668732,-0.0016861911863088608,-0.0,-0.0612720362842083,0.06924188137054443,0.008982846513390541,0.0005209153168834746,0.08409418165683746]},{"input":[true,6.885,-0.003633911706375448,false,false,0.0002942505586276729,false,0.2],"torque":0.0639075836262983,"active":true,"saturated":false,"log":[-0.010491504333913326,-0.07318873703479767,-0.0017229114891961217,-0.0,-0.05845007300376892,0.06390758603811264,0.010042866691946983,0.002349634189158678,0.0884668231010437]},{"input":[true,6.93,-0.046993223347003565,false,false,0.0003205268323319191,false,0.2],"torque":0.058228323282162306,"active":true,"saturated":false,"log":[-0.00933165568858385,-0.0640898123383522,-0.0017555722733959556,-0.0,-0.05550777167081833,0.058228325098752975,0.011083154007792473,0.0042428686283528805,0.09267042577266693]},{"input":[true,6.975,-0.08809604519514846,false,false,0.00034537906987493823,false,0.2],"torque":0.052243286815725605,"active":true,"saturated":false,"log":[-0.008061949163675308,-0.05449877679347992,-0.0017837891355156898,-0.0,-0.05245918035507202,0.05224328860640526,0.012098093517124653,0.006191168446093798,0.0966709554195404]},{"input":[true,7.02,-0.12675640879320416,false,false,0.0003686968577706227,false,0.2],"torque":0.04599270989384754,"active":true,"saturated":false,"log":[-0.00668767374008894,-0.044486407190561295,-0.0018071959493681788,-0.0,-0.049318961799144745,0.04599270969629288,0.01308206096291542,0.008184690959751606,0.10043515264987946]},{"input":[true,7.0649999999999995,-0.16279883880113116,false,false,0.0003903765997836073,false,0.2],"torque":0.039517721949016156,"active":true,"saturated":false,"log":[-0.005214580800384283,-0.0341242179274559,-0.0018254469614475965,-0.0,-0.046102363616228104,0.03951772302389145,0.014029448851943016,0.010213231667876244,0.10393065214157104]},{"input":[true,7.109999999999999,-0.19605914547226583,false,false,0.0004103219771864765,false,0.2],"torque":0.03286017234014234,"active":true,"saturated":false,"log":[-0.0036488757468760014,-0.023484164848923683,-0.0018382180714979768,-0.0,-0.04282514750957489,0.0328601710498333,0.014934699051082134,0.012266261503100395,0.10712617635726929]},{"input":[true,7.154999999999999,-0.22638516877897236,false,false,0.00042844437668447364,false,0.2],"torque":0.026062458677835146,"active":true,"saturated":false,"log":[-0.0019972107838839293,-0.012638350017368793,-0.001845208345912397,-0.0,-0.0395035482943058,0.026062458753585815,0.015792330726981163,0.014332967810332775,0.10999161750078201]},{"input":[true,7.199999999999999,-0.25363747083197946,false,false,0.0004446632841065207,false,0.2],"torque":0.019167335012547807,"active":true,"saturated":false,"log":[-0.00026667152997106314,-0.0016586969140917063,-0.0018461416475474834,-0.0,-0.03615422174334526,0.019167335703969002,0.016596969217061996,0.016402292996644974,0.11249824613332748]},{"input":[true,7.245,-0.27768997345805607,false,false,0.0004589066421134661,false,0.2],"torque":0.012217715929294697,"active":true,"saturated":false,"log":[0.0015352378832176328,0.009383373893797398,-0.0018407682655379176,-0.0,-0.03279418125748634,0.012217716313898563,0.017343375831842422,0.018462978303432465,0.11461883783340454]},{"input":[true,7.29,-0.2984305380355871,false,false,0.000471111170334329,false,0.2],"torque":0.005256470499720552,"active":true,"saturated":false,"log":[0.0034006135538220406,0.020417284220457077,-0.0018288661958649755,-0.0,-0.029440730810165405,0.005256470292806625,0.018026480451226234,0.02050361968576908,0.11632779985666275]},{"input":[true,7.335,-0.3157614849356846,false,false,0.00048122264650824387,false,0.2],"torque":-0.001673787022383149,"active":true,"saturated":false,"log":[0.005321172531694174,0.031373634934425354,-0.0018102420726791024,-0.0,-0.02611139789223671,-0.001673787017352879,0.018641412258148193,0.022512705996632576,0.11760133504867554]},{"input":[true,7.38,-0.32960005017657396,false,false,0.0004891961473830436,false,0.2],"torque":-0.008530913378065061,"active":true,"saturated":false,"log":[0.0072882771492004395,0.04218454658985138,-0.0017847331473603845,-0.0,-0.022823862731456757,-0.008530912920832634,0.01918351836502552,0.024478677660226822,0.11841755360364914]},{"input":[true,7.425,-0.33987877717011555,false,false,0.0004949962483002228,false,0.2],"torque":-0.015273452636544976,"active":true,"saturated":false,"log":[0.009292961098253727,0.0527840182185173,-0.0017522077541798353,-0.0,-0.019595883786678314,-0.01527345273643732,0.01964840665459633,0.02638997882604599,0.11875664442777634]},{"input":[true,7.47,-0.34654584172017283,false,false,0.0004985971805795595,false,0.2],"torque":-0.021860856755310864,"active":true,"saturated":false,"log":[0.011325959116220474,0.06310824304819107,-0.0017125668236985803,-0.0,-0.01644521951675415,-0.02186085656285286,0.020031964406371117,0.02823510952293873,0.11860097199678421]},{"input":[true,7.515,-0.3495653087220409,false,false,0.0004999829460041724,false,0.2],"torque":-0.02835650437818685,"active":true,"saturated":false,"log":[0.013361530378460884,0.07332807779312134,-0.0016658015083521605,-0.0,-0.013410424813628197,-0.028356503695249557,0.020330388098955154,0.030002683401107788,0.1179351955652237]},{"input":[true,7.56,-0.3489173193089434,false,false,0.0004991473878973766,false,0.2],"torque":-0.03488736020010047,"active":true,"saturated":false,"log":[0.015362853184342384,0.08375827223062515,-0.001612031483091414,-0.0,-0.01054386142641306,-0.03488735854625702,0.020540211349725723,0.03168148547410965,0.11674638837575912]},{"input":[true,7.6049999999999995,-0.34459820749448405,false,false,0.000496094218475557,false,0.2],"torque":-0.041241540350587344,"active":true,"saturated":false,"log":[0.017346467822790146,0.09394846856594086,-0.0015513189136981964,-0.0,-0.007828623056411743,-0.041241541504859924,0.020658325403928757,0.033260527998209,0.115024134516716]},{"input":[true,7.6499999999999995,-0.3366205456675651,false,false,0.0004908370023555395,false,0.2],"torque":-0.04738233216935349,"active":true,"saturated":false,"log":[0.01930171623826027,0.10384323447942734,-0.0014837628696113825,-0.0,-0.005282318219542503,-0.047382332384586334,0.02068200707435608,0.03472910448908806,0.11276064068078995]},{"input":[true,7.694999999999999,-0.32501311860729704,false,false,0.00048339909628973055,false,0.2],"torque":-0.05327388458695753,"active":true,"saturated":false,"log":[0.021217919886112213,0.1133885607123375,-0.0014095001388341188,-0.0,-0.0029220045544207096,-0.05327388644218445,0.020608937367796898,0.0360768586397171,0.10995080322027206]},{"input":[true,7.739999999999999,-0.30982082599852284,false,false,0.00047381354539676926,false,0.2],"torque":-0.058881387061269004,"active":true,"saturated":false,"log":[0.023084424436092377,0.12253212183713913,-0.001328704645857215,-0.0,-0.0007640996482223272,-0.05888138711452484,0.020437223836779594,0.03729383274912834,0.10659226030111313]},{"input":[true,7.785,-0.29110451374236596,false,false,0.0004621229363487218,false,0.2],"torque":-0.06417127714118143,"active":true,"saturated":false,"log":[0.024890657514333725,0.13122354447841644,-0.0012415873352438211,-0.0,0.0011757143074646592,-0.06417127698659897,0.020165419206023216,0.038370516151189804,0.10268551111221313]},{"input":[true,7.83,-0.26894073466930096,false,false,0.00044837920816707353,false,0.2],"torque":-0.06911137908261242,"active":true,"saturated":false,"log":[0.026626162230968475,0.13941457867622375,-0.0011483958223834634,-0.0,0.002882578643038869,-0.06911137700080872,0.01979253813624382,0.0392979234457016,0.09823388606309891]},{"input":[true,7.875,-0.24342143957332968,false,false,0.00043264342146811797,false,0.2],"torque":-0.0736711275050033,"active":true,"saturated":false,"log":[0.02828066609799862,0.14705947041511536,-0.0010494134621694684,-0.0,0.0043425471521914005,-0.0736711248755455,0.019318070262670517,0.040067631751298904,0.09324366599321365]},{"input":[true,7.92,-0.21465359979347237,false,false,0.00041498548718293574,false,0.2],"torque":-0.0778216828831904,"active":true,"saturated":false,"log":[0.02984411083161831,0.1541149914264679,-0.0009449591161683202,-0.0,0.00554266944527626,-0.07782167941331863,0.018741993233561516,0.040671832859516144,0.08772403001785278]},{"input":[true,7.965,-0.1827587628717004,false,false,0.00039548385595720845,false,0.2],"torque":-0.08153613018679724,"active":true,"saturated":false,"log":[0.03130671754479408,0.1605408489704132,-0.0008353855810128152,0.0,0.006471073720604181,-0.08153612911701202,0.01806478574872017,0.04110339283943176,0.08168711513280869]},{"input":[true,8.0,-0.14774276023607597,false,false,0.00037422516961079937,false,0.2],"torque":-0.08481554244183247,"active":true,"saturated":false,"log":[0.03263933211565018,0.16646058857440948,-0.0007211479241959751,0.0,0.007033736910670996,-0.08481553941965103,0.01724429614841938,0.041290611028671265,0.0751500129699707]},{"input":[true,8.0,-0.10947208051268731,false,false,0.0003513038762055989,false,0.2],"torque":-0.0876225592996107,"active":true,"saturated":false,"log":[0.033747099339962006,0.1721101999282837,-0.0006030330550856888,0.0,0.006985884625464678,-0.08762256056070328,0.016188083216547966,0.04105051979422569,0.06815039366483688]},{"input":[true,8.0,-0.06859544752202096,false,false,0.00032682181043180595,false,0.2],"torque":-0.08980936115896528,"active":true,"saturated":false,"log":[0.034672461450099945,0.17682954668998718,-0.00048167945351451635,0.0,0.0066033657640218735,-0.0898093581199646,0.015059949830174446,0.0406041257083416,0.060728371143341064]},{"input":[true,8.0,-0.02529446791180917,false,false,0.0003008877411769109,false,0.2],"torque":-0.091373357334596,"active":true,"saturated":false,"log":[0.035413581877946854,0.18060927093029022,-0.0003577319148462266,0.0,0.005890911910682917,-0.09137335419654846,0.013864907436072826,0.03995508700609207,0.05292530730366707]},{"input":[true,8.0,0.020238480786455182,false,false,0.00027361688828743294,false,0.2],"torque":-0.09231426628153822,"active":true,"saturated":false,"log":[0.03596928343176842,0.1834433525800705,-0.0002318394253961742,0.0,0.004854514729231596,-0.09231426566839218,0.012608266435563564,0.03910784795880318,0.04478366672992706]},{"input":[true,8.0,0.06780110485214266,false,false,0.0002451304106703497,false,0.2],"torque":-0.09263417168338055,"active":true,"saturated":false,"log":[0.03633907437324524,0.18532927334308624,-0.00010465266677783802,0.0,0.0035014115273952484,-0.0926341712474823,0.01129561010748148,0.03806762397289276,0.03634687885642052]},{"input":[true,8.0,0.11718209312574906,false,false,0.00021555486800847306,false,0.2],"torque":-0.0923374720159838,"active":true,"saturated":false,"log":[0.036523133516311646,0.18626798689365387,2.3178297851700336e-05,0.0,0.001840062439441681,-0.0923374742269516,0.009932768531143665,0.03684038668870926,0.027659164741635323]},{"input":[false,8.0,0.1681620558218167,false,false,0.00018502165848126905,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.22051449923356664,false,false,0.0001536664349892097,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.2740068319988187,false,false,0.00012162850247525857,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.32840139845657335,false,false,8.905019902106331e-05,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.3834565345032466,false,false,5.6076263467527434e-05,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.43892764125761197,false,false,2.2853192369290225e-05,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.49456827176436347,false,false,-1.0471410859959058e-05,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.5501312259083084,false,false,-4.37494917197232e-05,true,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.6053696486746889,false,false,-7.683320239902244e-05,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.6600381268762969,false,false,-0.00010957555863444268,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.7138937794748355,false,false,-0.00014183109273161314,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.7666973366524594,false,false,-0.00017345649984887746,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.8182142028393842,false,false,-0.00020431127467184204,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.8682154989747521,false,false,-0.00023425833565018857,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.9164790793702082,false,false,-0.00026316463402339594,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,8.0,0.9627905186584484,false,false,-0.00029090174492958534,false,0.2],"torque":-0.09615630590949055,"active":true,"saturated":false,"log":[0.04282383620738983,0.21840156614780426,0.00014988343173172325,0.0,-0.022499455139040947,-0.09615630656480789,-0.013404752127826214,0.018144769594073296,-0.013859129510819912]},{"input":[true,8.0,1.0069440644419436,false,false,-0.00031734643797131734,false,0.2],"torque":-0.08254415824223486,"active":true,"saturated":false,"log":[0.03872328996658325,0.19748878479003906,0.0002854149497579783,0.0,-0.029451370239257812,-0.08254415541887283,-0.014623323455452919,0.013905210420489311,-0.026758024469017982]},{"input":[true,8.0,1.0487435514094,false,false,-0.00034238122470342736,false,0.2],"torque":-0.06914498279939592,"active":true,"saturated":false,"log":[0.03467755764722824,0.17685554921627045,0.0004067863919772208,0.0,-0.03624369949102402,-0.06914497911930084,-0.015776926651597023,0.009771003387868404,-0.03870238736271858]},{"input":[true,8.0,1.088003272858732,false,false,-0.0003658948806105272,false,0.2],"torque":-0.05596832240139254,"active":true,"saturated":false,"log":[0.030688712373375893,0.15651243925094604,0.0005141968722455204,0.0,-0.042866773903369904,-0.05596832185983658,-0.016860436648130417,0.005748799070715904,-0.04969773441553116]},{"input":[true,8.0,1.1245488057545368,false,false,-0.00038778293925512483,false,0.2],"torque":-0.04302623538476577,"active":true,"saturated":false,"log":[0.02675968036055565,0.1364743709564209,0.0006078557926230133,0.0,-0.04931021109223366,-0.043026234954595566,-0.01786903664469719,0.0018455703975632787,-0.059749703854322433]},{"input":[true,8.0,1.1582177856545102,false,false,-0.0004079481564009665,false,0.2],"torque":-0.030337079621533803,"active":true,"saturated":false,"log":[0.02289417013525963,0.1167602688074112,0.0006879853899590671,0.0,-0.05556182935833931,-0.030337080359458923,-0.01879825070500374,-0.0019314734963700175,-0.06886430084705353]},{"input":[true,8.0,1.1888606280619647,false,false,-0.00042630094204959286,false,0.2],"torque":-0.017912326218102317,"active":true,"saturated":false,"log":[0.019096562638878822,0.09739246964454651,0.0007548233261331916,0.0,-0.06161091849207878,-0.017912326380610466,-0.01964394748210907,-0.0055749742314219475,-0.07704813033342361]},{"input":[true,8.0,1.2163411929996208,false,false,-0.00044275975847065947,false,0.2],"torque":-0.005765072464830704,"active":true,"saturated":false,"log":[0.0153718376532197,0.07839637249708176,0.0008086247835308313,0.0,-0.06744708120822906,-0.005765072535723448,-0.020402370020747185,-0.009077505208551884,-0.08430864661931992]},{"input":[true,8.0,1.2405373898521093,false,false,-0.00045725148245765016,false,0.2],"torque":0.006084863143098046,"active":true,"saturated":false,"log":[0.011725487187504768,0.059799984097480774,0.0008496639784425497,0.0,-0.07305841892957687,0.006084863096475601,-0.021070148795843124,-0.012431653216481209,-0.09065432846546173]},{"input":[true,8.0,1.2613417197899819,false,false,-0.00046971173019955657,false,0.2],"torque":0.017616540746495303,"active":true,"saturated":false,"log":[0.00816342607140541,0.041633471846580505,0.0008782359655015171,0.0,-0.07843299955129623,0.01761654019355774,-0.021644316613674164,-0.015630090609192848,-0.09609486162662506]},{"input":[true,8.0,1.2786617533653635,false,false,-0.000480085143325183,false,0.2],"torque":0.028808145743928436,"active":true,"saturated":false,"log":[0.004691918846219778,0.02392878569662571,0.0008946576854214072,0.0,-0.08355896919965744,0.028808144852519035,-0.022122323513031006,-0.018665654584765434,-0.10064129531383514]},{"input":[true,8.0,1.2924205411573724,false,false,-0.0004883256348492412,false,0.2],"torque":0.03963725933332303,"active":true,"saturated":false,"log":[0.0013174967607483268,0.006719233468174934,0.0008992688963189721,0.0,-0.08842471987009048,0.039637260138988495,-0.022502044215798378,-0.02153140865266323,-0.1043061912059784]},{"input":[true,8.0,1.3025569556428997,false,false,-0.0004943965939275431,false,0.2],"torque":0.05008110687132522,"active":true,"saturated":false,"log":[-0.0019531168509274721,-0.009960896335542202,0.0008924329886212945,0.0,-0.09301897138357162,0.05008110776543617,-0.022781794890761375,-0.02422071062028408,-0.10710371285676956]},{"input":[true,8.0,1.3090259627738827,false,false,-0.0004982710485116088,false,0.2],"torque":0.06011680083794973,"active":true,"saturated":false,"log":[-0.0051130796782672405,-0.026076706126332283,0.0008745372178964317,0.0,-0.09733092784881592,0.06011680141091347,-0.022960329428315163,-0.026727279648184776,-0.10904976725578308]},{"input":[true,8.0,1.3117988220545105,false,false,-0.0004999317851800449,false,0.2],"torque":0.06972157890143084,"active":true,"saturated":false,"log":[-0.008155502378940582,-0.04159306362271309,0.0008459929958917201,0.0,-0.1013503298163414,0.06972157955169678,-0.023036856204271317,-0.029045244678854942,-0.11016204208135605]},{"input":[true,8.0,1.31086321422946,false,false,-0.0004993714256143098,false,0.2],"torque":0.07887303000785648,"active":true,"saturated":false,"log":[-0.011073516681790352,-0.05647493526339531,0.0008072356577031314,0.0,-0.10506762564182281,0.07887303084135056,-0.023011034354567528,-0.031169205904006958,-0.1104600802063942]},{"input":[true,8.0,1.3062232960158637,false,false,-0.0004965924593790963,false,0.2],"torque":0.08754930958191252,"active":true,"saturated":false,"log":[-0.013860340230166912,-0.07068773359060287,0.0007587244617752731,0.0,-0.108474001288414,0.08754930645227432,-0.02288297936320305,-0.03309428319334984,-0.10996532440185547]},{"input":[true,8.0,1.2978996816358557,false,false,-0.0004916072328616975,false,0.2],"torque":0.09572934609934963,"active":true,"saturated":false,"log":[-0.016509339213371277,-0.08419763296842575,0.0007009417749941349,0.0,-0.11156152188777924,0.09572934359312057,-0.022653261199593544,-0.03481615334749222,-0.10870116949081421]},{"input":[true,8.0,1.2859293512317311,false,false,-0.0004844378944194944,false,0.2],"torque":0.10339304713334893,"active":true,"saturated":false,"log":[-0.019014092162251472,-0.09697186946868896,0.0006343924324028194,0.0,-0.11432316899299622,0.10339304804801941,-0.022322898730635643,-0.036331113427877426,-0.10669290274381638]},{"input":[true,8.0,1.270365486570624,false,false,-0.0004751162959792648,false,0.2],"torque":0.11052147246205116,"active":true,"saturated":false,"log":[-0.021368442103266716,-0.10897905379533768,0.0005596029222942889,0.0,-0.11675293743610382,0.11052147299051285,-0.02189335785806179,-0.03763609007000923,-0.10396776348352432]}]},{"name":"city","frames":[{"input":[false,8.0,2.701598189824963,false,false,-0.0013323183357528668,true,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.633013100623514,false,false,-0.001291240963619141,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.5620327571227923,false,false,-0.0012487290121424167,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.4887360190688987,false,false,-0.0012048297124508391,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.413204319741355,false,false,-0.0011595918370278437,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.335521575480063,false,false,-0.0011130656455255266,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.255774092453557,false,false,-0.0010653028289257553,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.17405047077214,false,false,-0.0010163564521110636,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.0904415060524353,false,false,-0.0009662808949091303,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.005040088542704,false,false,-0.000915131791676342,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.9179410999210167,false,false,-0.0008629659694875654,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.8292413078809286,false,false,-0.0008098413850007994,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.7390392586217738,false,false,-0.0007558170600668505,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.6474351673630265,false,false,-0.000700953016155571,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.5545308070043673,false,false,-0.0006453102076715109,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.4604293950551532,false,false,-0.000588950454233074,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.365235478958916,false,false,-0.0005319363719904111,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.269054819940291,false,false,-0.0004743313040583631,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.1719942755034232,false,false,-0.0004161992501417374,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.074161680712405,false,false,-0.00035760479543111017,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,8.0,0.9756657283856285,false,false,-0.000298613038848147,false,0.12],"torque":0.23037608200336213,"active":true,"saturated":false,"log":[-0.06119755282998085,-0.31210753321647644,-0.00021419143013190478,-0.0,-0.15658093988895416,0.23037607967853546,-0.013760088011622429,-0.058846041560173035,0.02496611699461937]},{"input":[true,8.0,0.8766158483371702,false,false,-0.00023928952072016532,false,0.12],"torque":0.21423191168730019,"active":true,"saturated":false,"log":[-0.05663348361849785,-0.2888307571411133,-0.00041240864084102213,-0.0,-0.14692990481853485,0.21423190832138062,-0.01102646067738533,-0.052749935537576675,0.048487477004528046]},{"input":[true,8.0,0.7771220857993668,false,false,-0.00017970014996429066,false,0.12],"torque":0.19856235006959763,"active":true,"saturated":false,"log":[-0.05222111940383911,-0.26632770895957947,-0.0005951825296506286,-0.0,-0.13745993375778198,0.198562353849411,-0.00828058272600174,-0.04675334692001343,0.0706373006105423]},{"input":[true,8.0,0.6772949791616554,false,false,-0.00011991113086210739,false,0.12],"torque":0.18333569046479845,"active":true,"saturated":false,"log":[-0.04795031249523163,-0.24454659223556519,-0.0007630086620338261,-0.0,-0.128160759806633,0.18333569169044495,-0.00552550470456481,-0.04085184633731842,0.09148335456848145]},{"input":[true,8.0,0.5772454371615132,false,false,-5.998888950615651e-05,false,0.12],"torque":0.16852327896087801,"active":true,"saturated":false,"log":[-0.04381181672215462,-0.2234402596950531,-0.00091634999262169,-0.0,-0.1190234124660492,0.16852328181266785,-0.0027642876375466585,-0.0350416824221611,0.11108840256929398]},{"input":[true,8.0,0.4770846156639367,false,false,0.0,false,0.12],"torque":0.15409929747196996,"active":true,"saturated":false,"log":[-0.03979722037911415,-0.20296582579612732,-0.0010556402849033475,-0.0,-0.11004021763801575,0.15409930050373077,3.714815399735727e-10,-0.029319729655981064,0.1295105367898941]},{"input":[true,8.0,0.3769237941663601,false,false,5.998888950615651e-05,false,0.12],"torque":0.13988929111546006,"active":true,"saturated":false,"log":[-0.035898882895708084,-0.18308430910110474,-0.0011812864104285836,-0.0,-0.10089629888534546,0.13988928496837616,0.0027642883360385895,-0.023683425039052963,0.14680355787277222]},{"input":[true,8.0,0.27687425216621797,false,false,0.00011991113086210739,false,0.12],"torque":0.12392304874006616,"active":true,"saturated":false,"log":[-0.032109882682561874,-0.16376040875911713,-0.0012936709681525826,-0.0,-0.08761133998632431,0.12392304837703705,0.005525505170226097,-0.018130745738744736,0.1630173623561859]},{"input":[true,8.0,0.17704714552850653,false,false,0.00017970014996429066,false,0.12],"torque":0.10842555781927404,"active":true,"saturated":false,"log":[-0.028423966839909554,-0.14496223628520966,-0.001393154845573008,-0.0,-0.07475360482931137,0.1084255576133728,0.008280583657324314,-0.012660151347517967,0.17819814383983612]},{"input":[true,8.0,0.07755338299070308,false,false,0.00023928952072016532,false,0.12],"torque":0.09337454443068954,"active":true,"saturated":false,"log":[-0.024835504591464996,-0.12666107714176178,-0.0014800791395828128,-0.0,-0.06230796128511429,0.09337454289197922,0.011026461608707905,-0.00727055175229907,0.19238875806331635]},{"input":[true,8.0,-0.021496497057755193,false,false,0.000298613038848147,false,0.12],"torque":0.07875016700782725,"active":true,"saturated":false,"log":[-0.021339435130357742,-0.10883112251758575,-0.0015547671355307102,-0.0,-0.05026114359498024,0.07875016331672668,0.013760088942945004,-0.001961271045729518,0.20562900602817535]},{"input":[true,8.0,-0.11999244938453169,false,false,0.00035760479543111017,false,0.12],"torque":0.06454704781631186,"active":true,"saturated":false,"log":[-0.017931228503584862,-0.09144926816225052,-0.0016175264026969671,-0.0,-0.03860129788517952,0.06454704701900482,0.01647842861711979,0.0032679904252290726,0.2179558128118515]},{"input":[true,8.0,-0.21782504417555,false,false,0.0004161992501417374,false,0.12],"torque":0.05073736578211258,"active":true,"saturated":false,"log":[-0.014606849290430546,-0.07449492812156677,-0.0016686504241079092,-0.0,-0.027317890897393227,0.05073736608028412,0.01917846128344536,0.00841718539595604,0.22940349578857422]},{"input":[true,8.0,-0.3148855886124174,false,false,0.0004743313040583631,false,0.12],"torque":0.037298984383388314,"active":true,"saturated":false,"log":[-0.011362718418240547,-0.057949863374233246,-0.0017084198771044612,-0.0,-0.01640232279896736,0.037298984825611115,0.021857187151908875,0.013485953211784363,0.2400040179491043]},{"input":[true,8.0,-0.41106624763104266,false,false,0.0005319363719904111,false,0.12],"torque":0.024220408918260678,"active":true,"saturated":false,"log":[-0.008195674978196621,-0.0417979434132576,-0.0017371047288179398,-0.0,-0.005847098305821419,0.024220408871769905,0.024511627852916718,0.01847364380955696,0.2497871220111847]},{"input":[true,8.0,-0.5062601637272799,false,false,0.000588950454233074,false,0.12],"torque":0.01149160748652583,"active":true,"saturated":false,"log":[-0.005102947354316711,-0.02602503076195717,-0.0017549650510773063,-0.0,0.004354292061179876,0.011491607874631882,0.027138836681842804,0.023379351943731308,0.2587805390357971]},{"input":[true,8.0,-0.6003615756764937,false,false,0.0006453102076715109,false,0.12],"torque":-0.0008961143939850674,"active":true,"saturated":false,"log":[-0.002082123653963208,-0.010618831031024456,-0.0017622525338083506,-0.0,0.014207486063241959,-0.0008961143903434277,0.02973589487373829,0.028201935812830925,0.2670102119445801]},{"input":[true,8.0,-0.6932659360351534,false,false,0.000700953016155571,false,0.12],"torque":-0.012950235826745066,"active":true,"saturated":false,"log":[0.0008688793750479817,0.004431284964084625,-0.0017592114163562655,-0.0,0.023717345669865608,-0.012950235977768898,0.032299917191267014,0.03294004499912262,0.27450037002563477]},{"input":[true,8.0,-0.7848700272939004,false,false,0.0007558170600668505,false,0.12],"torque":-0.024677119114251137,"active":true,"saturated":false,"log":[0.003751840442419052,0.019134385511279106,-0.001746080000884831,-0.0,0.03288804367184639,-0.02467711828649044,0.034828051924705505,0.03759213536977768,0.2812737226486206]},{"input":[true,8.0,-0.8750720765530553,false,false,0.0008098413850007994,false,0.12],"torque":-0.036082107883486515,"active":true,"saturated":false,"log":[0.006568259559571743,0.0334981232881546,-0.0017230911180377007,-0.0,0.04172312468290329,-0.036082107573747635,0.03731749206781387,0.042156513780355453,0.28735166788101196]},{"input":[true,8.0,-0.9637718685931431,false,false,0.0008629659694875654,false,0.12],"torque":-0.04716961386546678,"active":true,"saturated":false,"log":[0.009319380857050419,0.047528840601444244,-0.001690473291091621,-0.0,0.050225596874952316,-0.04716961458325386,0.03976547345519066,0.04663132131099701,0.29275426268577576]},{"input":[true,8.0,-1.0508708572148304,false,false,0.000915131791676342,false,0.12],"torque":-0.05794319700000822,"active":true,"saturated":false,"log":[0.012006213888525963,0.061231691390275955,-0.0016484515508636832,-0.0,0.05839795991778374,-0.0579431988298893,0.042169272899627686,0.05101458728313446,0.29750049114227295]},{"input":[true,8.0,-1.1362722747245617,false,false,0.0009662808949091303,false,0.12],"torque":-0.06840564336817072,"active":true,"saturated":false,"log":[0.014629554934799671,0.07461073249578476,-0.0015972480177879333,-0.0,0.0662422850728035,-0.0684056431055069,0.044526223093271255,0.05530422925949097,0.30160826444625854]},{"input":[true,8.0,-1.2198812394442669,false,false,0.0010163564521110636,false,0.12],"torque":-0.07855903479063826,"active":true,"saturated":false,"log":[0.017190005630254745,0.08766902983188629,-0.0015370830660685897,-0.0,0.07376023381948471,-0.0785590335726738,0.04683370515704155,0.05949806421995163,0.3050946593284607]},{"input":[true,8.0,-1.3016048611256836,false,false,0.0010653028289257553,false,0.12],"torque":-0.08840482187654966,"active":true,"saturated":false,"log":[0.019687993451952934,0.10040877014398575,-0.0014681750908493996,-0.0,0.08095314353704453,-0.0884048193693161,0.049089156091213226,0.06359384953975677,0.3079758286476135]},{"input":[true,8.0,-1.38135234415219,false,false,0.0011130656455255266,false,0.12],"torque":-0.09794386578785541,"active":true,"saturated":false,"log":[0.022123780101537704,0.11283127963542938,-0.0013907417887821794,-0.0,0.08782204240560532,-0.09794386476278305,0.05129006505012512,0.06758927553892136,0.31026726961135864]},{"input":[true,8.0,-1.459035088413482,false,false,0.0011595918370278437,false,0.12],"torque":-0.10717652597690269,"active":true,"saturated":false,"log":[0.024497488513588905,0.12493719160556793,-0.0013050006236881018,-0.0,0.09436769783496857,-0.1071765273809433,0.053433991968631744,0.07148198038339615,0.31198376417160034]},{"input":[true,8.0,-1.5345667877410256,false,false,0.0012048297124508391,false,0.12],"torque":-0.11610270065268886,"active":true,"saturated":false,"log":[0.02680911123752594,0.1367264688014984,-0.001211168710142374,-0.0,0.10059065371751785,-0.11610270291566849,0.05551855266094208,0.07526957988739014,0.31313955783843994]},{"input":[true,8.0,-1.6078635257949194,false,false,0.0012487290121424167,false,0.12],"torque":-0.12472187841220997,"active":true,"saturated":false,"log":[0.02905852347612381,0.1481984704732895,-0.0011094638612121344,-0.0,0.10649127513170242,-0.12472187727689743,0.05754143372178078,0.07894966006278992,0.31374841928482056]},{"input":[true,8.0,-1.6788438692956407,false,false,0.001291240963619141,false,0.12],"torque":-0.13303319410473835,"active":true,"saturated":false,"log":[0.03124549798667431,0.15935203433036804,-0.0010001047048717737,-0.0,0.11206977069377899,-0.13303320109844208,0.05950038507580757,0.08251982182264328,0.31382355093955994]},{"input":[true,8.0,-1.7474289584970901,false,false,0.0013323183357528668,false,0.12],"torque":-0.1410354936466207,"active":true,"saturated":false,"log":[0.033369723707437515,0.17018559575080872,-0.000883310625795275,-0.0,0.11732624471187592,-0.14103549718856812,0.06139322742819786,0.08597764372825623,0.31337791681289673]},{"input":[true,8.0,-1.8135425948004162,false,false,0.0013719154912449113,false,0.12],"torque":-0.14872734699712364,"active":true,"saturated":false,"log":[0.03543080389499664,0.18069709837436676,-0.0007593028130941093,-0.0,0.1222606971859932,-0.14872734248638153,0.06321786344051361,0.08932073414325714,0.31242403388023376]},{"input":[true,8.0,-1.8771113254112903,false,false,0.0014099884373294702,false,0.12],"torque":-0.15610711212635345,"active":true,"saturated":false,"log":[0.037428274750709534,0.19088420271873474,-0.0006283038528636098,-0.0,0.12687310576438904,-0.1561071127653122,0.06497226655483246,0.0925467237830162,0.31097421050071716]},{"input":[true,8.0,-1.938064524946561,false,false,0.0014464948746499401,true,0.12],"torque":-0.16317298454450238,"active":true,"saturated":false,"log":[0.0393616184592247,0.20074425637722015,-0.0004905381938442588,-0.0,0.13116338849067688,-0.16317299008369446,0.06665448099374771,0.09565329551696777,0.30904048681259155]},{"input":[true,8.0,-1.9963344738996398,false,false,0.0014813942442538548,false,0.12],"torque":-0.1699230276565619,"active":true,"saturated":false,"log":[0.04123026877641678,0.2102743685245514,-0.00034623226383700967,-0.0,0.13513149321079254,-0.16992302238941193,0.06826264411211014,0.0986381396651268,0.30663469433784485]},{"input":[true,8.0,-2.051856433877431,false,false,0.0015146477726542137,false,0.12],"torque":-0.17635519791939414,"active":true,"saturated":false,"log":[0.04303361475467682,0.21947143971920013,-0.00019561460067052394,-0.0,0.13877739012241364,-0.17635519802570343,0.06979496777057648,0.101499043405056,0.3037685751914978]},{"input":[true,8.0,-2.104568719525216,false,false,0.0015462185149071458,false,0.12],"torque":-0.18246742563546534,"active":true,"saturated":false,"log":[0.04477102681994438,0.22833223640918732,-3.8916008634259924e-05,-0.0,0.14210109412670135,-0.18246743083000183,0.07124975323677063,0.10423382371664047,0.30045366287231445]},{"input":[true,8.0,-2.1544127670595925,false,false,0.0015760713956580457,false,0.12],"torque":-0.18825759220551505,"active":true,"saturated":false,"log":[0.04644184187054634,0.23685339093208313,0.00012363043788354844,-0.0,0.14510270953178406,-0.1882575899362564,0.07262536883354187,0.10684037953615189,0.29670146107673645]},{"input":[true,8.0,-2.2013331993333227,false,false,0.0016041732481105837,false,0.12],"torque":-0.19372360034923292,"active":true,"saturated":false,"log":[0.048045385628938675,0.2450314611196518,0.0002917892998084426,-0.0,0.14778243005275726,-0.19372360408306122,0.07392030209302902,0.10931668430566788,0.29252341389656067]},{"input":[true,8.0,-2.245277887359793,false,false,0.001630492850875286,false,0.12],"torque":-0.19886338749648885,"active":true,"saturated":false,"log":[0.04958097264170647,0.25286296010017395,0.0004653226933442056,-0.0,0.15014059841632843,-0.19886338710784912,0.07513310760259628,0.11166080832481384,0.2879308760166168]},{"input":[true,8.0,-2.2861980082287556,false,false,0.0016550009626567592,false,0.12],"torque":-0.20367498528791372,"active":true,"saturated":false,"log":[0.051047924906015396,0.26034441590309143,0.0006439904100261629,-0.0,0.15217764675617218,-0.20367498695850372,0.0762624442577362,0.11387088149785995,0.2829352021217346]},{"input":[true,8.0,-2.3240480993489774,false,false,0.0016776703547410072,false,0.12],"torque":-0.2081564939858639,"active":true,"saturated":false,"log":[0.052445556968450546,0.2674723267555237,0.0008275498985312879,-0.0,0.15389421582221985,-0.2081564962863922,0.07730705291032791,0.11594516038894653,0.27754783630371094]},{"input":[true,8.0,-2.358786108957558,false,false,0.0016984758412467596,false,0.12],"torque":-0.21230615909247585,"active":true,"saturated":false,"log":[0.05377320200204849,0.2742433249950409,0.0010157560463994741,-0.0,0.1552910953760147,-0.2123061567544937,0.07826576381921768,0.11788199096918106,0.2717801630496979]},{"input":[true,8.0,-2.3903734428397776,false,false,0.0017173943071071896,false,0.12],"torque":-0.21612237214604516,"active":true,"saturated":false,"log":[0.055030208081007004,0.28065407276153564,0.0012083618203178048,-0.0,0.15636928379535675,-0.21612237393856049,0.07913752645254135,0.11967983096837997,0.2656436562538147]},{"input":[true,8.0,-2.418775007207595,false,false,0.0017344047337509473,false,0.12],"torque":-0.21960368928557863,"active":true,"saturated":false,"log":[0.05621594190597534,0.2867012917995453,0.001405117567628622,-0.0,0.1571299433708191,-0.21960368752479553,0.07992137223482132,0.12133723497390747,0.2591497600078583]},{"input":[true,8.0,-2.443959247689133,false,false,0.0017494882224539628,false,0.12],"torque":-0.22274884880292686,"active":true,"saturated":false,"log":[0.05732979252934456,0.2923819422721863,0.0016057718312367797,-0.0,0.15757451951503754,-0.22274884581565857,0.08061641454696655,0.1228528842329979,0.2523101270198822]},{"input":[true,8.0,-2.4658981843858427,false,false,0.0017626280153360862,false,0.12],"torque":-0.2255568251499552,"active":true,"saturated":false,"log":[0.05837118998169899,0.29769307374954224,0.0018100710585713387,-0.0,0.15770462155342102,-0.22555682063102722,0.08122190088033676,0.12422559410333633,0.2451363503932953]},{"input":[true,8.0,-2.484567442958402,false,false,0.0017738095139792282,false,0.12],"torque":-0.22802679746858462,"active":true,"saturated":false,"log":[0.05933959037065506,0.3026319146156311,0.002017759718000889,-0.0,0.15752212703227997,-0.22802679240703583,0.08173714578151703,0.12545427680015564,0.23764021694660187]},{"input":[true,8.0,-2.499946281706811,false,false,0.0017830202956463255,false,0.12],"torque":-0.23015817321843327,"active":true,"saturated":false,"log":[0.06023448333144188,0.30719587206840515,0.002228580415248871,-0.0,0.1570291668176651,-0.23015817999839783,0.08216157555580139,0.1265380084514618,0.22983354330062866]},{"input":[true,8.0,-2.5120176146145847,false,false,0.0017902501270831014,false,0.12],"torque":-0.2319506388298476,"active":true,"saturated":false,"log":[0.06105541065335274,0.31138259172439575,0.0024422742426395416,-0.0,0.15622808039188385,-0.23195064067840576,0.08249472826719284,0.12747596204280853,0.22172825038433075]},{"input":[true,8.0,-2.5207680303314657,false,false,0.001795490975887298,false,0.12],"torque":-0.23340412484683007,"active":true,"saturated":false,"log":[0.06180195137858391,0.3151899576187134,0.0026585811283439398,-0.0,0.15512147545814514,-0.23340412974357605,0.08273622393608093,0.12826745212078094,0.21333640813827515]},{"input":[true,8.0,-2.5261878070735393,false,false,0.0017987370194327387,false,0.12],"torque":-0.23451885440228182,"active":true,"saturated":false,"log":[0.06247374042868614,0.3186160624027252,0.0028772391378879547,-0.0,0.15371224284172058,-0.2345188558101654,0.08288580179214478,0.12891195714473724,0.20467014610767365]},{"input":[true,8.0,-2.5282709234242153,false,false,0.0017999846513383159,false,0.12],"torque":-0.23529530630446557,"active":true,"saturated":false,"log":[0.06307045370340347,0.3216593265533447,0.0030979858711361885,-0.0,0.15200349688529968,-0.23529531061649323,0.08294329047203064,0.129409059882164,0.1957417130470276]},{"input":[true,8.0,-2.527015065024077,false,false,0.001799232485474709,false,0.12],"torque":-0.23573427104415728,"active":true,"saturated":false,"log":[0.06359183043241501,0.3243183493614197,0.003320557065308094,-0.0,0.14999857544898987,-0.2357342690229416,0.08290863037109375,0.12975852191448212,0.18656346201896667]},{"input":[true,8.0,-2.522421627142147,false,false,0.0017964813575043867,false,0.12],"torque":-0.2358367935082049,"active":true,"saturated":false,"log":[0.06403765082359314,0.32659202814102173,0.0035446889232844114,-0.0,0.14770109951496124,-0.23583678901195526,0.08278185874223709,0.12996019423007965,0.17714786529541016]},{"input":[true,8.0,-2.5144957131257373,false,false,0.001791734323953177,false,0.12],"torque":-0.23560426519376573,"active":true,"saturated":false,"log":[0.06440777331590652,0.3284796476364136,0.003770116250962019,-0.0,0.1451149433851242,-0.23560427129268646,0.082563117146492,0.13001413643360138,0.16750748455524445]},{"input":[true,8.0,-2.5032461287305825,false,false,0.0017849966588144434,false,0.12],"torque":-0.23503835653850425,"active":true,"saturated":false,"log":[0.064702108502388,0.3299807608127594,0.003996573388576508,-0.0,0.14224418997764587,-0.23503835499286652,0.08225264400243759,0.12992049753665924,0.1576550006866455]},{"input":[true,8.0,-2.4886853723375784,false,false,0.0017762758476896352,false,0.12],"torque":-0.2341409958352029,"active":true,"saturated":false,"log":[0.06492061167955399,0.3310951292514801,0.004223795607686043,-0.0,0.13909317553043365,-0.23414099216461182,0.08185078948736191,0.1296796202659607,0.14760316908359528]},{"input":[true,8.0,-2.470829621066974,false,false,0.0017655815794717213,false,0.12],"torque":-0.2340825219061705,"active":true,"saturated":false,"log":[0.06506332755088806,0.3318229615688324,0.004451517481356859,-0.0,0.1380331814289093,-0.23408252000808716,0.08135800063610077,0.12929198145866394,0.13736487925052643]},{"input":[true,8.0,-2.449698712805461,false,false,0.0017529257355807513,false,0.12],"torque":-0.2339853240721955,"active":true,"saturated":false,"log":[0.06513035297393799,0.33216479420661926,0.004679473582655191,-0.0,0.1372857540845871,-0.2339853197336197,0.08077482134103775,0.12875817716121674,0.12695305049419403]},{"input":[true,8.0,-2.4253161241661148,false,false,0.0017383223767634996,false,0.12],"torque":-0.23358607919052266,"active":true,"saturated":false,"log":[0.0651218444108963,0.3321214020252228,0.004907399881631136,-0.0,0.1363167017698288,-0.23358607292175293,0.08010189235210419,0.12807898223400116,0.11638074368238449]},{"input":[true,8.0,-2.397708944405686,false,false,0.00172178772747186,false,0.12],"torque":-0.2328861311667234,"active":true,"saturated":false,"log":[0.0650380477309227,0.3316940367221832,0.005135033279657364,0.0,0.13512755930423737,-0.23288613557815552,0.07933998107910156,0.12725533545017242,0.1056610494852066]},{"input":[true,8.0,-2.3669078453282144,false,false,0.001703340157837346,false,0.12],"torque":-0.23188706527395395,"active":true,"saturated":false,"log":[0.06487925350666046,0.33088418841362,0.005362110678106546,0.0,0.1337200552225113,-0.2318870723247528,0.07848991453647614,0.1262882798910141,0.09480715543031693]},{"input":[true,8.0,-2.3329470472083975,false,false,0.001683000163261724,false,0.12],"torque":-0.2305907812526602,"active":true,"saturated":false,"log":[0.06464582681655884,0.3296937048435211,0.005588370840996504,0.0,0.13209615647792816,-0.23059077560901642,0.07755264639854431,0.12517903745174408,0.08383229374885559]},{"input":[true,8.0,-2.2958642807725904,false,false,0.0016607903416464523,false,0.12],"torque":-0.22899951015808404,"active":true,"saturated":false,"log":[0.06433821469545364,0.32812488079071045,0.005813554860651493,0.0,0.1302579939365387,-0.22899951040744781,0.07652921974658966,0.1239289864897728,0.07274975627660751]},{"input":[true,8.0,-2.255700745279652,false,false,0.001636735368286227,false,0.12],"torque":-0.22711575616058266,"active":true,"saturated":false,"log":[0.06395692378282547,0.3261803090572357,0.006037403829395771,0.0,0.1282079517841339,-0.22711575031280518,0.07542076706886292,0.12253962457180023,0.06157287582755089]},{"input":[true,8.0,-2.2125010627482316,false,false,0.0016108619684545263,false,0.12],"torque":-0.22494229454377446,"active":true,"saturated":false,"log":[0.06350252032279968,0.32386285066604614,0.006259662564843893,0.0,0.12594857811927795,-0.22494229674339294,0.07422851771116257,0.1210126057267189,0.0503150150179863]},{"input":[true,8.0,-2.1663132283813393,false,false,0.001583198887711611,false,0.12],"torque":-0.22248226336053173,"active":true,"saturated":false,"log":[0.06297566741704941,0.3211759030818939,0.006480077747255564,0.0,0.1234826147556305,-0.22248226404190063,0.07295380532741547,0.11934974789619446,0.038989562541246414]},{"input":[true,8.0,-2.1171885572432796,false,false,0.0015537768599679727,false,0.12],"torque":-0.21973902993896097,"active":true,"saturated":false,"log":[0.062377072870731354,0.31812307238578796,0.00669839745387435,0.0,0.12081301957368851,-0.21973903477191925,0.07159803807735443,0.1175529807806015,0.027609923854470253]},{"input":[true,8.0,-2.065181627248193,false,false,0.0015226285733387025,false,0.12],"torque":-0.21671629153988328,"active":true,"saturated":false,"log":[0.06170753017067909,0.314708411693573,0.006914373487234116,0.0,0.1179429441690445,-0.21671628952026367,0.07016272842884064,0.11562439799308777,0.01618950255215168]},{"input":[true,8.0,-2.0103502185235422,true,false,0.0014897886338267289,false,0.12],"torque":-0.21331283620095576,"active":true,"saturated":false,"log":[0.06096789985895157,0.31093630194664,0.006914373487234116,0.0,0.11487572640180588,-0.21331283450126648,0.06864946335554123,0.11356622725725174,0.004741694778203964]},{"input":[true,8.0,-1.9527552492159155,true,false,0.001455293526875262,false,0.12],"torque":-0.20964298739159942,"active":true,"saturated":false,"log":[0.0601591132581234,0.30681148171424866,0.006914373487234116,0.0,0.1116219013929367,-0.20964299142360687,0.06705992668867111,0.11138083040714264,-0.006720121018588543]},{"input":[true,8.0,-1.8924607078104672,true,false,0.0014191815768321676,false,0.12],"torque":-0.2057104424935438,"active":true,"saturated":false,"log":[0.05928216129541397,0.30233901739120483,0.006914373487234116,0.0,0.10818327218294144,-0.20571044087409973,0.06539588421583176,0.10907071828842163,-0.018182598054409027]},{"input":[true,8.0,-1.8295335820391807,true,false,0.0013814929043712983,false,0.12],"torque":-0.2015177587393819,"active":true,"saturated":false,"log":[0.05833812057971954,0.29752442240715027,0.006914373487234116,0.0,0.10455891489982605,-0.2015177607536316,0.0636591911315918,0.10663852095603943,-0.029632432386279106]},{"input":[true,8.0,-1.7640437844569445,true,false,0.0013422693819180966,false,0.12],"torque":-0.19707011708472508,"active":true,"saturated":false,"log":[0.05732811614871025,0.292373389005661,0.006914373487234116,0.0,0.10075294226408005,-0.19707012176513672,0.06185177341103554,0.10408700257539749,-0.041056372225284576]},{"input":[true,8.0,-1.6960640747681324,true,false,0.0013015545871289842,false,0.12],"torque":-0.19237299700398067,"active":true,"saturated":false,"log":[0.05625335872173309,0.2868921160697937,0.006914373487234116,0.0,0.09676966071128845,-0.19237299263477325,0.05997563526034355,0.10141906142234802,-0.05244123190641403]},{"input":[true,8.0,-1.6256699789899731,true,false,0.0012593937544762292,false,0.12],"torque":-0.1874320869738121,"active":true,"saturated":false,"log":[0.05511511117219925,0.28108707070350647,0.006914373487234116,0.0,0.09261353313922882,-0.18743208050727844,0.05803286284208298,0.09863771498203278,-0.06377390772104263]},{"input":[true,8.0,-1.5529397055425194,true,false,0.001215833724992072,false,0.12],"torque":-0.18225333543462002,"active":true,"saturated":false,"log":[0.05391471087932587,0.2749650180339813,0.006914373487234116,0.0,0.08828916400671005,-0.18225333094596863,0.05602561682462692,0.09574609994888306,-0.07504138350486755]},{"input":[true,8.0,-1.4779540583584507,true,false,0.0011709228942279536,false,0.12],"torque":-0.17684290831963026,"active":true,"saturated":false,"log":[0.05265355482697487,0.2685331404209137,0.006914373487234116,0.0,0.08380132913589478,-0.17684291303157806,0.053956128656864166,0.09274747967720032,-0.08623073995113373]},{"input":[true,8.0,-1.4007963471092395,true,false,0.0011247111584866602,false,0.12],"torque":-0.1712071936199244,"active":true,"saturated":false,"log":[0.05133310332894325,0.26179882884025574,0.006914373487234116,0.0,0.07915496081113815,-0.17120718955993652,0.05182668939232826,0.08964522927999496,-0.09732917696237564]},{"input":[true,8.0,-1.321552294647423,true,false,0.0010772498593871215,false,0.12],"torque":-0.16535279691603316,"active":true,"saturated":false,"log":[0.04995488002896309,0.254769891500473,0.006914373487234116,0.0,0.0743551254272461,-0.16535279154777527,0.04963967204093933,0.08644283562898636,-0.10832402110099792]},{"input":[true,8.0,-1.2403099417678014,true,false,0.0010285917268234453,false,0.12],"torque":-0.15928652796621942,"active":true,"saturated":false,"log":[0.048520468175411224,0.24745438992977142,0.006914373487234116,0.0,0.06940703839063644,-0.1592865288257599,0.04739750549197197,0.08314389735460281,-0.11920274794101715]},{"input":[true,8.0,-1.1571595493933933,true,false,0.0009787908203815724,false,0.12],"torque":-0.15301540646173692,"active":true,"saturated":false,"log":[0.04703151434659958,0.23986072838306427,0.006914373487234116,0.0,0.06431606411933899,-0.1530154049396515,0.04510268196463585,0.079752117395401,-0.12995296716690063]},{"input":[true,8.0,-1.0721934982948116,true,false,0.0009279024692786355,false,0.12],"torque":-0.14654661221751983,"active":true,"saturated":false,"log":[0.045489709824323654,0.2319975197315216,0.006914373487234116,0.0,0.059087686240673065,-0.1465466171503067,0.04275774583220482,0.07627128809690475,-0.14056245982646942]},{"input":[true,8.0,-0.9855061864544579,true,false,0.0008759832108917369,false,0.12],"torque":-0.13914867297199457,"active":true,"saturated":false,"log":[0.043896809220314026,0.22387371957302094,0.006914373487234116,0.0,0.05222479999065399,-0.13914866745471954,0.0403653085231781,0.07270532101392746,-0.15101918578147888]},{"input":[true,8.0,-0.8971939241895934,true,false,0.0008230907279444615,false,0.12],"torque":-0.13087342483076383,"active":true,"saturated":false,"log":[0.04225463047623634,0.21549861133098602,0.006914373487234116,0.0,0.04382229223847389,-0.1308734267950058,0.0379280224442482,0.06905819475650787,-0.1613112837076187]},{"input":[true,8.0,-0.8073548271507796,false,false,0.0007692837844208935,true,0.12],"torque":-0.12183783484057512,"active":true,"saturated":false,"log":[0.04056502506136894,0.20688162744045258,0.005673476494848728,0.0,0.03534843027591705,-0.12183783203363419,0.0354485958814621,0.06533398479223251,-0.17142710089683533]},{"input":[true,8.0,-0.7160887073145881,false,false,0.0007146221602783498,false,0.12],"torque":-0.11333799321587816,"active":true,"saturated":false,"log":[0.038829904049634933,0.19803251326084137,0.005809381138533354,0.0,0.026812469586730003,-0.11333799362182617,0.032929789274930954,0.061536870896816254,-0.18135519325733185]},{"input":[true,8.0,-0.6234969620916639,false,false,0.0006591665850313498,false,0.12],"torque":-0.10470364842109493,"active":true,"saturated":false,"log":[0.037051230669021606,0.18896128237247467,0.005939060356467962,0.0,0.018223756924271584,-0.10470364987850189,0.030374396592378616,0.05767107754945755,-0.1910843402147293]},{"input":[true,8.0,-0.5296824616733757,false,false,0.0006029786702806292,false,0.12],"torque":-0.09594444990626873,"active":true,"saturated":false,"log":[0.035231009125709534,0.17967814207077026,0.0060623688623309135,0.0,0.009591731242835522,-0.09594444930553436,0.02778525836765766,0.053740933537483215,-0.20060355961322784]},{"input":[true,8.0,-0.43474943474217914,false,false,0.000546120841262141,false,0.12],"torque":-0.08707017494008495,"active":true,"saturated":false,"log":[0.03337128832936287,0.17019356787204742,0.006179168354719877,0.0,0.0009259124635718763,-0.08707017451524734,0.025165248662233353,0.04975081607699394,-0.2099020928144455]},{"input":[true,8.0,-0.3388033526727013,false,false,0.0004886562674921086,false,0.12],"torque":-0.07809071674623384,"active":true,"saturated":false,"log":[0.03147415816783905,0.16051819920539856,0.00628932798281312,0.0,-0.007764098234474659,-0.07809071987867355,0.022517280653119087,0.04570517688989639,-0.218969464302063]},{"input":[true,8.0,-0.24195081235216548,false,false,0.00043064879258516834,false,0.12],"torque":-0.06901609072275242,"active":true,"saturated":false,"log":[0.029541753232479095,0.15066294372081757,0.006392724346369505,0.0,-0.016468632966279984,-0.06901609152555466,0.01984429731965065,0.0416085347533226,-0.22779546678066254]},{"input":[true,8.0,-0.14429941775037686,false,false,0.0003721628633235905,false,0.12],"torque":-0.059856393402445945,"active":true,"saturated":false,"log":[0.02757623791694641,0.1406388133764267,0.006489241030067205,0.0,-0.0251779668033123,-0.059856392443180084,0.017149265855550766,0.037465453147888184,-0.23637014627456665]},{"input":[true,8.0,-0.04595766037082416,false,false,0.0003132634580563725,false,0.12],"torque":-0.05062182151210887,"active":true,"saturated":false,"log":[0.025579815730452538,0.1304570585489273,0.006578770466148853,0.0,-0.03388233110308647,-0.050621822476387024,0.014435180462896824,0.033280547708272934,-0.2446838617324829]},{"input":[true,8.0,0.05296520128426957,false,false,0.00025401601450776096,false,0.12],"torque":-0.04132266255286914,"active":true,"saturated":false,"log":[0.023554727435112,0.12012910842895508,0.00666121207177639,0.0,-0.04257192090153694,-0.04132266342639923,0.011705058626830578,0.029058486223220825,-0.2527272701263428]},{"input":[true,8.0,0.15235926310104986,false,false,0.00019448635707539666,false,0.12],"torque":-0.031969257018039605,"active":true,"saturated":false,"log":[0.021503238007426262,0.10966651141643524,0.0067364731803536415,0.0,-0.051236920058727264,-0.031969256699085236,0.008961931802332401,0.024803970009088516,-0.260491281747818]},{"input":[true,8.0,0.25211409745840274,false,false,0.00013474062369886897,false,0.12],"torque":-0.022572006977665884,"active":true,"saturated":false,"log":[0.019427642226219177,0.09908097237348557,0.006804469972848892,0.0,-0.05986751243472099,-0.022572007030248642,0.006208848208189011,0.02052173763513565,-0.26796722412109375]},{"input":[true,8.0,0.35211887591394714,false,false,7.484519237992288e-05,false,0.12],"torque":-0.01314136585509336,"active":true,"saturated":false,"log":[0.017330262809991837,0.08838433772325516,0.006865126080811024,0.0,-0.06845389306545258,-0.013141365721821785,0.0034488667733967304,0.01621655933558941,-0.2751466929912567]},{"input":[true,8.0,0.45226249233535454,false,false,1.4866607435947705e-05,false,0.12],"torque":-0.0036878188313693655,"active":true,"saturated":false,"log":[0.015213444828987122,0.07758856564760208,0.006918373052030802,0.0,-0.07698629051446915,-0.003687818767502904,0.0006850536447018385,0.011893224902451038,-0.2820216119289398]},{"input":[true,8.0,0.552433686340172,false,false,-4.5128494431304356e-05,false,0.12],"torque":0.005778136710063275,"active":true,"saturated":false,"log":[0.013079550117254257,0.06670570373535156,0.006964151281863451,0.0,-0.08545498549938202,0.005778136663138866,-0.002079520607367158,0.007556550670415163,-0.28858426213264465]},{"input":[true,8.0,0.6525211669070252,false,false,-0.00010507345816964415,false,0.12],"torque":0.015245982670997353,"active":true,"saturated":false,"log":[0.010930962860584259,0.05574791133403778,0.007002410013228655,0.0,-0.09385031461715698,0.015245982445776463,-0.004841784480959177,0.0032113627530634403,-0.29482728242874146]},{"input":[true,8.0,0.7524137360208554,false,false,-0.0001649016844307542,false,0.12],"torque":0.024706819112076457,"active":true,"saturated":false,"log":[0.008770082145929337,0.04472741857171059,0.007033105008304119,0.0,-0.10216262936592102,0.024706820026040077,-0.007598669268190861,-0.0011375034227967262,-0.30074363946914673]},{"input":[true,8.0,0.8520004122148366,false,false,-0.0002245467035626178,false,0.12],"torque":0.03415627864312804,"active":true,"saturated":false,"log":[0.006599319167435169,0.0336565263569355,0.007056202739477158,0.0,-0.11038263887166977,0.034156277775764465,-0.010347112081944942,-0.005485208239406347,-0.3063267469406128]},{"input":[true,8.0,0.9511705538716946,false,false,-0.00028394224945784677,false,0.12],"torque":0.04358120557276786,"active":true,"saturated":false,"log":[0.004421094432473183,0.022547582164406776,0.007071676664054394,0.0,-0.11850135028362274,0.04358120635151863,-0.01308405864983797,-0.00982691440731287,-0.31157031655311584]},{"input":[true,8.0,1.0498139821474568,false,true,-0.0003430223331758737,false,0.12],"torque":0.052975010041205514,"active":true,"saturated":false,"log":[0.002237837528809905,0.011412971653044224,0.007071676664054394,0.0,-0.12650959193706512,0.05297501012682915,-0.015806468203663826,-0.014157790690660477,-0.3164684474468231]},{"input":[true,8.0,1.1478211033810501,false,true,-0.00040172131625720533,false,0.12],"torque":0.06231970946756523,"active":true,"saturated":false,"log":[5.1982791774207726e-05,0.0002651122340466827,0.007071676664054394,0.0,-0.13439826667308807,0.06231971085071564,-0.018511317670345306,-0.01847301982343197,-0.32101568579673767]},{"input":[true,8.0,1.2450830308537693,false,true,-0.0004599739836482962,false,0.12],"torque":0.07160496208057225,"active":true,"saturated":false,"log":[-0.0021340337116271257,-0.010883571580052376,0.007071676664054394,0.0,-0.14215849339962006,0.07160495966672897,-0.021195601671934128,-0.022767802700400352,-0.3252069056034088]},{"input":[true,8.0,1.3414917057633116,false,true,-0.000517715616156007,false,0.12],"torque":0.08082048708657337,"active":true,"saturated":false,"log":[-0.004317775834351778,-0.022020656615495682,0.007071676664054394,0.0,-0.1497814655303955,0.0808204859495163,-0.023856334388256073,-0.02703736163675785,-0.3290373980998993]},{"input":[true,8.0,1.4369400172779996,false,true,-0.0005748820623511629,false,0.12],"torque":0.08995607644747361,"active":true,"saturated":false,"log":[-0.006496811285614967,-0.03313373774290085,0.007071676664054394,0.0,-0.15725861489772797,0.08995607495307922,-0.02649056538939476,-0.031276948750019073,-0.33250290155410767]},{"input":[true,8.0,1.531321921537791,false,true,-0.0006314098098413157,false,0.12],"torque":0.09900159605258768,"active":true,"saturated":false,"log":[-0.00866871140897274,-0.04421042650938034,0.007071676664054394,0.0,-0.16458149254322052,0.09900159388780594,-0.0290953628718853,-0.03548184782266617,-0.33559948205947876]},{"input":[true,8.0,1.6245325594698894,false,true,-0.0006872360558335404,false,0.12],"torque":0.10794700678573833,"active":true,"saturated":false,"log":[-0.010831058956682682,-0.055238399654626846,0.007071676664054394,0.0,-0.17174184322357178,0.10794700682163239,-0.031667836010456085,-0.039647381752729416,-0.338323712348938]},{"input":[true,8.0,1.7164683732880324,false,false,-0.0007422987769088523,false,0.12],"torque":0.11680466649601184,"active":true,"saturated":false,"log":[-0.012981445528566837,-0.06620537489652634,0.00702624162659049,0.0,-0.17873159050941467,0.11680466681718826,-0.034205127507448196,-0.04376892372965813,-0.34067249298095703]},{"input":[true,8.0,1.8070272215460421,false,false,-0.0007965367979307344,false,0.12],"torque":0.1255461033641183,"active":true,"saturated":false,"log":[-0.01511747669428587,-0.07709912955760956,0.006973330397158861,0.0,-0.18554286658763885,0.12554609775543213,-0.0367044135928154,-0.047841884195804596,-0.34264320135116577]},{"input":[true,8.0,1.8961084926178196,false,false,-0.000849889860011222,false,0.12],"torque":0.1341615599798323,"active":true,"saturated":false,"log":[-0.017236774787306786,-0.08790755271911621,0.006913001649081707,0.0,-0.1921679675579071,0.1341615617275238,-0.03916292265057564,-0.05186174064874649,-0.34423360228538513]},{"input":[true,8.0,1.9836132164776854,false,false,-0.0009022986874590205,false,0.12],"torque":0.1426413925507267,"active":true,"saturated":false,"log":[-0.01933697983622551,-0.09861859679222107,0.0068453224375844,0.0,-0.1985994428396225,0.14264139533042908,-0.04157792404294014,-0.05582401901483536,-0.34544190764427185]},{"input":[true,8.0,2.069444174656896,false,false,-0.0009537050536352881,false,0.12],"torque":0.15097609180226143,"active":true,"saturated":false,"log":[-0.02141575701534748,-0.10922036319971085,0.00677036726847291,0.0,-0.20483002066612244,0.15097609162330627,-0.043946728110313416,-0.059724316000938416,-0.3462667763233185]},{"input":[true,8.0,2.1535060082541566,false,false,-0.001004051845643908,false,0.12],"torque":0.15915626685039413,"active":true,"saturated":false,"log":[-0.023470789194107056,-0.11970102787017822,0.006688219495117664,0.0,-0.21085265278816223,0.15915626287460327,-0.04626670852303505,-0.06355829536914825,-0.3467072546482086]},{"input":[true,8.0,2.235705323880154,false,false,-0.0010532831277843902,false,0.12],"torque":0.16717269247299246,"active":true,"saturated":false,"log":[-0.025499794632196426,-0.1300489455461502,0.006598969921469688,0.0,-0.2166605442762375,0.16717268526554108,-0.048535287380218506,-0.06732170283794403,-0.3467628061771393]},{"input":[true,8.0,2.3159507974183793,false,false,-0.001101344203696894,false,0.12],"torque":0.17501627039765436,"active":true,"saturated":false,"log":[-0.027500510215759277,-0.14025260508060455,0.006502718199044466,0.0,-0.22224707901477814,0.17501626908779144,-0.05074993893504143,-0.07101033627986908,-0.3464333713054657]},{"input":[true,8.0,1.3119127173518106,false,false,-0.0005,false,0.12],"torque":0.2738973833697583,"active":true,"saturated":false,"log":[-0.06606456637382507,-0.3369292914867401,0.006271492224186659,-0.0,-0.22644966840744019,0.2738973796367645,-0.02304000034928322,-0.07171161472797394,-0.3214818239212036]},{"input":[true,8.0,1.3100582307920379,false,false,-0.0004988893003505612,false,0.12],"torque":0.27322175399283355,"active":true,"saturated":false,"log":[-0.06696595251560211,-0.3415263593196869,0.006037111394107342,-0.0,-0.22022512555122375,0.2732217609882355,-0.02298881858587265,-0.07232451438903809,-0.29610052704811096]},{"input":[true,8.0,1.3045030102230066,false,false,-0.0004955621360170897,false,0.12],"torque":0.2724395632133539,"active":true,"saturated":false,"log":[-0.06787578761577606,-0.3461665213108063,0.005799546372145414,-0.0,-0.21374055743217468,0.272439569234848,-0.0228355024009943,-0.07284149527549744,-0.27026060223579407]},{"input":[true,8.0,1.2952717363708712,false,false,-0.0004900332889206208,false,0.12],"torque":0.2715173065987804,"active":true,"saturated":false,"log":[-0.06878378987312317,-0.3507973253726959,0.005558802746236324,-0.0,-0.20698048174381256,0.2715173065662384,-0.02258073352277279,-0.07325568050146103,-0.24393808841705322]},{"input":[true,8.0,1.2824054219261578,false,false,-0.00048232732261528193,false,0.12],"torque":0.2704233455338863,"active":true,"saturated":false,"log":[-0.06967993080615997,-0.35536766052246094,0.005314923357218504,-0.0,-0.19993197917938232,0.27042335271835327,-0.022225642576813698,-0.07356080412864685,-0.21711401641368866]},{"input":[true,8.0,1.2659612293326394,false,false,-0.00047247847315736883,false,0.12],"torque":0.2691277323098354,"active":true,"saturated":false,"log":[-0.0705544650554657,-0.35982775688171387,0.0050679827108979225,-0.0,-0.1925840675830841,0.2691277265548706,-0.021771807223558426,-0.07375126332044601,-0.18977421522140503]},{"input":[true,8.0,1.2460122168261116,false,false,-0.00046053049700144256,false,0.12],"torque":0.2676019182242659,"active":true,"saturated":false,"log":[-0.07139790803194046,-0.36412933468818665,0.00481808977201581,-0.0,-0.184927299618721,0.2676019072532654,-0.02122124470770359,-0.07382208853960037,-0.16190946102142334]},{"input":[true,8.0,1.2226470138513648,false,false,-0.00044653647659921467,false,0.12],"torque":0.2675171962288169,"active":true,"saturated":false,"log":[-0.07220107316970825,-0.36822548508644104,0.004565386101603508,-0.0,-0.18040776252746582,0.26751720905303955,-0.02057640068233013,-0.07376895844936371,-0.1335153579711914]},{"input":[true,8.0,1.1959694272994121,false,false,-0.00043055858456490516,false,0.12],"torque":0.26947440012426077,"active":true,"saturated":false,"log":[-0.07295508682727814,-0.37207093834877014,0.004310043063014746,-0.0,-0.180293008685112,0.2694743871688843,-0.019840139895677567,-0.07358819991350174,-0.10459230840206146]},{"input":[true,8.0,1.1660979803143776,false,false,-0.0004126678074548392,false,0.12],"torque":0.27119277529183716,"active":true,"saturated":false,"log":[-0.07365138828754425,-0.37562209367752075,0.004052263218909502,-0.0,-0.17998850345611572,0.2711927890777588,-0.019015731289982796,-0.07327678054571152,-0.07514551281929016]},{"input":[true,8.0,1.1331653857190394,false,false,-0.00039294363038847404,false,0.12],"torque":0.2726489971362591,"active":true,"saturated":false,"log":[-0.07428175956010818,-0.3788369596004486,0.003792277304455638,-0.0,-0.17948907613754272,0.27264899015426636,-0.018106842413544655,-0.07283230125904083,-0.04518482834100723]},{"input":[true,8.0,1.0973179563984858,false,false,-0.00037147368391202205,false,0.12],"torque":0.27382084223572367,"active":true,"saturated":false,"log":[-0.07483832538127899,-0.38167545199394226,0.0035303430631756783,-0.0,-0.17878997325897217,0.2738208472728729,-0.017117507755756378,-0.07225300371646881,-0.014724771492183208]},{"input":[true,8.0,1.058714955261436,false,false,-0.0003483533546735827,false,0.12],"torque":0.11000311723572459,"active":true,"saturated":false,"log":[-0.022953519597649574,-0.11706294864416122,0.0035303430631756783,-0.0,-0.1106266900897026,0.11000311374664307,-0.016052121296525,-0.0329626239836216,-0.008021863177418709]},{"input":[true,8.0,1.0175278876672105,false,false,-0.0003236853616394762,false,0.12],"torque":0.11016411374947799,"active":true,"saturated":false,"log":[-0.02324121817946434,-0.11853021383285522,0.003448998788371682,-0.0,-0.10941445827484131,0.110164113342762,-0.014915420673787594,-0.0320378802716732,-0.0009080310119315982]},{"input":[true,8.0,0.9739397394619622,false,false,-0.0002975792997345639,false,0.12],"torque":0.10977938875800423,"active":true,"saturated":false,"log":[-0.023368820548057556,-0.11918098479509354,0.003367207944393158,-0.0,-0.1079077273607254,0.10977938771247864,-0.013712453655898571,-0.030928919091820717,0.006558808498084545]},{"input":[true,8.0,0.9281441640094168,false,false,-0.00027015115293406986,false,0.12],"torque":0.10886198155331907,"active":true,"saturated":false,"log":[-0.02333974279463291,-0.11903268843889236,0.0032855188474059105,-0.0,-0.1061159148812294,0.10886198282241821,-0.012448565103113651,-0.029643608257174492,0.01432101521641016]},{"input":[true,8.0,0.8803446218279778,false,false,-0.00024152277897013832,false,0.12],"torque":0.10742742454776152,"active":true,"saturated":false,"log":[-0.023157821968197823,-0.11810488998889923,0.0032044665422290564,-0.0,-0.10405132919549942,0.10742742568254471,-0.011129369027912617,-0.028190387412905693,0.022321350872516632]},{"input":[true,8.0,0.8307534766566222,false,false,-0.0002118213679424723,false,0.12],"torque":0.10549294567009862,"active":true,"saturated":false,"log":[-0.022827310487627983,-0.11641928553581238,0.0031245709396898746,-0.0,-0.10172754526138306,0.10549294203519821,-0.009760728105902672,-0.026578249409794807,0.030503112822771072]},{"input":[true,8.0,0.7795910519655789,false,false,-0.00018117887723833681,false,0.12],"torque":0.1030772497843614,"active":true,"saturated":false,"log":[-0.022352881729602814,-0.11399969458580017,0.0030463358853012323,-0.0,-0.09915898740291595,0.1030772477388382,-0.008348722010850906,-0.024816717952489853,0.038810282945632935]},{"input":[true,8.0,0.7270846521035224,false,false,-0.00014973144527246395,true,0.12],"torque":0.10020047891612051,"active":true,"saturated":false,"log":[-0.021739624440670013,-0.11087208241224289,0.0029702470637857914,-0.0,-0.09636086225509644,0.10020048171281815,-0.006899624597281218,-0.02291581593453884,0.04718765616416931]},{"input":[true,8.0,0.6734675524301335,false,false,-0.00011761878665149471,false,0.12],"torque":0.09688415659937395,"active":true,"saturated":false,"log":[-0.020993031561374664,-0.10706446319818497,0.0028967715334147215,-0.0,-0.09334909915924072,0.09688415378332138,-0.005419873166829348,-0.020886028185486794,0.05558100342750549]},{"input":[true,8.0,0.6189779629206564,false,false,-8.498357145012052e-05,false,0.12],"torque":0.09315115396268263,"active":true,"saturated":false,"log":[-0.020118998363614082,-0.1026068925857544,0.002826354932039976,-0.0,-0.09014032781124115,0.09315115213394165,-0.0039160423912107944,-0.0187382735311985,0.0639372169971466]},{"input":[true,8.0,0.5638579698469566,false,false,-5.197079135667549e-05,false,0.12],"torque":0.0890256172371846,"active":true,"saturated":false,"log":[-0.019123805686831474,-0.09753140807151794,0.0027594217099249363,-0.0,-0.0867517963051796,0.08902561664581299,-0.0023948135785758495,-0.016483858227729797,0.07220445573329926]},{"input":[true,8.0,0.5083524602369697,false,false,-1.872711550426666e-05,false,0.12],"torque":0.08453288023149143,"active":true,"saturated":false,"log":[-0.018014097586274147,-0.09187189489603043,0.0026963723357766867,-0.0,-0.0832013413310051,0.08453287929296494,-0.0008629451040178537,-0.014134436845779419,0.08033232390880585]},{"input":[true,8.0,0.45270803389095904,false,false,1.4599761150644409e-05,false,0.12],"torque":0.07969943188365602,"active":true,"saturated":false,"log":[-0.016796881332993507,-0.08566409349441528,0.0026375832967460155,-0.0,-0.07950729876756668,0.07969943434000015,0.0006727573927491903,-0.011701976880431175,0.08827198296785355]},{"input":[true,8.0,0.3971719077882674,false,false,4.786177400718783e-05,false,0.12],"torque":0.07455278522399052,"active":true,"saturated":false,"log":[-0.015479488298296928,-0.07894539088010788,0.002583405002951622,-0.0,-0.07568845897912979,0.07455278187990189,0.002205470809713006,-0.009198702871799469,0.0959763377904892]},{"input":[true,8.0,0.34199081775206863,false,false,8.091114664128603e-05,false,0.12],"torque":0.06912142026610904,"active":true,"saturated":false,"log":[-0.014069564640522003,-0.07175478339195251,0.0025341615546494722,-0.0,-0.0717640072107315,0.06912142038345337,0.003728386014699936,-0.006637056823819876,0.10340015590190887]},{"input":[true,8.0,0.2874099222517982,false,false,0.00011360104734654355,false,0.12],"torque":0.06343466806461953,"active":true,"saturated":false,"log":[-0.012575038708746433,-0.0641326978802681,0.0024901488795876503,-0.0,-0.06775342673063278,0.0634346678853035,0.005234736483544111,-0.00402964698150754,0.11050022393465042]},{"input":[true,8.0,0.23367171321344885,false,false,0.0001457862414779463,false,0.12],"torque":0.05752262215374272,"active":true,"saturated":false,"log":[-0.011004099622368813,-0.05612090975046158,0.002451634733006358,-0.0,-0.06367646157741547,0.057522621005773544,0.006717830430716276,-0.0013891984708607197,0.11723549664020538]},{"input":[true,8.0,0.18101493867678214,false,false,0.0001773237367001388,false,0.12],"torque":0.051419928863836215,"active":true,"saturated":false,"log":[-0.00936516746878624,-0.04776235297322273,0.0024188566021621227,-0.0,-0.059553150087594986,0.051419928669929504,0.008171077817678452,0.0012714965268969536,0.12356720119714737]},{"input":[true,8.0,0.12967354208587178,false,false,0.0002080734182735712,false,0.12],"torque":0.0451567550922687,"active":true,"saturated":false,"log":[-0.007666863035410643,-0.039101000875234604,0.002392022404819727,-0.0,-0.05540340393781662,0.045156754553318024,0.009588023647665977,0.003939630463719368,0.12945899367332458]},{"input":[true,8.0,0.07987562292549863,false,false,0.0002378986715560677,false,0.12],"torque":0.03875993774170249,"active":true,"saturated":false,"log":[-0.005917977541685104,-0.030181685462594032,0.002371309557929635,-0.0,-0.051247045397758484,0.03875993937253952,0.010962370783090591,0.006602431181818247,0.1348770260810852]},{"input":[true,8.0,0.031842423321079893,false,false,0.00026666698895416686,false,0.12],"torque":0.032261435031630974,"active":true,"saturated":false,"log":[-0.0041274381801486015,-0.02104993537068367,0.0023568635806441307,-0.0,-0.047104042023420334,0.0322614349424839,0.012288015335798264,0.00924721546471119,0.1397901475429535]},{"input":[true,8.0,-0.014212654895536636,false,false,0.0002942505586276729,false,0.12],"torque":0.02569343451198084,"active":true,"saturated":false,"log":[-0.0023042778484523296,-0.011751816608011723,0.0023487985599786043,-0.0,-0.042994216084480286,0.025693435221910477,0.013559065759181976,0.011861439794301987,0.1441698968410492]},{"input":[true,8.0,-0.058084998287430356,false,false,0.0003205268323319191,false,0.12],"torque":0.018772215975198066,"active":true,"saturated":false,"log":[-0.0004575976054184139,-0.0023337476886808872,0.0023471969179809093,-0.0,-0.038292791694402695,0.018772216513752937,0.01476987637579441,0.014432751573622227,0.14799068868160248]},{"input":[true,8.0,-0.0995796908688999,false,false,0.00034537906987493823,false,0.12],"torque":0.01171225702750132,"active":true,"saturated":false,"log":[0.0014034670311957598,0.007157681975513697,0.0023521091789007187,-0.0,-0.033390771597623825,0.011712256819009781,0.015915067866444588,0.016949040815234184,0.15122981369495392]},{"input":[true,8.0,-0.1385123800779295,false,false,0.0003686968577706227,false,0.12],"torque":0.004762811915191963,"active":true,"saturated":false,"log":[0.003269781591370702,0.016675885766744614,0.0023635532706975937,-0.0,-0.028749853372573853,0.004762812051922083,0.01698955148458481,0.019398491829633713,0.15386755764484406]},{"input":[true,8.0,-0.17471009581749491,false,false,0.0003903765997836073,false,0.12],"torque":-0.0020419182240575522,"active":true,"saturated":false,"log":[0.005132249090820551,0.026174470782279968,0.0023815161548554897,-0.0,-0.02439328096807003,-0.0020419182255864143,0.017988553270697594,0.021769626066088676,0.15588727593421936]},{"input":[true,8.0,-0.208012018926866,false,false,0.0004103219771864765,false,0.12],"torque":-0.008668580124205148,"active":true,"saturated":false,"log":[0.006981846410781145,0.035607416182756424,0.002405952662229538,-0.0,-0.020342860370874405,-0.00866858009248972,0.018907636404037476,0.024051358923316002,0.15727537870407104]},{"input":[true,8.0,-0.23827019566875046,false,false,0.00042844437668447364,false,0.12],"torque":-0.015084802904256286,"active":true,"saturated":false,"log":[0.008809659630060196,0.04492926597595215,0.00243678642436862,-0.0,-0.016618868336081505,-0.015084803104400635,0.01974271796643734,0.026233039796352386,0.1580214500427246]},{"input":[true,8.0,-0.26535019505794816,false,false,0.0004446632841065207,false,0.12],"torque":-0.0212593326792975,"active":true,"saturated":false,"log":[0.010606920346617699,0.05409529432654381,0.002473910804837942,-0.0,-0.013239973224699497,-0.021259332075715065,0.02049008384346962,0.02830450050532818,0.15811817348003387]},{"input":[true,8.0,-0.2891317061111463,false,false,0.0004589066421134661,false,0.12],"torque":-0.027162167398388413,"active":true,"saturated":false,"log":[0.01236504316329956,0.06306172162294388,0.002517188433557749,-0.0,-0.010223145596683025,-0.02716216817498207,0.021146418526768684,0.03025609254837036,0.15756143629550934]},{"input":[true,8.0,-0.30950907236438435,false,false,0.000471111170334329,false,0.12],"torque":-0.03276466711718353,"active":true,"saturated":false,"log":[0.014075654558837414,0.0717858374118805,0.002566453069448471,-0.0,-0.007583597209304571,-0.03276466578245163,0.02170880325138569,0.032078731805086136,0.1563502848148346]},{"input":[true,8.0,-0.32639176128344743,false,false,0.00048122264650824387,false,0.12],"torque":-0.03803969710703337,"active":true,"saturated":false,"log":[0.015730636194348335,0.08022624254226685,0.002621510298922658,-0.0,-0.005334713961929083,-0.03803969547152519,0.022174740210175514,0.03376394137740135,0.15448687970638275]},{"input":[true,8.0,-0.33970476648167885,false,false,0.0004891961473830436,false,0.12],"torque":-0.04296172422857649,"active":true,"saturated":false,"log":[0.017322150990366936,0.08834297209978104,0.0026821380015462637,-0.0,-0.0034880046732723713,-0.0429617241024971,0.022542158141732216,0.03530387207865715,0.1519765555858612]},{"input":[true,8.0,-0.3493889409582524,false,false,0.0004949962483002228,false,0.12],"torque":-0.047506928040822065,"active":true,"saturated":false,"log":[0.018842676654458046,0.09609764814376831,0.0027480872813612223,-0.0,-0.002053053816780448,-0.04750692844390869,0.022809427231550217,0.03669135645031929,0.1488276720046997]},{"input":[true,8.0,-0.35540125987638266,false,false,0.0004985971805795595,false,0.12],"torque":-0.05174539421007629,"active":true,"saturated":false,"log":[0.020285042002797127,0.10345371067523956,0.0028190850280225277,-0.0,-0.0008498899405822158,-0.05174539238214493,0.022975359112024307,0.037919916212558746,0.14505167305469513]},{"input":[true,8.0,-0.3577150117140115,false,false,0.0004999829460041724,false,0.12],"torque":-0.056082700847653384,"active":true,"saturated":false,"log":[0.021642444655299187,0.11037646979093552,0.0028948334511369467,-0.0,0.0009830421768128872,-0.056082699447870255,0.023039214313030243,0.03898381069302559,0.14066292345523834]},{"input":[true,8.0,-0.356319916937719,false,false,0.0004991473878973766,false,0.12],"torque":-0.06006565564774807,"active":true,"saturated":false,"log":[0.022908497601747513,0.11683333665132523,0.0029750133398920298,-0.0,0.0025555994361639023,-0.060065656900405884,0.02300071157515049,0.03987804427742958,0.1356787234544754]},{"input":[true,8.0,-0.3512221736726206,false,false,0.000496094218475557,false,0.12],"torque":-0.06367537639141896,"active":true,"saturated":false,"log":[0.024077236652374268,0.12279390543699265,0.003059283597394824,-0.0,0.0038603690918534994,-0.06367537379264832,0.022860022261738777,0.040598396211862564,0.13011913001537323]},{"input":[true,8.0,-0.3424444301653466,false,false,0.0004908370023555395,false,0.12],"torque":-0.06689474084975137,"active":true,"saturated":false,"log":[0.025143155828118324,0.12823009490966797,0.0031472845003008842,-0.0,0.004891180898994207,-0.06689473986625671,0.022617770358920097,0.041141439229249954,0.12400692701339722]},{"input":[true,8.0,-0.33002568416244255,false,false,0.00048339909628973055,false,0.12],"torque":-0.06970845456975136,"active":true,"saturated":false,"log":[0.02610122784972191,0.13311626017093658,0.003238638862967491,-0.0,0.00564312981441617,-0.069708451628685,0.022275030612945557,0.04150453954935074,0.11736748367547989]},{"input":[true,8.0,-0.3140211096512358,false,false,0.00047381354539676926,false,0.12],"torque":-0.07210312440494791,"active":true,"saturated":false,"log":[0.02694692835211754,0.137429341673851,0.0033329532016068697,-0.0,0.006112595554441214,-0.07210312783718109,0.021833328530192375,0.041685886681079865,0.11022864282131195]},{"input":[true,8.0,-0.2945018117329226,false,false,0.0004621229363487218,false,0.12],"torque":-0.07406729569616277,"active":true,"saturated":false,"log":[0.02767624706029892,0.1411488652229309,0.0034298200625926256,-0.0,0.006297251675277948,-0.07406729459762573,0.021294625476002693,0.04168449342250824,0.10262057185173035]},{"input":[true,8.0,-0.27155451071692355,false,false,0.00044837920816707353,false,0.12],"torque":-0.07559151371218503,"active":true,"saturated":false,"log":[0.028285710141062737,0.14425712823867798,0.0035288201179355383,-0.0,0.006196070462465286,-0.07559151202440262,0.020661314949393272,0.041500188410282135,0.09457562863826752]},{"input":[true,8.0,-0.24528115684001478,false,false,0.00043264342146811797,false,0.12],"torque":-0.076668348735719,"active":true,"saturated":false,"log":[0.028772389516234398,0.1467391848564148,0.003629523329436779,0.0,0.005809322465211153,-0.07666835188865662,0.01993620954453945,0.041133634746074677,0.08612822741270065]},{"input":[true,8.0,-0.21579847732195895,false,false,0.00041498548718293574,false,0.12],"torque":-0.07729245019305504,"active":true,"saturated":false,"log":[0.029133925214409828,0.14858302474021912,0.003731492208316922,0.0,0.005138568114489317,-0.07729244977235794,0.0191225316375494,0.040586311370134354,0.07731465250253677]},{"input":[true,8.0,-0.18323745776997719,false,false,0.00039548385595720845,false,0.12],"torque":-0.07746052731865696,"active":true,"saturated":false,"log":[0.02936851978302002,0.14977945387363434,0.0038342818152159452,0.0,0.004186646081507206,-0.07746052742004395,0.018223896622657776,0.03986050933599472,0.06817290186882019]},{"input":[true,8.0,-0.14774276023607597,false,false,0.00037422516961079937,false,0.12],"torque":-0.07717138246736545,"active":true,"saturated":false,"log":[0.029474955052137375,0.15032227337360382,0.003937444183975458,0.0,0.0029576560482382774,-0.07717138528823853,0.01724429614841938,0.0389593243598938,0.05874252691864967]},{"input":[true,8.0,-0.10947208051268731,false,false,0.0003513038762055989,false,0.12],"torque":-0.07642591329054219,"active":true,"saturated":false,"log":[0.029452597722411156,0.1502082496881485,0.004040528554469347,0.0,0.001456933910958469,-0.07642590999603271,0.016188083216547966,0.037886638194322586,0.04906441643834114]},{"input":[true,8.0,-0.06859544752202096,false,false,0.00032682181043180595,false,0.12],"torque":-0.07522708860790228,"active":true,"saturated":false,"log":[0.029301395639777184,0.14943711459636688,0.004143083468079567,0.0,-0.00030897240503691137,-0.07522708922624588,0.015059949830174446,0.03664711117744446,0.03918064385652542]},{"input":[true,8.0,-0.02529446791180917,false,false,0.0003008877411769109,false,0.12],"torque":-0.073579935464916,"active":true,"saturated":false,"log":[0.02902187965810299,0.14801158010959625,0.004244660027325153,0.0,-0.002332336502149701,-0.07357993721961975,0.013864907436072826,0.035246141254901886,0.029134254902601242]},{"input":[true,8.0,0.020238480786455182,false,false,0.00027361688828743294,false,0.12],"torque":-0.07149150955590104,"active":true,"saturated":false,"log":[0.028615159913897514,0.14593730866909027,0.00434481306001544,0.0,-0.004604288376867771,-0.07149150967597961,0.012608266435563564,0.03368985652923584,0.018969079479575157]},{"input":[true,8.0,0.06780110485214266,false,false,0.0002451304106703497,false,0.12],"torque":-0.06897086316133509,"active":true,"saturated":false,"log":[0.028082922101020813,0.1432228982448578,0.004443102981895208,0.0,-0.007114855572581291,-0.06897086650133133,0.01129561010748148,0.0319850854575634,0.008729536086320877]},{"input":[true,8.0,0.11718209312574906,false,false,0.00021555486800847306,false,0.12],"torque":-0.06602948658756182,"active":true,"saturated":false,"log":[0.027427418157458305,0.13987983763217926,0.004539099056273699,0.0,-0.00985200796276331,-0.06602948904037476,0.009932768531143665,0.030139317736029625,-0.001539579825475812]},{"input":[false,8.0,0.1681620558218167,false,false,0.00018502165848126905,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.22051449923356664,false,false,0.0001536664349892097,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.2740068319988187,false,false,0.00012162850247525857,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.32840139845657335,false,false,8.905019902106331e-05,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.3834565345032466,false,false,5.6076263467527434e-05,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.43892764125761197,false,false,2.2853192369290225e-05,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.49456827176436347,false,false,-1.0471410859959058e-05,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.5501312259083084,false,false,-4.37494917197232e-05,true,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.6053696486746889,false,false,-7.683320239902244e-05,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.6600381268762969,false,false,-0.00010957555863444268,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.7138937794748355,false,false,-0.00014183109273161314,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.7666973366524594,false,false,-0.00017345649984887746,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.8182142028393842,false,false,-0.00020431127467184204,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.8682154989747521,false,false,-0.00023425833565018857,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.9164790793702082,false,false,-0.00026316463402339594,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,8.0,0.9627905186584484,false,false,-0.00029090174492958534,false,0.12],"torque":-0.035925535360569125,"active":true,"saturated":false,"log":[0.023616664111614227,0.12044498324394226,0.0,0.0,-0.04716290161013603,-0.03592553362250328,-0.013404752127826214,0.0039943065494298935,-0.014207550324499607]},{"input":[true,8.0,1.0069440644419436,false,false,-0.00031734643797131734,false,0.12],"torque":-0.02473125010625934,"active":true,"saturated":false,"log":[0.02029408887028694,0.10349985212087631,7.102930976543576e-05,0.0,-0.05311772972345352,-0.02473125047981739,-0.014623323455452919,0.00032790080877020955,-0.02717483416199684]},{"input":[true,8.0,1.0487435514094,false,false,-0.00034238122470342736,false,0.12],"torque":-0.013856772103746213,"active":true,"saturated":false,"log":[0.01705964282155037,0.0870041772723198,0.00013073805894237012,0.0,-0.05886874347925186,-0.01385677233338356,-0.015776926651597023,-0.003208608366549015,-0.03892677277326584]},{"input":[true,8.0,1.088003272858732,false,false,-0.0003658948806105272,false,0.12],"torque":-0.003303787388837868,"active":true,"saturated":false,"log":[0.013915198855102062,0.07096751779317856,0.00017944125283975154,0.0,-0.0644083246588707,-0.003303787438198924,-0.016860436648130417,-0.006608718074858189,-0.049487896263599396]},{"input":[true,8.0,1.1245488057545368,false,false,-0.00038778293925512483,false,0.12],"torque":0.006917330308518331,"active":true,"saturated":false,"log":[0.010863101109862328,0.055401816964149475,0.00021746210404671729,0.0,-0.06972687691450119,0.00691733043640852,-0.01786903664469719,-0.00986588653177023,-0.05888265743851662]},{"input":[true,8.0,1.1582177856545102,false,false,-0.0004079481564009665,false,0.12],"torque":0.016794546162556108,"active":true,"saturated":false,"log":[0.007906099781394005,0.04032110795378685,0.000245133473072201,0.0,-0.07481449842453003,0.016794545575976372,-0.01879825070500374,-0.01297360472381115,-0.06713566929101944]},{"input":[true,8.0,1.1888606280619647,false,false,-0.00042630094204959286,false,0.12],"torque":0.026314959967263768,"active":true,"saturated":false,"log":[0.005047281738370657,0.02574113756418228,0.0002627989451866597,0.0,-0.07966133207082748,0.026314960792660713,-0.01964394748210907,-0.015925472602248192,-0.07427197694778442]},{"input":[true,8.0,1.2163411929996208,false,false,-0.00044275975847065947,false,0.12],"torque":0.03546503811743906,"active":true,"saturated":false,"log":[0.002290001604706049,0.011679008603096008,0.0002708139654714614,0.0,-0.08425763249397278,0.03546503931283951,-0.020402370020747185,-0.018715260550379753,-0.08031725883483887]},{"input":[true,8.0,1.2405373898521093,false,false,-0.00045725148245765016,false,0.12],"torque":0.04423083122866236,"active":true,"saturated":false,"log":[-0.00036218183231540024,-0.001847127336077392,0.0002695463190320879,0.0,-0.08859392255544662,0.044230829924345016,-0.021070148795843124,-0.021336978301405907,-0.08529800921678543]},{"input":[true,8.0,1.2613417197899819,false,false,-0.00046971173019955657,false,0.12],"torque":0.05259818569347157,"active":true,"saturated":false,"log":[-0.0029055674094706774,-0.014818393625319004,0.0002593768294900656,0.0,-0.0926610678434372,0.05259818583726883,-0.021644316613674164,-0.023784929886460304,-0.08924172818660736]},{"input":[true,8.0,1.2786617533653635,false,false,-0.000480085143325183,false,0.12],"torque":0.060552945556148194,"active":true,"saturated":false,"log":[-0.0053363703191280365,-0.027215488255023956,0.0002406995336059481,0.0,-0.09645042568445206,0.06055294722318649,-0.022122323513031006,-0.02605377696454525,-0.09217702597379684]},{"input":[true,8.0,1.2924205411573724,false,false,-0.0004883256348492412,false,0.12],"torque":0.06808114870851471,"active":true,"saturated":false,"log":[-0.007650779560208321,-0.03901897743344307,0.00021392179769463837,0.0,-0.09995389729738235,0.06808114796876907,-0.022502044215798378,-0.02813858911395073,-0.09413377195596695]},{"input":[true,8.0,1.3025569556428997,false,false,-0.0004943965939275431,false,0.12],"torque":0.07516920907716854,"active":true,"saturated":false,"log":[-0.009845010936260223,-0.05020955577492714,0.0001794642594177276,0.0,-0.10316405445337296,0.07516920566558838,-0.022781794890761375,-0.03003489039838314,-0.095143161714077]},{"input":[true,8.0,1.3090259627738827,false,false,-0.0004982710485116088,false,0.12],"torque":0.08180409225458019,"active":true,"saturated":false,"log":[-0.01191535871475935,-0.06076832860708237,0.00013776050764136016,0.0,-0.10607419908046722,0.08180408924818039,-0.022960329428315163,-0.03173870965838432,-0.09523780643939972]},{"input":[true,8.0,1.3117988220545105,false,false,-0.0004999317851800449,false,0.12],"torque":0.0879734789577989,"active":true,"saturated":false,"log":[-0.013858243823051453,-0.07067704200744629,8.925666043069214e-05,0.0,-0.10867847502231598,0.08797347545623779,-0.023036856204271317,-0.033246614038944244,-0.09445179253816605]},{"input":[true,8.0,1.31086321422946,false,false,-0.0004993714256143098,false,0.12],"torque":0.09366592064079346,"active":true,"saturated":false,"log":[-0.015670260414481163,-0.07991832494735718,3.441074659349397e-05,0.0,-0.11097188293933868,0.09366592019796371,-0.023011034354567528,-0.03455575555562973,-0.09282068908214569]},{"input":[true,8.0,1.3062232960158637,false,false,-0.0004965924593790963,false,0.12],"torque":0.09887097245158702,"active":true,"saturated":false,"log":[-0.017348214983940125,-0.08847589790821075,-2.6308005544706248e-05,0.0,-0.1129503846168518,0.09887097030878067,-0.02288297936320305,-0.035663895308971405,-0.09038157016038895]},{"input":[true,8.0,1.2978996816358557,false,false,-0.0004916072328616975,false,0.12],"torque":0.10357933365039304,"active":true,"saturated":false,"log":[-0.018889170140028,-0.09633477032184601,-9.24201012821868e-05,0.0,-0.11461092531681061,0.1035793349146843,-0.022653261199593544,-0.036569442600011826,-0.08717300742864609]},{"input":[true,8.0,1.2859293512317311,false,false,-0.0004844378944194944,false,0.12],"torque":0.10778297435042863,"active":true,"saturated":false,"log":[-0.02029048465192318,-0.1034814715385437,-0.00016343679453711957,0.0,-0.11595150828361511,0.10778297483921051,-0.022322898730635643,-0.037271466106176376,-0.08323506265878677]},{"input":[true,8.0,1.270365486570624,false,false,-0.0004751162959792648,false,0.12],"torque":0.11147521813481218,"active":true,"saturated":false,"log":[-0.021549837663769722,-0.10990417003631592,-0.0002388612338108942,0.0,-0.1169712096452713,0.11147522181272507,-0.02189335785806179,-0.037769727408885956,-0.07860919833183289]}]},{"name":"highway","frames":[{"input":[false,23.0,4.043602867493436,false,false,-0.0013323183357528668,true,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,3.9336417617121064,false,false,-0.001291240963619141,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,3.819840392479317,false,false,-0.0012487290121424167,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,3.702325194053356,false,false,-0.0012048297124508391,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,3.5812267267873246,false,false,-0.0011595918370278437,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,3.4566795320755133,false,false,-0.0011130656455255266,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,3.3288219828768004,false,false,-0.0010653028289257553,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,3.1977961299811577,false,false,-0.0010163564521110636,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,3.0637475441900555,false,false,-0.0009662808949091303,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.9268251545860977,false,false,-0.000915131791676342,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.7871810830715895,false,false,-0.0008629659694875654,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.644970475359849,false,false,-0.0008098413850007994,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.5003513286070445,false,false,-0.0007558170600668505,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.35348431587605,false,false,-0.000700953016155571,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.204532607627351,false,false,-0.0006453102076715109,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.05366169043532,false,false,-0.000588950454233074,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.9010391831312672,false,false,-0.0005319363719904111,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.7468346505775425,false,false,-0.0004743313040583631,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.5912194152795727,false,false,-0.0004161992501417374,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.4343663670451507,false,false,-0.00035760479543111017,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,23.0,1.2764497709024345,false,false,-0.000298613038848147,false,0.35],"torque":-0.028335098501989977,"active":true,"saturated":false,"log":[0.11046585440635681,0.1384505331516266,0.0,-0.0,-0.08203154057264328,-0.028335098177194595,-0.11373573541641235,-0.011075318790972233,-0.03164376690983772]},{"input":[true,23.0,1.1176450734900656,false,false,-0.00023928952072016532,false,0.35],"torque":-0.003296781655824745,"active":true,"saturated":false,"log":[0.07743847370147705,0.09705621749162674,0.0002710346598178148,-0.0,-0.0907629206776619,-0.003296781564131379,-0.09114059060811996,-0.019173864275217056,-0.05478246882557869]},{"input":[true,23.0,0.9581287081345089,false,false,-0.00017970014996429066,false,0.35],"torque":0.01823510700574345,"active":true,"saturated":false,"log":[0.04729127883911133,0.059271737933158875,0.0004365541390143335,-0.0,-0.09601684659719467,0.018235107883810997,-0.06844419240951538,-0.024494482204318047,-0.06998423486948013]},{"input":[true,23.0,0.7980778988311734,false,false,-0.00011991113086210739,false,0.35],"torque":0.036455074486563765,"active":true,"saturated":false,"log":[0.019849786534905434,0.024878399446606636,0.0005060283583588898,-0.0,-0.09797138720750809,0.036455076187849045,-0.045671749860048294,-0.02722453512251377,-0.07778438925743103]},{"input":[true,23.0,0.6376704633470961,false,false,-5.998888950615651e-05,false,0.35],"torque":0.051541160143985555,"active":true,"saturated":false,"log":[-0.00504889665171504,-0.006327950395643711,0.000488357269205153,-0.0,-0.09678579866886139,0.05154116079211235,-0.022848566994071007,-0.02754071168601513,-0.07868774980306625]},{"input":[true,23.0,0.4770846156639367,false,false,0.0,false,0.35],"torque":0.06366381020603958,"active":true,"saturated":false,"log":[-0.02755693532526493,-0.03453802689909935,0.0003919079608749598,-0.0,-0.09261710196733475,0.06366381049156189,1.9151533692252087e-09,-0.025609780102968216,-0.07317079603672028]},{"input":[true,23.0,0.31649876798077725,false,false,5.998888950615651e-05,false,0.35],"torque":0.07299156192136247,"active":true,"saturated":false,"log":[-0.04781654477119446,-0.059930071234703064,0.00022455006546806544,-0.0,-0.08563051372766495,0.07299156486988068,0.022848570719361305,-0.021589288488030434,-0.06168368458747864]},{"input":[true,23.0,0.15609133249669996,false,false,0.00011991113086210739,false,0.35],"torque":0.07969363133881104,"active":true,"saturated":false,"log":[-0.06596070528030396,-0.08267074823379517,-6.31240027360036e-06,-0.0,-0.07600369304418564,0.07969363033771515,0.04567175358533859,-0.015628214925527573,-0.04465204104781151]},{"input":[true,23.0,-0.003959476806635387,false,false,0.00017970014996429066,false,0.35],"torque":0.08393990549174267,"active":true,"saturated":false,"log":[-0.08211386203765869,-0.10291603952646255,-0.0002937109093181789,-0.0,-0.06392590701580048,0.08393990248441696,0.06844419240951538,-0.007867556065320969,-0.022478731349110603]},{"input":[true,23.0,-0.1634758421621924,false,false,0.00023928952072016532,false,0.35],"torque":0.08593339953028915,"active":true,"saturated":false,"log":[-0.09639251232147217,-0.12081194669008255,-0.0006310847238637507,-0.0,-0.049661945551633835,0.08593340218067169,0.09114059805870056,0.0015591104747727513,0.004454601090401411]},{"input":[true,23.0,-0.3222805395745611,false,false,0.000298613038848147,false,0.35],"torque":0.0860504076676839,"active":true,"saturated":false,"log":[-0.10890582948923111,-0.13649530708789825,-0.0010122550884261727,-0.0,-0.033830393105745316,0.08605040609836578,0.11373573541641235,0.01252511702477932,0.03578604757785797]},{"input":[true,23.0,-0.48019713571727723,false,false,0.00035760479543111017,false,0.35],"torque":0.08430902652181294,"active":true,"saturated":false,"log":[-0.11975616961717606,-0.1500944048166275,-0.0014314017025753856,-0.0,-0.01634482480585575,0.08430902659893036,0.13620451092720032,0.024910233914852142,0.07117209583520889]},{"input":[true,23.0,-0.6370501839516991,false,false,0.0004161992501417374,false,0.35],"torque":0.0808737593286707,"active":true,"saturated":false,"log":[-0.12903960049152374,-0.161729633808136,-0.0018830402987077832,-0.0,0.0025821304880082607,0.0808737576007843,0.15852196514606476,0.03860022500157356,0.11028636246919632]},{"input":[true,23.0,-0.7926654192496692,false,false,0.0004743313040583631,false,0.35],"torque":0.07589948314029431,"active":true,"saturated":false,"log":[-0.13684631884098053,-0.17151404917240143,-0.0023620023857802153,-0.0,0.022749971598386765,0.07589948177337646,0.1806633025407791,0.05348645895719528,0.1528184562921524]},{"input":[true,23.0,-0.9468699518033936,false,false,0.0005319363719904111,false,0.35],"torque":0.0669690006421237,"active":true,"saturated":false,"log":[-0.1432611495256424,-0.17955397069454193,-0.0028634164482355118,-0.0,0.049073103815317154,0.06696899980306625,0.20260392129421234,0.0694655179977417,0.19847290217876434]},{"input":[true,23.0,-1.0994924591074464,false,false,0.000588950454233074,false,0.35],"torque":0.6197823169960004,"active":true,"saturated":false,"log":[-0.8642393350601196,-1.0831799507141113,-0.005888253916054964,-0.0,-0.1450004279613495,0.6197823286056519,0.2243194580078125,-0.5788533091545105,0.3598373532295227]},{"input":[true,23.0,-1.250363376299478,false,false,0.0006453102076715109,false,0.35],"torque":0.5709512871719346,"active":true,"saturated":false,"log":[-0.8074448108673096,-1.0119974613189697,-0.008714310824871063,-0.0,-0.11612781137228012,0.5709512829780579,0.24578575789928436,-0.5046055912971497,0.509888231754303]},{"input":[true,23.0,-1.399315084548177,false,false,0.000700953016155571,false,0.35],"torque":0.5237291207029671,"active":true,"saturated":false,"log":[-0.7535020112991333,-0.9443891644477844,-0.011351567693054676,-0.0,-0.08707331120967865,0.523729145526886,0.2669789791107178,-0.4332810640335083,0.6494187712669373]},{"input":[true,23.0,-1.546182097279171,false,false,0.0007558170600668505,false,0.35],"torque":0.47792492616729143,"active":true,"saturated":false,"log":[-0.7021345496177673,-0.8800086379051208,-0.013809039257466793,-0.0,-0.0577942356467247,0.4779249131679535,0.28787559270858765,-0.3646466135978699,0.779166579246521]},{"input":[true,23.0,-1.690801244031976,false,false,0.0008098413850007994,false,0.35],"torque":0.43220280904506636,"active":true,"saturated":false,"log":[-0.6530870199203491,-0.8185357451438904,-0.013809039257466793,-0.0,-0.02822844684123993,0.4322028160095215,0.30845239758491516,-0.29848796129226685,0.8998174667358398]},{"input":[true,23.0,-1.8330118517437162,false,false,0.0008629659694875654,false,0.35],"torque":0.38772343603857357,"active":true,"saturated":false,"log":[-0.6061232089996338,-0.7596744298934937,-0.013809039257466793,-0.0,0.0014744311338290572,0.387723445892334,0.3286864757537842,-0.234608456492424,1.0120093822479248]},{"input":[true,23.0,-1.9726559232582244,false,false,0.000915131791676342,false,0.35],"torque":0.3445370136978531,"active":true,"saturated":false,"log":[-0.5610249042510986,-0.7031512260437012,-0.013809039257466793,-0.0,0.03094097599387169,0.34453701972961426,0.34855538606643677,-0.17282786965370178,1.1163352727890015]},{"input":[true,23.0,-2.1095783128621823,false,false,0.0009662808949091303,false,0.35],"torque":0.29633973044503786,"active":true,"saturated":false,"log":[-0.5175909399986267,-0.648714005947113,-0.013809039257466793,-0.0,0.07247084379196167,0.2963397204875946,0.36803707480430603,-0.11298125237226486,1.2133466005325317]},{"input":[true,23.0,-2.243626898653285,false,false,0.0010163564521110636,false,0.35],"torque":0.21550325989142644,"active":true,"saturated":false,"log":[-0.475635826587677,-0.5961302518844604,-0.013809039257466793,-0.0,0.1808432936668396,0.2155032604932785,0.3871098458766937,-0.05491787940263748,1.3035563230514526]},{"input":[true,23.0,-2.3746527515489273,false,false,0.0010653028289257553,false,0.35],"torque":0.13774218760806542,"active":true,"saturated":false,"log":[-0.4349888265132904,-0.5451859831809998,-0.013809039257466793,-0.0,0.28473177552223206,0.13774219155311584,0.4057525396347046,0.0014997281832620502,1.3874412775039673]},{"input":[true,23.0,-2.50251030074764,false,false,0.0011130656455255266,false,0.35],"torque":0.06348983185661061,"active":true,"saturated":false,"log":[-0.3954930007457733,-0.49568456411361694,-0.015193264000117779,-0.0,0.38446101546287537,0.06348983198404312,0.4239444434642792,0.056396715342998505,1.4654449224472046]},{"input":[true,23.0,-2.627057495459451,false,false,0.0011595918370278437,false,0.35],"torque":-0.008620148485257252,"active":true,"saturated":false,"log":[-0.3570042848587036,-0.4474453628063202,-0.016442779451608658,-0.0,0.4810520112514496,-0.00862014852464199,0.4416653513908386,0.10988674312829971,1.5379794836044312]},{"input":[true,23.0,-2.748155962725483,false,false,0.0012048297124508391,false,0.35],"torque":-0.07870693781681773,"active":true,"saturated":false,"log":[-0.3193906843662262,-0.4003029763698578,-0.017560645937919617,-0.0,0.574579656124115,-0.07870693504810333,0.4588955342769623,0.1620727926492691,1.6054285764694214]},{"input":[true,23.0,-2.8656711611514445,false,false,0.0012487290121424167,false,0.35],"torque":-0.15198137056371627,"active":true,"saturated":false,"log":[-0.2825314700603485,-0.3541060984134674,-0.01854950748383999,-0.0,0.6752707958221436,-0.15198136866092682,0.475615918636322,0.21304790675640106,1.6681487560272217]},{"input":[true,23.0,-2.9794725303842333,false,false,0.001291240963619141,false,0.35],"torque":-0.22611043979389336,"active":true,"saturated":false,"log":[-0.24631652235984802,-0.30871671438217163,-0.019411614164710045,-0.0,0.7783442735671997,-0.22611044347286224,0.49180784821510315,0.2628958821296692,1.7264714241027832]},{"input":[true,23.0,-3.089433636165562,false,false,0.0013323183357528668,false,0.35],"torque":-0.2984615220878036,"active":true,"saturated":false,"log":[-0.21064552664756775,-0.2640090584754944,-0.020148873329162598,-0.0,0.8784344792366028,-0.29846152663230896,0.5074533820152283,0.31169193983078003,1.7807049751281738]},{"input":[true,23.0,-3.195432310801459,false,false,0.0013719154912449113,false,0.35],"torque":-0.36767267165972095,"active":true,"saturated":false,"log":[-0.17542745172977448,-0.21986907720565796,-0.020762870088219643,-0.0,0.9727169871330261,-0.3676726818084717,0.5225351452827454,0.35950329899787903,1.8311361074447632]},{"input":[true,23.0,-3.297350788891584,false,false,0.0014099884373294702,false,0.35],"torque":-0.42961104661097205,"active":true,"saturated":false,"log":[-0.1405799388885498,-0.17619352042675018,-0.021254900842905045,-0.0,1.052860975265503,-0.42961105704307556,0.5370364189147949,0.4063897430896759,1.8780314922332764]},{"input":[true,23.0,-3.395075838167552,false,false,0.0014464948746499401,true,0.35],"torque":-0.47930039182489614,"active":true,"saturated":false,"log":[-0.10602869093418121,-0.1328892856836319,-0.021626001223921776,-0.0,1.1088658571243286,-0.4793003797531128,0.550940990447998,0.4524042010307312,1.9216388463974]},{"input":[true,23.0,-3.4884988852949266,false,false,0.0014813942442538548,false,0.35],"torque":-0.5104870178433581,"active":true,"saturated":false,"log":[-0.07170704752206802,-0.0898728296160698,-0.021876975893974304,-0.0,1.1281970739364624,-0.5104870200157166,0.5642334222793579,0.4975931644439697,1.9621888399124146]},{"input":[true,23.0,-3.5775161364990793,false,false,0.0015146477726542137,false,0.35],"torque":-0.5411038858824728,"active":true,"saturated":false,"log":[-0.037555448710918427,-0.0470694974064827,-0.022008419036865234,-0.0,1.1464873552322388,-0.5411038994789124,0.5768990516662598,0.5419972538948059,1.9998958110809326]},{"input":[true,23.0,-3.662028692880929,false,false,0.0015462185149071458,false,0.35],"torque":-0.5711957068883788,"active":true,"saturated":false,"log":[-0.0035209772177040577,-0.0044129579328000546,-0.022020742297172546,-0.0,1.1637599468231201,-0.5711957216262817,0.5889236927032471,0.5856515169143677,2.034959077835083]},{"input":[true,23.0,-3.7419426602944377,false,false,0.0015760713956580457,false,0.35],"torque":-0.6007991301613551,"active":true,"saturated":false,"log":[0.03044302575290203,0.03815525770187378,-0.02191419154405594,-0.0,1.1800295114517212,-0.6007991433143616,0.6002940535545349,0.6285859942436218,2.067564010620117]},{"input":[true,23.0,-3.817169253663778,false,false,0.0016041732481105837,false,0.35],"torque":-0.6299430513494046,"active":true,"saturated":false,"log":[0.06437738239765167,0.08068631589412689,-0.02168887108564377,-0.0,1.1953026056289673,-0.6299430727958679,0.6109974980354309,0.6708260178565979,2.0978829860687256]},{"input":[true,23.0,-3.8876248956242767,false,false,0.001630492850875286,false,0.35],"torque":-0.6586497584872636,"active":true,"saturated":false,"log":[0.09831743687391281,0.12322451919317245,-0.021344760432839394,-0.0,1.2095791101455688,-0.658649742603302,0.62102210521698,0.7123925089836121,2.1260759830474854]},{"input":[true,23.0,-3.9532313093775695,false,false,0.0016550009626567592,false,0.35],"torque":-0.6869361623058737,"active":true,"saturated":false,"log":[0.13229340314865112,0.16580773890018463,-0.020881732925772667,-0.0,1.2228548526763916,-0.6869361400604248,0.6303567886352539,0.7533023953437805,2.1522927284240723]},{"input":[true,23.0,-4.013915605657754,false,false,0.0016776703547410072,false,0.35],"torque":-0.7138978727709744,"active":true,"saturated":false,"log":[0.164878711104393,0.20664797723293304,-0.020304657518863678,-0.0,1.2351218461990356,-0.7138978838920593,0.6389910578727722,0.7922195792198181,2.1728155612945557]},{"input":[true,23.0,-4.069610363711976,false,false,0.0016984758412467596,false,0.35],"torque":-0.7395371680077362,"active":true,"saturated":false,"log":[0.19608694314956665,0.24576230347156525,-0.019618352875113487,-0.0,1.2463724613189697,-0.7395371794700623,0.6469154953956604,0.8291470408439636,2.1877543926239014]},{"input":[true,23.0,-4.120253706205427,false,false,0.0017173943071071896,false,0.35],"torque":-0.7638781790150415,"active":true,"saturated":false,"log":[0.2259671539068222,0.28321215510368347,-0.018827468156814575,-0.0,1.256597876548767,-0.7638781666755676,0.6541211605072021,0.8641216158866882,2.197314977645874]},{"input":[true,23.0,-4.165789367967588,false,false,0.0017344047337509473,false,0.35],"torque":-0.7869422112473686,"active":true,"saturated":false,"log":[0.25456398725509644,0.3190535306930542,-0.017936494201421738,-0.0,1.2657890319824219,-0.786942183971405,0.6606000661849976,0.8971767425537109,2.201693058013916]},{"input":[true,23.0,-4.206166758503289,false,false,0.0017494882224539628,false,0.35],"torque":-0.8087482302651351,"active":true,"saturated":false,"log":[0.2819180190563202,0.35333725810050964,-0.016949782148003578,-0.0,1.2739373445510864,-0.8087482452392578,0.6663450598716736,0.9283429980278015,2.2010762691497803]},{"input":[true,23.0,-4.241341018199168,false,false,0.0017626280153360862,false,0.35],"torque":-0.8293132954989303,"active":true,"saturated":false,"log":[0.308066189289093,0.3861096203327179,-0.015871549025177956,-0.0,1.2810344696044922,-0.8293132781982422,0.6713497638702393,0.9576482176780701,2.195643663406372]},{"input":[true,23.0,-4.271273068163066,false,false,0.0017738095139792282,false,0.35],"torque":-0.8486528171782661,"active":true,"saturated":false,"log":[0.33304205536842346,0.4174126982688904,-0.014705902896821499,-0.0,1.2870732545852661,-0.8486528396606445,0.6756085753440857,0.9851180911064148,2.185567617416382]},{"input":[true,23.0,-4.295929653641007,false,false,0.0017830202956463255,false,0.35],"torque":-0.866780756094078,"active":true,"saturated":false,"log":[0.356876015663147,0.44728460907936096,-0.013456836342811584,-0.0,1.2920475006103516,-0.8667807579040527,0.6791167855262756,1.010776162147522,2.1710140705108643]},{"input":[true,23.0,-4.315283380963495,false,false,0.0017902501270831014,false,0.35],"torque":-0.883709922458402,"active":true,"saturated":false,"log":[0.37959572672843933,0.47575998306274414,-0.012128251604735851,-0.0,1.2959517240524292,-0.8837099075317383,0.6818704605102539,1.0346442461013794,2.1521425247192383]},{"input":[true,23.0,-4.329312747980113,false,false,0.001795490975887298,false,0.35],"torque":-0.899452110432459,"active":true,"saturated":false,"log":[0.401226282119751,0.5028702616691589,-0.010723959654569626,-0.0,1.2987818717956543,-0.8994520902633667,0.6838666200637817,1.056742548942566,2.129107713699341]},{"input":[true,23.0,-4.3380021679485825,false,false,0.0017987370194327387,false,0.35],"torque":-0.9140182425753505,"active":true,"saturated":false,"log":[0.4217904210090637,0.5286439657211304,-0.009247693233191967,-0.0,1.300534963607788,-0.9140182137489319,0.6851029396057129,1.0770899057388306,2.1020596027374268]},{"input":[true,23.0,-4.341341986851766,false,false,0.0017999846513383159,false,0.35],"torque":-0.9274185651214999,"active":true,"saturated":false,"log":[0.4413088262081146,0.5531070828437805,-0.007703111972659826,-0.0,1.3012092113494873,-0.92741858959198,0.6855781674385071,1.095704436302185,2.071143627166748]},{"input":[true,23.0,-4.339328494123363,false,false,0.001799232485474709,false,0.35],"torque":-0.9396627645308331,"active":true,"saturated":false,"log":[0.4598003029823303,0.5762830376625061,-0.006093810778111219,-0.0,1.3008036613464355,-0.939662754535675,0.6852916479110718,1.1126028299331665,2.03650164604187]},{"input":[true,23.0,-4.331963926770374,false,false,0.0017964813575043867,false,0.35],"torque":-0.9507600251266474,"active":true,"saturated":false,"log":[0.4772818684577942,0.5981932878494263,-0.0044233244843780994,-0.0,1.2993191480636597,-0.950760006904602,0.6842437982559204,1.1278012990951538,1.9982719421386719]},{"input":[true,23.0,-4.31925646688777,false,false,0.001791734323953177,false,0.35],"torque":-0.9607192917835721,"active":true,"saturated":false,"log":[0.49376916885375977,0.6188573837280273,-0.002695132279768586,-0.0,1.2967571020126343,-0.9607192873954773,0.6824357509613037,1.1413155794143677,1.956590175628662]},{"input":[true,23.0,-4.301220232568118,false,false,0.0017849966588144434,false,0.35],"torque":-0.9695491743683607,"active":true,"saturated":false,"log":[0.5092763304710388,0.6382930278778076,-0.0009126652148552239,-0.0,1.2931203842163086,-0.9695491790771484,0.679869532585144,1.153160810470581,1.9115886688232422]},{"input":[true,23.0,-4.277875262216261,false,false,0.0017762758476896352,false,0.35],"torque":-0.9772582645219703,"active":true,"saturated":false,"log":[0.5238164663314819,0.6565166115760803,0.0009206924587488174,-0.0,1.2884132862091064,-0.9772582650184631,0.6765479445457458,1.1633518934249878,1.8633980751037598]},{"input":[true,23.0,-4.2492474922864885,false,false,0.0017655815794717213,false,0.35],"torque":-0.9838550196070691,"active":true,"saturated":false,"log":[0.5374014973640442,0.6735432147979736,0.002801597584038973,-0.0,1.282640814781189,-0.9838550090789795,0.67247474193573,1.1719038486480713,1.8121464252471924]},{"input":[true,23.0,-4.215368728466922,false,false,0.0017529257355807513,false,0.35],"torque":-0.9893479840249145,"active":true,"saturated":false,"log":[0.5500425100326538,0.6893866062164307,0.0047267465852200985,-0.0,1.275809407234192,-0.9893479943275452,0.6676543354988098,1.1788312196731567,1.7579600811004639]},{"input":[true,23.0,-4.176276610343131,false,false,0.0017383223767634996,false,0.35],"torque":-0.9937458222042719,"active":true,"saturated":false,"log":[0.5617498159408569,0.7040597796440125,0.006692870985716581,-0.0,1.267926812171936,-0.9937458038330078,0.6620922088623047,1.184149146080017,1.700964093208313]},{"input":[true,23.0,-4.132014569580246,false,false,0.00172178772747186,false,0.35],"torque":-0.9970573132913533,"active":true,"saturated":false,"log":[0.5725329518318176,0.7175746560096741,0.008696735836565495,0.0,1.2590017318725586,-0.997057318687439,0.6557945013046265,1.1878726482391357,1.6412817239761353]},{"input":[true,23.0,-4.0826317816700195,false,false,0.001703340157837346,false,0.35],"torque":-0.9992915708412093,"active":true,"saturated":false,"log":[0.5824010372161865,0.7299426198005676,0.010735140182077885,0.0,1.2490440607070923,-0.9992915987968445,0.6487681865692139,1.1900172233581543,1.5790350437164307]},{"input":[true,23.0,-4.0281831112964515,false,false,0.001683000163261724,false,0.35],"torque":-0.9994184678779714,"active":true,"saturated":false,"log":[0.5913626551628113,0.7411745190620422,0.010735140182077885,0.0,1.2380647659301758,-0.9994184970855713,0.6410211324691772,1.190598487854004,1.5143451690673828]},{"input":[true,23.0,-3.968729051380671,false,false,0.0016607903416464523,false,0.35],"torque":-0.9995267169975299,"active":true,"saturated":false,"log":[0.5994260311126709,0.7512806057929993,0.012833130545914173,0.0,1.2260762453079224,-0.9995267391204834,0.6325618028640747,1.189632773399353,1.4473323822021484]},{"input":[true,23.0,-3.9043356558728046,false,false,0.001636735368286227,false,0.35],"torque":-0.9985869392271692,"active":true,"saturated":false,"log":[0.6065990924835205,0.7602708339691162,0.014956227503716946,0.0,1.2130917310714722,-0.9985869526863098,0.6233997941017151,1.1871370077133179,1.3781160116195679]},{"input":[true,23.0,-3.835074466365495,false,false,0.0016108619684545263,false,0.35],"torque":-0.9966097432250223,"active":true,"saturated":false,"log":[0.6128896474838257,0.768155038356781,0.017101341858506203,0.0,1.1991255283355713,-0.9966097474098206,0.6135451197624207,1.1831283569335938,1.3068145513534546]},{"input":[true,23.0,-3.7610224326105985,false,false,0.001583198887711611,false,0.35],"torque":-0.9936061858372657,"active":true,"saturated":false,"log":[0.6183053255081177,0.7749426960945129,0.01926540955901146,0.0,1.1841933727264404,-0.9936062097549438,0.6030088067054749,1.177625060081482,1.233546257019043]},{"input":[true,23.0,-3.68226182702738,false,false,0.0015537768599679727,false,0.35],"torque":-0.9895878383528661,"active":true,"saturated":false,"log":[0.622853696346283,0.7806432843208313,0.021445397287607193,0.0,1.1683117151260376,-0.9895878434181213,0.5918025374412537,1.170645833015442,1.1584285497665405]},{"input":[true,23.0,-3.5988801532971806,false,false,0.0015226285733387025,false,0.35],"torque":-0.984566814718894,"active":true,"saturated":false,"log":[0.626542329788208,0.7852663993835449,0.023638296872377396,0.0,1.1514981985092163,-0.9845668077468872,0.5799387693405151,1.1622099876403809,1.0815784931182861]},{"input":[true,23.0,-3.5109700491460987,true,false,0.0014897886338267289,false,0.35],"torque":-0.9774494810256518,"active":true,"saturated":false,"log":[0.6293788552284241,0.7888215184211731,0.023638296872377396,0.0,1.133771538734436,-0.9774494767189026,0.5674306750297546,1.1523381471633911,1.0031126737594604]},{"input":[true,23.0,-3.4186291834237204,true,false,0.001455293526875262,false,0.35],"torque":-0.969352047933974,"active":true,"saturated":false,"log":[0.6313712000846863,0.7913185954093933,0.023638296872377396,0.0,1.1151514053344727,-0.9693520665168762,0.5542922019958496,1.141051173210144,0.9231472611427307]},{"input":[true,23.0,-3.321960147592226,true,false,0.0014191815768321676,false,0.35],"torque":-0.9602898158477412,"active":true,"saturated":false,"log":[0.6325271725654602,0.7927674055099487,0.023638296872377396,0.0,1.0956584215164185,-0.9602898359298706,0.5405378937721252,1.1283711194992065,0.8417981863021851]},{"input":[true,23.0,-3.2210703417464197,true,false,0.0013814929043712983,false,0.35],"torque":-0.9502788292427509,"active":true,"saturated":false,"log":[0.6328549981117249,0.7931782603263855,0.023638296872377396,0.0,1.0753144025802612,-0.9502788186073303,0.526183009147644,1.114320993423462,0.7591808438301086]},{"input":[true,23.0,-3.116071855291351,true,false,0.0013422693819180966,false,0.35],"torque":-0.9393357312800878,"active":true,"saturated":false,"log":[0.6323631405830383,0.7925618290901184,0.023638296872377396,0.0,1.054141640663147,-0.9393357038497925,0.5112435817718506,1.0989243984222412,0.6754104495048523]},{"input":[true,23.0,-3.007081342410053,true,false,0.0013015545871289842,false,0.35],"torque":-0.9274776425723519,"active":true,"saturated":false,"log":[0.6310603022575378,0.7909289002418518,0.023638296872377396,0.0,1.0321635007858276,-0.9274776577949524,0.49573612213134766,1.082206130027771,0.5906016826629639]},{"input":[true,23.0,-2.8942198924597977,true,false,0.0012593937544762292,false,0.35],"torque":-0.9147216322014112,"active":true,"saturated":false,"log":[0.6289554834365845,0.7882908582687378,0.023638296872377396,0.0,1.0094026327133179,-0.9147216081619263,0.47967788577079773,1.0641918182373047,0.5048691034317017]},{"input":[true,23.0,-2.777612895440809,true,false,0.001215833724992072,false,0.35],"torque":-0.9010820786792083,"active":true,"saturated":false,"log":[0.6260581016540527,0.7846595048904419,0.023638296872377396,0.0,0.9858759045600891,-0.9010820984840393,0.4630867540836334,1.044908046722412,0.41832658648490906]},{"input":[true,23.0,-2.657389902686947,true,false,0.0011709228942279536,false,0.35],"torque":-0.88655823681615,"active":true,"saturated":false,"log":[0.6223779916763306,0.7800470590591431,0.023638296872377396,0.0,0.9615694284439087,-0.8865582346916199,0.44598111510276794,1.0243823528289795,0.33108779788017273]},{"input":[true,23.0,-2.5336844829331087,true,false,0.0011247111584866602,false,0.35],"torque":-0.8710822524401842,"active":true,"saturated":false,"log":[0.6179253458976746,0.7744664549827576,0.023638296872377396,0.0,0.9363353252410889,-0.8710822463035583,0.42837998270988464,1.00264310836792,0.24326588213443756]},{"input":[true,23.0,-2.4066340739192658,true,false,0.0010772498593871215,false,0.35],"torque":-0.8543466537176907,"active":true,"saturated":false,"log":[0.6127107739448547,0.7679308652877808,0.023638296872377396,0.0,0.9095481634140015,-0.8543466329574585,0.41030293703079224,0.9797199964523315,0.15497352182865143]},{"input":[true,23.0,-2.276379829695999,true,false,0.0010285917268234453,false,0.35],"torque":-0.8356091822281853,"active":true,"saturated":false,"log":[0.6067454218864441,0.7604542374610901,0.023638296872377396,0.0,0.8797159194946289,-0.8356091976165771,0.39177000522613525,0.9556432366371155,0.06632284075021744]},{"input":[true,23.0,-2.1430664638011843,true,false,0.0009787908203815724,false,0.35],"torque":-0.8143601562236795,"active":true,"saturated":false,"log":[0.6000407338142395,0.752051055431366,0.023638296872377396,0.0,0.8458095192909241,-0.8143601417541504,0.37280184030532837,0.9304441213607788,-0.02257462963461876]},{"input":[true,23.0,-2.006842088482073,true,false,0.0009279024692786355,false,0.35],"torque":-0.7923587375563863,"active":true,"saturated":false,"log":[0.5926087498664856,0.742736279964447,0.023638296872377396,0.0,0.8113165497779846,-0.7923587560653687,0.3534194827079773,0.9041549563407898,-0.11160798370838165]},{"input":[true,23.0,-1.8678580501413486,true,false,0.0008759832108917369,false,0.35],"torque":-0.7722624966613344,"active":true,"saturated":false,"log":[0.58446204662323,0.7325257658958435,0.023638296872377396,0.0,0.7815127968788147,-0.7722625136375427,0.33364447951316833,0.8768088817596436,-0.20066702365875244]},{"input":[true,23.0,-1.7262687611900407,true,false,0.0008230907279444615,false,0.35],"torque":-0.7526333154704983,"active":true,"saturated":false,"log":[0.5756135582923889,0.7214356660842896,0.023638296872377396,0.0,0.7535185813903809,-0.7526333332061768,0.3134987950325012,0.8484399318695068,-0.2896423041820526]},{"input":[true,23.0,-1.5822315284940551,false,false,0.0007692837844208935,true,0.35],"torque":-0.7313122139370073,"active":true,"saturated":false,"log":[0.5660768747329712,0.7094830274581909,0.020891906693577766,0.0,0.7257645130157471,-0.7313122153282166,0.2930048108100891,0.819083034992218,-0.3784252107143402]},{"input":[true,23.0,-1.4359063786049613,false,false,0.0007146221602783498,false,0.35],"torque":-0.711806525682201,"active":true,"saturated":false,"log":[0.5558658838272095,0.696685254573822,0.022837435826659203,0.0,0.6977783441543579,-0.7118065357208252,0.2721852958202362,0.7887740731239319,-0.4669080078601837]},{"input":[true,23.0,-1.2874558799691633,false,false,0.0006591665850313498,false,0.35],"torque":-0.6916941672493473,"active":true,"saturated":false,"log":[0.5449951887130737,0.6830606460571289,0.024744920432567596,0.0,0.6694490909576416,-0.6916941404342651,0.25106337666511536,0.7575495839118958,-0.5549840331077576]},{"input":[true,23.0,-1.1370449623130268,false,false,0.0006029786702806292,false,0.35],"torque":-0.6709787561004691,"active":true,"saturated":false,"log":[0.5334798097610474,0.6686280369758606,0.02661209926009178,0.0,0.6407673954963684,-0.6709787845611572,0.22966252267360687,0.7254469990730286,-0.6425474882125854]},{"input":[true,23.0,-0.9848407334045886,false,false,0.000546120841262141,false,0.35],"torque":-0.6496787476784788,"active":true,"saturated":false,"log":[0.5213353037834167,0.6534069180488586,0.028436772525310516,0.0,0.6117526888847351,-0.6496787667274475,0.20800650119781494,0.6925045847892761,-0.7294938564300537]},{"input":[true,23.0,-0.8310122933954589,false,false,0.0004886562674921086,false,0.35],"torque":-0.6278171894923249,"active":true,"saturated":false,"log":[0.5085777044296265,0.6374173760414124,0.030216794461011887,0.0,0.5824329257011414,-0.6278172135353088,0.18611940741539001,0.6587613224983215,-0.815719723701477]},{"input":[true,23.0,-0.6757305469491508,false,false,0.00043064879258516834,false,0.35],"torque":-0.6034457734740795,"active":true,"saturated":false,"log":[0.4952235221862793,0.6206801533699036,0.031950075179338455,0.0,0.5489102005958557,-0.6034457683563232,0.16402551531791687,0.6242568492889404,-0.9011229872703552]},{"input":[true,23.0,-0.519168013364596,false,false,0.0003721628633235905,false,0.35],"torque":-0.5593218230206918,"active":true,"saturated":false,"log":[0.4812898337841034,0.6032165884971619,0.03363459184765816,0.0,0.47683262825012207,-0.5593218207359314,0.14174939692020416,0.5890316367149353,-0.9856029152870178]},{"input":[true,23.0,-0.3614986349057823,false,false,0.0003132634580563725,false,0.35],"torque":-0.5144901894324285,"active":true,"saturated":false,"log":[0.4667942225933075,0.5850487351417542,0.03526837006211281,0.0,0.4041009545326233,-0.5144901871681213,0.11931578814983368,0.5531266331672668,-1.069060206413269]},{"input":[true,23.0,-0.20289758355047277,false,false,0.00025401601450776096,false,0.35],"torque":-0.46899963013379437,"active":true,"saturated":false,"log":[0.451754629611969,0.5661991238594055,0.03684951364994049,0.0,0.330791711807251,-0.46899962425231934,0.09674961864948273,0.5165835618972778,-1.1513969898223877]},{"input":[true,23.0,-0.043541066372683734,false,false,0.00019448635707539666,false,0.35],"torque":-0.42289653216090656,"active":true,"saturated":false,"log":[0.43618956208229065,0.5466909408569336,0.03837617486715317,0.0,0.256975919008255,-0.42289653420448303,0.07407596707344055,0.479444682598114,-1.2325172424316406]},{"input":[true,23.0,0.11639387022481873,false,false,0.00013474062369886897,false,0.35],"torque":-0.3762235500486624,"active":true,"saturated":false,"log":[0.4201180040836334,0.526547908782959,0.03984658792614937,0.0,0.18271644413471222,-0.3762235641479492,0.05132000893354416,0.44175276160240173,-1.312326192855835]},{"input":[true,23.0,0.2767295372105245,false,false,7.484519237992288e-05,false,0.35],"torque":-0.3290183902801556,"active":true,"saturated":false,"log":[0.40355929732322693,0.50579434633255,0.0412590466439724,0.0,0.10806585103273392,-0.32901838421821594,0.028507038950920105,0.4035511314868927,-1.3907313346862793]},{"input":[true,23.0,0.4372878003381685,false,false,1.4866607435947705e-05,false,0.35],"torque":-0.2813138095473039,"active":true,"saturated":false,"log":[0.38653329014778137,0.48445504903793335,0.04261191189289093,0.0,0.03306613117456436,-0.28131380677223206,0.005662395153194666,0.36488351225852966,-1.4676415920257568]},{"input":[true,23.0,0.5978902780553368,false,false,-4.5128494431304356e-05,false,0.35],"torque":-0.23851135355249992,"active":true,"saturated":false,"log":[0.3701696991920471,0.46394601464271545,0.04390750825405121,0.0,-0.03294576331973076,-0.2385113537311554,-0.0171885397285223,0.32682523131370544,-1.5400222539901733]},{"input":[true,23.0,0.7583585396868447,false,false,-0.00010507345816964415,false,0.35],"torque":-0.19893228630126,"active":true,"saturated":false,"log":[0.3546616733074188,0.4445092976093292,0.04390750825405121,0.0,-0.09231618046760559,-0.19893229007720947,-0.040020376443862915,0.28958114981651306,-1.607328176498413]},{"input":[true,23.0,0.9185143036726777,false,false,-0.0001649016844307542,false,0.35],"torque":-0.16075623252456758,"active":true,"saturated":false,"log":[0.3398642838001251,0.42596322298049927,0.04390750825405121,0.0,-0.1497836858034134,-0.16075623035430908,-0.06280775368213654,0.25304195284843445,-1.6699150800704956]},{"input":[true,23.0,1.0781796356402849,false,false,-0.0002245467035626178,false,0.35],"torque":-0.12380217781427579,"active":true,"saturated":false,"log":[0.3256431519985199,0.4081394076347351,0.04390750825405121,0.0,-0.205540269613266,-0.12380217760801315,-0.08552534878253937,0.2171081006526947,-1.7281138896942139]},{"input":[true,23.0,1.2371771460911236,false,false,-0.00028394224945784677,false,0.35],"torque":-0.08791474598145668,"active":true,"saturated":false,"log":[0.3118739426136017,0.3908820152282715,0.04390750825405121,0.0,-0.2597395181655884,-0.08791474252939224,-0.10814791917800903,0.18168920278549194,-1.7822322845458984]},{"input":[true,23.0,1.3953301874818587,false,true,-0.0003430223331758737,false,0.35],"torque":-0.0478968760894493,"active":true,"saturated":false,"log":[0.29844146966934204,0.37404665350914,0.04390750825405121,0.0,-0.3225850462913513,-0.04789687693119049,-0.1306503415107727,0.14670346677303314,-1.8325563669204712]},{"input":[true,23.0,1.5524630504812256,false,true,-0.00040172131625720533,false,0.35],"torque":-0.008476276753363259,"active":true,"saturated":false,"log":[0.28523948788642883,0.3575001657009125,0.04390750825405121,0.0,-0.384530246257782,-0.008476276881992817,-0.1530076116323471,0.11207703500986099,-1.87935209274292]},{"input":[true,23.0,1.7084011591845623,false,true,-0.0004599739836482962,false,0.35],"torque":0.030233705400546488,"active":true,"saturated":false,"log":[0.2721698582172394,0.34111955761909485,0.04390750825405121,0.0,-0.44522643089294434,0.030233705416321754,-0.1751948893070221,0.07774363458156586,-1.922866940498352]},{"input":[true,23.0,1.8629712650690626,false,true,-0.000517715616156007,false,0.35],"torque":0.0683800234534,"active":true,"saturated":false,"log":[0.25914230942726135,0.32479169964790344,0.04390750825405121,0.0,-0.5048530101776123,0.06838002055883408,-0.19718752801418304,0.04364395886659622,-1.963330864906311]},{"input":[true,23.0,2.0160016394743216,false,true,-0.0005748820623511629,false,0.35],"torque":0.10606671063932611,"active":true,"saturated":false,"log":[0.24607378244400024,0.30841246247291565,0.04390750825405121,0.0,-0.5635131001472473,0.10606671124696732,-0.21896107494831085,0.00972529873251915,-2.000957727432251]},{"input":[true,23.0,2.1673222643942767,false,true,-0.0006314098098413157,false,0.35],"torque":0.14326159901684513,"active":true,"saturated":false,"log":[0.23288817703723907,0.29188650846481323,0.04390750825405121,0.0,-0.6210471391677856,0.1432615965604782,-0.2404913604259491,-0.024058910086750984,-2.0359466075897217]},{"input":[true,23.0,2.3167650213686324,false,true,-0.0006872360558335404,false,0.35],"torque":0.18019942295472516,"active":true,"saturated":false,"log":[0.2195158302783966,0.27512651681900024,0.04390750825405121,0.0,-0.6778352856636047,0.18019942939281464,-0.26175445318222046,-0.05774947628378868,-2.0684821605682373]},{"input":[true,23.0,2.4641638782638386,false,false,-0.0007422987769088523,false,0.35],"torque":0.21659859661173322,"active":true,"saturated":false,"log":[0.20589320361614227,0.2580528259277344,0.04462813213467598,0.0,-0.733957827091217,0.21659860014915466,-0.2827267646789551,-0.09138181805610657,-2.0987367630004883]},{"input":[true,23.0,2.609355073736162,false,false,-0.0007965367979307344,false,0.35],"torque":0.25293339897714584,"active":true,"saturated":false,"log":[0.19196255505084991,0.2405930757522583,0.04530000314116478,0.0,-0.7895174026489258,0.25293341279029846,-0.303384929895401,-0.12498631328344345,-2.1268701553344727]},{"input":[true,23.0,2.7521772991718905,false,false,-0.000849889860011222,false,0.35],"torque":0.28930142907274614,"active":true,"saturated":false,"log":[0.17767158150672913,0.22268171608448029,0.04592185094952583,0.0,-0.8446414470672607,0.28930142521858215,-0.3237060606479645,-0.15858861804008484,-2.1530308723449707]},{"input":[true,23.0,2.892471877902531,false,false,-0.0009022986874590205,false,0.35],"torque":0.3276356543873958,"active":true,"saturated":false,"log":[0.16297310590744019,0.20425961911678314,0.046492259949445724,0.0,-0.9031182527542114,0.32763564586639404,-0.34366753697395325,-0.19220998883247375,-2.1773576736450195]},{"input":[true,23.0,3.0300829414958925,false,false,-0.0009537050536352881,false,0.35],"torque":0.36466066491858345,"active":true,"saturated":false,"log":[0.14782486855983734,0.18527384102344513,0.04700964689254761,0.0,-0.9583714604377747,0.3646606504917145,-0.36324718594551086,-0.22586750984191895,-2.1999785900115967]},{"input":[true,23.0,3.164857602927192,false,false,-0.001004051845643908,false,0.35],"torque":0.38828185684154193,"active":true,"saturated":false,"log":[0.13218919932842255,0.16567713022232056,0.047472309321165085,0.0,-0.9862703084945679,0.38828185200691223,-0.3824232518672943,-0.2595744729042053,-2.2210137844085693]},{"input":[true,23.0,3.2966461264378126,false,false,-0.0010532831277843902,false,0.35],"torque":0.41198816012675593,"active":true,"saturated":false,"log":[0.11603277921676636,0.14542774856090546,0.04787842184305191,0.0,-1.0136293172836304,0.41198816895484924,-0.40117448568344116,-0.2933405041694641,-2.2405738830566406]},{"input":[true,23.0,3.425302093892954,false,false,-0.001101344203696894,false,0.35],"torque":0.435754011250044,"active":true,"saturated":false,"log":[0.09932643920183182,0.12448913604021072,0.048226065933704376,0.0,-1.0403592586517334,0.4357540011405945,-0.41947996616363525,-0.3271718919277191,-2.258762836456299]},{"input":[true,23.0,1.8155478628180524,false,false,-0.0005,false,0.35],"torque":0.4439138061621256,"active":true,"saturated":false,"log":[-0.17039629817008972,-0.21356336772441864,0.04762967675924301,-0.0,-0.717957615852356,0.4439138174057007,-0.19043999910354614,-0.34879621863365173,-2.240603446960449]},{"input":[true,23.0,1.8125746014992508,false,false,-0.0004988893003505612,false,0.35],"torque":0.45825753476936404,"active":true,"saturated":false,"log":[-0.19292829930782318,-0.24180346727371216,0.046954430639743805,-0.0,-0.7176024913787842,0.4582575261592865,-0.1900169551372528,-0.3693130612373352,-2.217897653579712]},{"input":[true,23.0,1.8036680271440637,false,false,-0.0004955621360170897,false,0.35],"torque":0.47182091358711736,"active":true,"saturated":false,"log":[-0.2151404470205307,-0.2696426808834076,0.0462014377117157,-0.0,-0.7160167098045349,0.4718209207057953,-0.18874970078468323,-0.38868847489356995,-2.190638780593872]},{"input":[true,23.0,1.7888677098685468,false,false,-0.0004900332889206208,false,0.35],"torque":0.48458341368829577,"active":true,"saturated":false,"log":[-0.23699279129505157,-0.2970309555530548,0.04537196084856987,-0.0,-0.7132108211517334,0.48458340764045715,-0.18664388358592987,-0.4068909287452698,-2.1588287353515625]},{"input":[true,23.0,1.7682394045015388,false,false,-0.00048232732261528193,false,0.35],"torque":0.4965268458176192,"active":true,"saturated":false,"log":[-0.25844424962997437,-0.32391679286956787,0.04446740821003914,-0.0,-0.7092013359069824,0.496526837348938,-0.18370883166790009,-0.4238916039466858,-2.1224780082702637]},{"input":[true,23.0,1.741874758449198,false,false,-0.00047247847315736883,false,0.35],"torque":0.5076352129043366,"active":true,"saturated":false,"log":[-0.279452919960022,-0.35024765133857727,0.04348932206630707,-0.0,-0.7040106058120728,0.507635235786438,-0.17995759844779968,-0.4396645724773407,-2.0816071033477783]},{"input":[true,23.0,1.7098909045240358,false,false,-0.00046053049700144256,false,0.35],"torque":0.5178945812026869,"active":true,"saturated":false,"log":[-0.29997625946998596,-0.3759702444076538,0.042439404875040054,-0.0,-0.6976658701896667,0.5178945660591125,-0.17540685832500458,-0.4541870057582855,-2.0362462997436523]},{"input":[true,23.0,1.6724299405474223,false,false,-0.00044653647659921467,false,0.35],"torque":0.5272931230881547,"active":true,"saturated":false,"log":[-0.3199714124202728,-0.40103083848953247,0.041319504380226135,-0.0,-0.6901991367340088,0.5272931456565857,-0.17007681727409363,-0.4674392640590668,-1.9864356517791748]},{"input":[true,23.0,1.6296582980375824,false,false,-0.00043055858456490516,false,0.35],"torque":0.5358211445569343,"active":true,"saturated":false,"log":[-0.3393954336643219,-0.4253756105899811,0.040131621062755585,-0.0,-0.6816468834877014,0.5358211398124695,-0.16399115324020386,-0.47940513491630554,-1.932226538658142]},{"input":[true,23.0,1.5817660027878835,false,false,-0.0004126678074548392,false,0.35],"torque":0.5434711686533765,"active":true,"saturated":false,"log":[-0.35820555686950684,-0.44895097613334656,0.03887790068984032,-0.0,-0.6720499992370605,0.5434711575508118,-0.1571769118309021,-0.4900718927383423,-1.873680830001831]},{"input":[true,23.0,1.5289658306205038,false,false,-0.00039294363038847404,false,0.35],"torque":0.5502379739248248,"active":true,"saturated":false,"log":[-0.37635937333106995,-0.47170373797416687,0.03756064549088478,-0.0,-0.6614535450935364,0.5502379536628723,-0.14966437220573425,-0.49943044781684875,-1.8108712434768677]},{"input":[true,23.0,1.47149236206631,false,false,-0.00037147368391202205,false,0.35],"torque":0.5561187464185521,"active":true,"saturated":false,"log":[-0.39381518959999084,-0.49358171224594116,0.03618229180574417,-0.0,-0.6499066352844238,0.5561187267303467,-0.14148689806461334,-0.5074753761291504,-1.7438818216323853]},{"input":[true,23.0,1.4096009401708025,false,false,-0.0003483533546735827,false,0.35],"torque":0.5611130570595776,"active":true,"saturated":false,"log":[-0.4105321168899536,-0.5145335793495178,0.03474542871117592,-0.0,-0.6374622583389282,0.5611130595207214,-0.13268081843852997,-0.5142050385475159,-1.672808051109314]},{"input":[true,23.0,1.343566536056392,false,false,-0.0003236853616394762,false,0.35],"torque":0.565222980419966,"active":true,"saturated":false,"log":[-0.42647039890289307,-0.534509539604187,0.0332527831196785,-0.0,-0.6241770386695862,0.565222978591919,-0.12328527867794037,-0.5196215510368347,-1.597756266593933]},{"input":[true,23.0,1.2736825272810812,false,false,-0.0002975792997345639,false,0.35],"torque":0.568453090520475,"active":true,"saturated":false,"log":[-0.4415915608406067,-0.5534614324569702,0.03170721232891083,-0.0,-0.6101111769676208,0.5684530735015869,-0.11334200203418732,-0.523730993270874,-1.5188441276550293]},{"input":[true,23.0,1.200259394421063,false,false,-0.00027015115293406986,false,0.35],"torque":0.570810502346264,"active":true,"saturated":false,"log":[-0.45585864782333374,-0.5713428258895874,0.030111705884337425,-0.0,-0.595328152179718,0.5708104968070984,-0.10289517045021057,-0.526543140411377,-1.436200499534607]},{"input":[true,23.0,1.123623341668051,false,false,-0.00024152277897013832,false,0.35],"torque":0.5723048465627396,"active":true,"saturated":false,"log":[-0.4692363739013672,-0.5881096124649048,0.028469379991292953,-0.0,-0.5798945426940918,0.5723048448562622,-0.09199119359254837,-0.5280716419219971,-1.3499650955200195]},{"input":[true,23.0,1.044114847569753,false,false,-0.0002118213679424723,false,0.35],"torque":0.572948346743726,"active":true,"saturated":false,"log":[-0.4816914498806,-0.60371994972229,0.02678345888853073,-0.0,-0.5638795495033264,0.5729483366012573,-0.08067852258682251,-0.5283339619636536,-1.2602885961532593]},{"input":[true,23.0,0.962087152352259,false,false,-0.00018117887723833681,false,0.35],"torque":0.5727556414334323,"active":true,"saturated":false,"log":[-0.49319252371788025,-0.618134617805481,0.025057286024093628,-0.0,-0.5473549962043762,0.5727556347846985,-0.06900741159915924,-0.5273512601852417,-1.1673316955566406]},{"input":[true,23.0,0.8779046885448585,false,false,-0.00014973144527246395,true,0.35],"torque":0.5717438746284854,"active":true,"saturated":false,"log":[-0.5037105679512024,-0.6313172578811646,0.023294297978281975,-0.0,-0.5303947925567627,0.5717438459396362,-0.057029709219932556,-0.5251483917236328,-1.0712655782699585]},{"input":[true,23.0,0.7919414618797103,false,false,-0.00011761878665149471,false,0.35],"torque":0.56993248492839,"active":true,"saturated":false,"log":[-0.5132188200950623,-0.6432342529296875,0.02149803191423416,-0.0,-0.5130748152732849,0.5699324607849121,-0.04479864239692688,-0.5217537879943848,-0.9722713232040405]},{"input":[true,23.0,0.7045793896597009,false,false,-8.498357145012052e-05,false,0.35],"torque":0.5673432453363652,"active":true,"saturated":false,"log":[-0.5216931700706482,-0.6538554430007935,0.019672106951475143,-0.0,-0.4954721927642822,0.5673432350158691,-0.03236854076385498,-0.5171992778778076,-0.8705390691757202]},{"input":[true,23.0,0.6162066039767866,false,false,-5.197079135667549e-05,false,0.35],"torque":0.5640000155071863,"active":true,"saturated":false,"log":[-0.5291121602058411,-0.663153886795044,0.017820212990045547,-0.0,-0.4776650071144104,0.5640000104904175,-0.019794633612036705,-0.5115200877189636,-0.7662681937217712]},{"input":[true,23.0,0.5272157273192786,false,false,-1.872711550426666e-05,false,0.35],"torque":0.5599284933816062,"active":true,"saturated":false,"log":[-0.5354570150375366,-0.6711061000823975,0.01594611443579197,-0.0,-0.4597317576408386,0.5599284768104553,-0.00713278166949749,-0.5047547817230225,-0.6596662402153015]},{"input":[true,23.0,0.4380021282292046,false,false,1.4599761150644409e-05,false,0.35],"torque":0.5551562580777606,"active":true,"saturated":false,"log":[-0.5407118201255798,-0.677692174911499,0.01405362319201231,-0.0,-0.441751092672348,0.5551562309265137,0.005560758989304304,-0.49694472551345825,-0.5509489178657532]},{"input":[true,23.0,0.3489621647595025,false,false,4.786177400718783e-05,false,0.35],"torque":0.5497139450548594,"active":true,"saturated":false,"log":[-0.544863760471344,-0.6828958988189697,0.01214659959077835,-0.0,-0.4238039553165436,0.5497139692306519,0.018229594454169273,-0.4881344437599182,-0.44033899903297424]},{"input":[true,23.0,0.26049142353501964,false,false,8.091114664128603e-05,false,0.35],"torque":0.5436397776544658,"active":true,"saturated":false,"log":[-0.5479028820991516,-0.6867049336433411,0.010228939354419708,-0.0,-0.4059827923774719,0.5436397790908813,0.030817439779639244,-0.4783709943294525,-0.3280661106109619]},{"input":[true,23.0,0.1729829622408104,false,false,0.00011360104734654355,false,0.35],"torque":0.5369917098423258,"active":true,"saturated":false,"log":[-0.5498225092887878,-0.6891108751296997,0.00830456055700779,-0.0,-0.38841530680656433,0.5369917154312134,0.043268367648124695,-0.46770408749580383,-0.21436578035354614]},{"input":[true,23.0,0.08682556334600455,false,false,0.0001457862414779463,false,0.35],"torque":0.529857364175361,"active":true,"saturated":false,"log":[-0.5506190657615662,-0.6901092529296875,0.006377394311130047,-0.0,-0.3712843656539917,0.529857337474823,0.05552706494927406,-0.45618563890457153,-0.09947902709245682]},{"input":[true,23.0,0.002402006821598246,false,false,0.0001773237367001388,false,0.35],"torque":0.5223333571876724,"active":true,"saturated":false,"log":[-0.5502920746803284,-0.6896994113922119,0.004451371729373932,-0.0,-0.35478687286376953,0.5223333835601807,0.06753906607627869,-0.4438697397708893,0.0163485836237669]},{"input":[true,23.0,-0.07991263047386454,false,false,0.0002080734182735712,false,0.35],"torque":0.5145319905097461,"active":true,"saturated":false,"log":[-0.5488443374633789,-0.6878849267959595,0.0025304167065769434,-0.0,-0.3391468822956085,0.5145319700241089,0.0792510062456131,-0.43081235885620117,0.13286752998828888]},{"input":[true,23.0,-0.15975264118523302,false,false,0.0002378986715560677,false,0.35],"torque":0.5063118870504518,"active":true,"saturated":false,"log":[-0.5462819337844849,-0.6846733689308167,0.0006184299709275365,-0.0,-0.32407912611961365,0.5063118934631348,0.09061084687709808,-0.4170711636543274,0.24982516467571259]},{"input":[true,23.0,-0.23676331222487307,false,false,0.00026666698895416686,false,0.35],"torque":0.4976266150097243,"active":true,"saturated":false,"log":[-0.5426140427589417,-0.6800762414932251,-0.001280719181522727,-0.0,-0.3094835579395294,0.4976266026496887,0.10156812518835068,-0.4027051627635956,0.3669664263725281]},{"input":[true,23.0,-0.3106025006914785,false,false,0.0002942505586276729,false,0.35],"torque":0.4884976379829752,"active":true,"saturated":false,"log":[-0.5378531217575073,-0.6741092205047607,-0.003163205226883292,-0.0,-0.29539111256599426,0.48849764466285706,0.11207415163516998,-0.38777461647987366,0.4840347170829773]},{"input":[true,23.0,-0.3809421539420699,false,false,0.0003205268323319191,false,0.35],"torque":0.47897191589898486,"active":true,"saturated":false,"log":[-0.5320147275924683,-0.6667917966842651,-0.0050252568908035755,-0.0,-0.2818795442581177,0.478971928358078,0.12208226323127747,-0.37234067916870117,0.6007725596427917]},{"input":[true,23.0,-0.4474697670638194,false,false,0.00034537906987493823,false,0.35],"torque":0.1064337216475535,"active":true,"saturated":false,"log":[-0.18202754855155945,-0.228141188621521,-0.0050252568908035755,-0.0,0.02124258130788803,0.10643371939659119,0.1315479874610901,-0.037617627531290054,0.6482353806495667]},{"input":[true,23.0,-0.5098897712704371,false,false,0.0003686968577706227,false,0.35],"torque":0.08257574139853942,"active":true,"saturated":false,"log":[-0.17344382405281067,-0.21738292276859283,-0.0050252568908035755,-0.0,0.05798875913023949,0.0825757384300232,0.14042925834655762,-0.02075914666056633,0.6947236657142639]},{"input":[true,23.0,-0.5679248470547626,false,false,0.0003903765997836073,false,0.35],"torque":0.05748112235147625,"active":true,"saturated":false,"log":[-0.16342219710350037,-0.20482248067855835,-0.0050252568908035755,-0.0,0.09539508074522018,0.05748112127184868,0.14868664741516113,-0.003188258968293667,0.7398974299430847]},{"input":[true,23.0,-0.62131715626348,false,false,0.0004103219771864765,false,0.35],"torque":0.03162020675471536,"active":true,"saturated":false,"log":[-0.15205398201942444,-0.19057431817054749,-0.0055574458092451096,-0.0,0.1331716775894165,0.03162020817399025,0.156283438205719,0.014973489567637444,0.7834317088127136]},{"input":[true,23.0,-0.6698294876201067,false,false,0.00042844437668447364,false,0.35],"torque":0.0048809961071722575,"active":true,"saturated":false,"log":[-0.13943317532539368,-0.1747562438249588,-0.006045461632311344,-0.0,0.1710829883813858,0.0048809959553182125,0.16318589448928833,0.033604975789785385,0.825017511844635]},{"input":[true,23.0,-0.7132463106069167,false,false,0.0004446632841065207,false,0.35],"torque":-0.022553710091440953,"active":true,"saturated":false,"log":[-0.12565630674362183,-0.15748924016952515,-0.00648525869473815,-0.0,0.20888197422027588,-0.022553710266947746,0.1693633496761322,0.05258582904934883,0.8643627166748047]},{"input":[true,23.0,-0.7513747330236265,false,false,0.0004589066421134661,false,0.35],"torque":-0.05050064228863858,"active":true,"saturated":false,"log":[-0.11082223057746887,-0.13889719545841217,-0.0068731363862752914,-0.0,0.24632389843463898,-0.050500642508268356,0.17478837072849274,0.07179675251245499,0.9011925458908081]},{"input":[true,23.0,-0.7840453579685867,false,false,0.000471111170334329,false,0.35],"torque":-0.07877758951669504,"active":true,"saturated":false,"log":[-0.09503181278705597,-0.11910653859376907,-0.007205747999250889,-0.0,0.28316906094551086,-0.07877758890390396,0.1794368177652359,0.09111989289522171,0.9352505207061768]},{"input":[true,23.0,-0.8111130364351058,false,false,0.00048122264650824387,false,0.35],"torque":-0.1072035559759421,"active":true,"saturated":false,"log":[-0.07838774472475052,-0.09824597090482712,-0.007480104919523001,-0.0,0.31918275356292725,-0.10720355808734894,0.1832880824804306,0.11043916642665863,0.9662989974021912]},{"input":[true,23.0,-0.8324575121792475,false,false,0.0004891961473830436,false,0.35],"torque":-0.135599621017948,"active":true,"saturated":false,"log":[-0.060994215309619904,-0.07644608616828918,-0.007693585008382797,-0.0,0.35413670539855957,-0.13559962809085846,0.18632502853870392,0.1296406239271164,0.9941197037696838]},{"input":[true,23.0,-0.8479839559941054,false,false,0.0004949962483002228,false,0.35],"torque":-0.16378999206825134,"active":true,"saturated":false,"log":[-0.0429566353559494,-0.05383898317813873,-0.007843933068215847,-0.0,0.38781073689460754,-0.16378998756408691,0.18853417038917542,0.1486128270626068,1.0185142755508423]},{"input":[true,23.0,-0.8576233870168717,false,false,0.0004985971805795595,false,0.35],"torque":-0.19160297443855656,"active":true,"saturated":false,"log":[-0.024381298571825027,-0.030557895079255104,-0.00792926736176014,-0.0,0.4199943244457245,-0.19160297513008118,0.18990570306777954,0.16724716126918793,1.0393049716949463]},{"input":[true,23.0,-0.8613329791969142,false,false,0.0004999829460041724,false,0.35],"torque":-0.21969806636691114,"active":true,"saturated":false,"log":[-0.005375050473958254,-0.006736729759722948,-0.007948080077767372,-0.0,0.45213302969932556,-0.21969807147979736,0.19043350219726562,0.18543826043605804,1.0563347339630127]},{"input":[true,23.0,-0.8590962515632983,false,false,0.0004991473878973766,false,0.35],"torque":-0.24894248376340364,"active":true,"saturated":false,"log":[0.013955047354102135,0.0174903254956007,-0.007899237796664238,-0.0,0.4860866367816925,-0.24894247949123383,0.19011525809764862,0.20308424532413483,1.0694680213928223]},{"input":[true,23.0,-0.8509231414464178,false,false,0.000496094218475557,false,0.35],"torque":-0.2772280622684469,"active":true,"saturated":false,"log":[0.033502086997032166,0.0419892817735672,-0.0077819800935685635,-0.0,0.5177907347679138,-0.27722805738449097,0.18895237147808075,0.2200872153043747,1.0785905122756958]},{"input":[true,23.0,-0.8368499603284387,false,false,0.0004908370023555395,false,0.35],"torque":-0.3043916854073413,"active":true,"saturated":false,"log":[0.05315966531634331,0.06662677973508835,-0.007595921400934458,-0.0,0.5470535755157471,-0.30439168214797974,0.18694999814033508,0.23635344207286835,1.083609700202942]},{"input":[true,23.0,-0.8169392325186988,false,false,0.00048339909628973055,false,0.35],"torque":-0.33027810199379853,"active":true,"saturated":false,"log":[0.07282225787639618,0.0912705659866333,-0.007341043557971716,-0.0,0.5736981630325317,-0.33027809858322144,0.18411704897880554,0.2517937421798706,1.084455132484436]},{"input":[true,23.0,-0.7912794173707911,false,false,0.00047381354539676926,false,0.35],"torque":-0.3477419461189472,"active":true,"saturated":false,"log":[0.09238560497760773,0.11578995734453201,-0.0070176939480006695,-0.0,0.583628237247467,-0.3477419316768646,0.18046610057353973,0.2663238048553467,1.081078052520752]},{"input":[true,23.0,-0.7599845162754733,false,false,0.0004621229363487218,false,0.35],"torque":-0.35704948080610827,"active":true,"saturated":false,"log":[0.11174706369638443,0.1400563269853592,-0.006626579444855452,-0.0,0.5775032639503479,-0.35704949498176575,0.17601338028907776,0.27986449003219604,1.0734517574310303]},{"input":[true,23.0,-0.723193566175448,false,false,0.00044837920816707353,false,0.35],"torque":-0.3656364594643759,"active":true,"saturated":false,"log":[0.13080598413944244,0.16394349932670593,-0.006168758496642113,-0.0,0.5702560544013977,-0.3656364679336548,0.17077867686748505,0.29234200716018677,1.0615712404251099]},{"input":[true,23.0,-0.681070021852231,false,false,0.00043264342146811797,false,0.35],"torque":-0.37345648487607486,"active":true,"saturated":false,"log":[0.14946411550045013,0.18732835352420807,-0.005645633675158024,0.0,0.5619187355041504,-0.3734564781188965,0.1647852212190628,0.30368831753730774,1.0454533100128174]},{"input":[true,23.0,-0.6338010297294729,false,false,0.00041498548718293574,false,0.35],"torque":-0.380467517039883,"active":true,"saturated":false,"log":[0.16762588918209076,0.21009111404418945,-0.005058943293988705,0.0,0.5525291562080383,-0.3804675042629242,0.15805967152118683,0.31384122371673584,1.02513587474823]},{"input":[true,23.0,-0.5815965964190948,false,false,0.00039548385595720845,false,0.35],"torque":-0.3866319885853991,"active":true,"saturated":false,"log":[0.18519888818264008,0.2321159392595291,-0.004410746973007917,0.0,0.5421304106712341,-0.3866319954395294,0.1506318897008896,0.322744756937027,1.0006780624389648]},{"input":[true,23.0,-0.5246886557042039,false,false,0.00037422516961079937,false,0.35],"torque":-0.39191676639307554,"active":true,"saturated":false,"log":[0.20209407806396484,0.2532912492752075,-0.003703417954966426,0.0,0.530770480632782,-0.39191675186157227,0.14253488183021545,0.3303491473197937,0.9721593856811523]},{"input":[true,23.0,-0.46333003810401,false,false,0.0003513038762055989,false,0.35],"torque":-0.3962932574806895,"active":true,"saturated":false,"log":[0.21822625398635864,0.27351024746894836,-0.0029396258760243654,0.0,0.518501877784729,-0.3962932527065277,0.13380461931228638,0.33661115169525146,0.9396796822547913]},{"input":[true,23.0,-0.3977933475987471,false,false,0.00032682181043180595,false,0.35],"torque":-0.39973739807631575,"active":true,"saturated":false,"log":[0.23351427912712097,0.29267123341560364,-0.0021223260555416346,0.0,0.5053812861442566,-0.3997373878955841,0.12447988986968994,0.34149420261383057,0.9033581018447876]},{"input":[true,23.0,-0.3283697505050939,false,false,0.0003008877411769109,false,0.35],"torque":-0.40222972700293524,"active":true,"saturated":false,"log":[0.2478814274072647,0.31067806482315063,-0.0012547409860417247,0.0,0.49146944284439087,-0.4022297263145447,0.11460212618112564,0.34496843814849854,0.8633326888084412]},{"input":[true,23.0,-0.25536768188286824,false,false,0.00027361688828743294,false,0.35],"torque":-0.40375547259596756,"active":true,"saturated":false,"log":[0.2612557113170624,0.3274405002593994,-0.0003403460141271353,0.0,0.4768305718898773,-0.4037554860115051,0.10421520471572876,0.3470107913017273,0.8197594881057739]},{"input":[true,23.0,-0.17911147522017967,false,false,0.0002451304106703497,false,0.35],"torque":-0.40430461384075805,"active":true,"saturated":false,"log":[0.27357012033462524,0.34287455677986145,0.0006171494023874402,0.0,0.4615324139595032,-0.4043046236038208,0.09336527436971664,0.3476051390171051,0.772812008857727]},{"input":[true,23.0,-0.0999399214850587,false,false,0.00021555486800847306,false,0.35],"torque":-0.40387198619076714,"active":true,"saturated":false,"log":[0.2847628593444824,0.3569027781486511,0.001613819389604032,0.0,0.44564610719680786,-0.40387198328971863,0.08210054039955139,0.34674227237701416,0.7226800322532654]},{"input":[false,23.0,-0.018204763945421754,false,false,0.00018502165848126905,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.06573086455542787,false,false,0.0001536664349892097,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.15149405492488272,false,false,0.00012162850247525857,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.23870377858103137,false,false,8.905019902106331e-05,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.3269725802859037,false,false,5.6076263467527434e-05,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.41590829953106095,false,false,2.2853192369290225e-05,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.505115812827748,false,false,-1.0471410859959058e-05,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.5941987891609823,false,false,-4.37494917197232e-05,true,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.6827614508084267,false,false,-7.683320239902244e-05,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.7704103317011014,false,false,-0.00010957555863444268,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.8567560255138795,false,false,-0.00014183109273161314,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.9414149157193685,false,false,-0.00017345649984887746,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.024010879918877,false,false,-0.00020431127467184204,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.104176960878477,false,false,-0.00023425833565018857,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.1815569968460946,false,false,-0.00026316463402339594,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,23.0,1.2558072039064387,false,false,-0.00029090174492958534,false,0.35],"torque":-0.32245143401461784,"active":true,"saturated":false,"log":[0.3696204721927643,0.46325767040252686,0.0012936716666445136,0.0,0.17749245464801788,-0.3224514424800873,-0.1107986569404602,0.23270468413829803,-0.08328093588352203]},{"input":[true,23.0,1.3265977033437,false,false,-0.00031734643797131734,false,0.35],"torque":-0.30459228422742335,"active":true,"saturated":false,"log":[0.35214367508888245,0.4413534104824066,0.0012936716666445136,0.0,0.16383682191371918,-0.3045922815799713,-0.1208709105849266,0.20639052987098694,-0.16390679776668549]},{"input":[true,23.0,1.3936139872262414,false,false,-0.00034238122470342736,false,0.35],"torque":-0.28652151677533033,"active":true,"saturated":false,"log":[0.33390259742736816,0.4184912443161011,0.0012936716666445136,0.0,0.15071772038936615,-0.2865215241909027,-0.13040615618228912,0.17990311980247498,-0.2416795939207077]},{"input":[true,23.0,1.4565583157020043,false,false,-0.0003658948806105272,false,0.35],"torque":-0.2550238271767233,"active":true,"saturated":false,"log":[0.3149222433567047,0.3947025537490845,0.0012936716666445136,0.0,0.11179037392139435,-0.2550238370895386,-0.13936203718185425,0.15330800414085388,-0.31640276312828064]},{"input":[true,23.0,1.5151510397966999,false,false,-0.00038778293925512483,false,0.35],"torque":-0.21516388092683603,"active":true,"saturated":false,"log":[0.29523468017578125,0.37002745270729065,0.0012936716666445136,0.0,0.057099007070064545,-0.21516388654708862,-0.1476987600326538,0.1266748458147049,-0.38788285851478577]},{"input":[true,23.0,1.569131843837882,false,false,-0.0004079481564009665,false,0.35],"torque":-0.17479611110742113,"active":true,"saturated":false,"log":[0.27487891912460327,0.3445149064064026,0.0012936716666445136,0.0,0.002233948791399598,-0.17479610443115234,-0.1553792953491211,0.10007686913013458,-0.455931156873703]},{"input":[true,23.0,1.61826090198505,false,false,-0.00042630094204959286,false,0.35],"torque":-0.1341910054814517,"active":true,"saturated":false,"log":[0.2539002597332001,0.31822165846824646,0.0012936716666445136,0.0,-0.05232299491763115,-0.13419100642204285,-0.162369504570961,0.07359034568071365,-0.5203650593757629]},{"input":[true,23.0,1.6623199437275584,false,false,-0.00044275975847065947,false,0.35],"torque":-0.09365055018446675,"active":true,"saturated":false,"log":[0.2323499619960785,0.29121196269989014,0.0012936716666445136,0.0,-0.1060347780585289,-0.09365054965019226,-0.16863833367824554,0.04729394242167473,-0.5810098052024841]},{"input":[true,23.0,1.701113223616536,false,false,-0.00045725148245765016,false,0.35],"torque":-0.053419402307481455,"active":true,"saturated":false,"log":[0.2102847695350647,0.26355689764022827,0.0012936716666445136,0.0,-0.15848535299301147,-0.05341940373182297,-0.17415794730186462,0.021268244832754135,-0.6376994252204895]},{"input":[true,23.0,1.7344683909224894,false,false,-0.00046971173019955657,false,0.35],"torque":-0.013692426647081804,"active":true,"saturated":false,"log":[0.1877664178609848,0.2353339046239853,0.0012936716666445136,0.0,-0.209364116191864,-0.01369242649525404,-0.17890380322933197,-0.004404833074659109,-0.6902784109115601]},{"input":[true,23.0,1.7622372553548837,false,false,-0.000480085143325183,false,0.35],"torque":0.025096099245161464,"active":true,"saturated":false,"log":[0.16486123204231262,0.2066260725259781,0.0018706859555095434,0.0,-0.2584664821624756,0.025096099823713303,-0.18285483121871948,-0.029642580077052116,-0.7386026978492737]},{"input":[true,23.0,1.7842964454417571,false,false,-0.0004883256348492412,false,0.35],"torque":0.06318151084435472,"active":true,"saturated":false,"log":[0.1416396051645279,0.17752163112163544,0.002366424538195133,0.0,-0.3056909441947937,0.06318151205778122,-0.18599346280097961,-0.054362017661333084,-0.7825409770011902]},{"input":[true,23.0,1.800547956644325,false,false,-0.0004943965939275431,false,0.35],"torque":0.10043482637707128,"active":true,"saturated":false,"log":[0.11817554384469986,0.14811335504055023,0.00278003909625113,0.0,-0.35087263584136963,0.10043482482433319,-0.18830576539039612,-0.07848042994737625,-0.8219754099845886]},{"input":[true,23.0,1.8109195867714045,false,false,-0.0004982710485116088,false,0.35],"torque":0.13672724307852624,"active":true,"saturated":false,"log":[0.09454617649316788,0.11849787086248398,0.003110950579866767,0.0,-0.3938511312007904,0.1367272436618805,-0.18978147208690643,-0.1019158661365509,-0.8568028211593628]},{"input":[true,23.0,1.81536525675921,false,false,-0.0004999317851800449,false,0.35],"torque":0.17192968897915295,"active":true,"saturated":false,"log":[0.07083124667406082,0.0887751653790474,0.003358860034495592,0.0,-0.4344691038131714,0.17192968726158142,-0.19041401147842407,-0.12458765506744385,-0.8869354724884033]},{"input":[true,23.0,1.813865215391355,false,false,-0.0004993714256143098,false,0.35],"torque":0.20591466049080637,"active":true,"saturated":false,"log":[0.04711268097162247,0.059047892689704895,0.003523754421621561,0.0,-0.4725753366947174,0.2059146612882614,-0.1902005821466446,-0.14641685783863068,-0.9123017191886902]},{"input":[true,23.0,1.806426127049524,false,false,-0.0004965924593790963,false,0.35],"torque":0.23855828706188928,"active":true,"saturated":false,"log":[0.023474035784602165,0.029420791193842888,0.003605913370847702,0.0,-0.5080281496047974,0.23855829238891602,-0.1891421377658844,-0.16732676327228546,-0.9328467845916748]},{"input":[true,23.0,1.793081042104971,false,false,-0.0004916072328616975,false,0.35],"torque":0.2708422234869613,"active":true,"saturated":false,"log":[6.077176806229545e-08,7.616728225912084e-08,0.0036059136036783457,0.0,-0.5428890585899353,0.27084222435951233,-0.18724335730075836,-0.1872432976961136,-0.9485332369804382]},{"input":[true,23.0,1.773889250082375,false,false,-0.0004844378944194944,false,0.35],"torque":0.30331656965035864,"active":true,"saturated":false,"log":[-0.023223813623189926,-0.02910717949271202,0.003524630330502987,0.0,-0.5783612132072449,0.30331656336784363,-0.18451270461082458,-0.20609553158283234,-0.9593415260314941]},{"input":[true,23.0,1.748936016248422,false,false,-0.0004751162959792648,false,0.35],"torque":0.333938301161626,"active":true,"saturated":false,"log":[-0.04611194506287575,-0.05779363960027695,0.003363238647580147,0.0,-0.610485315322876,0.3339383006095886,-0.18096229434013367,-0.22381599247455597,-0.9652700424194336]}]},{"name":"delay_zero","frames":[{"input":[false,15.0,3.166249271770391,false,false,-0.0013323183357528668,true,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,3.083338292957414,false,false,-0.001291240963619141,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.997531744848385,false,false,-0.0012487290121424167,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.908924959224808,false,false,-0.0012048297124508391,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.8176163789551207,false,false,-0.0011595918370278437,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.7237074486238644,false,false,-0.0011130656455255266,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.6273025018259055,false,false,-0.0010653028289257553,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.5285086452509615,false,false,-0.0010163564521110636,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.427435639687181,false,false,-0.0009662808949091303,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.3241957780760076,false,false,-0.000915131791676342,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.2189037607537956,false,false,-0.0008629659694875654,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.1116765680187917,false,false,-0.0008098413850007994,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.0026333301650614,false,false,-0.0007558170600668505,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.8918951951277512,false,false,-0.000700953016155571,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.7795851938867335,false,false,-0.0006453102076715109,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.6658281037781788,false,false,-0.000588950454233074,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.5507503098659023,false,false,-0.0005319363719904111,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.434479664526522,false,false,-0.0004743313040583631,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.3171453454044126,false,false,-0.0004161992501417374,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.1988777118942804,false,false,-0.00035760479543111017,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,15.0,1.0798081603108023,false,false,-0.000298613038848147,false,0.0],"torque":0.12504981042915936,"active":true,"saturated":false,"log":[-0.03818652033805847,-0.07637304067611694,-0.00013365282211452723,-0.0,-0.17253687977790833,0.12504981458187103,-0.048375312238931656,-0.08046107739210129,0.09306055307388306]},{"input":[true,15.0,0.9600689779062372,false,false,-0.00023928952072016532,false,0.0],"torque":0.11149795047757113,"active":true,"saturated":false,"log":[-0.03382735699415207,-0.06765471398830414,-0.00013365282211452723,-0.0,-0.1542675942182541,0.11149795353412628,-0.03876490145921707,-0.06718793511390686,0.1801198124885559]},{"input":[true,15.0,0.8397931958982008,false,false,-0.00017970014996429066,false,0.0],"torque":0.08665569546234822,"active":true,"saturated":false,"log":[-0.029430609196424484,-0.05886121839284897,-0.00013365282211452723,-0.0,-0.11358718574047089,0.08665569871664047,-0.02911142259836197,-0.053840141743421555,0.26149457693099976]},{"input":[true,15.0,0.7191144416708863,false,false,-0.00011991113086210739,false,0.0],"torque":0.06210240309194261,"active":true,"saturated":false,"log":[-0.02500116638839245,-0.0500023327767849,-0.00013365282211452723,-0.0,-0.0735468938946724,0.06210240349173546,-0.01942560262978077,-0.040432535111904144,0.3374789357185364]},{"input":[true,15.0,0.5981667903139408,false,false,-5.998888950615651e-05,false,0.0],"torque":0.037919288005174454,"active":true,"saturated":false,"log":[-0.02054394595324993,-0.04108789190649986,-0.00013365282211452723,-0.0,-0.03429876267910004,0.037919286638498306,-0.009718199260532856,-0.026980005204677582,0.4083460569381714]},{"input":[true,15.0,0.4770846156639367,false,false,0.0,false,0.0],"torque":0.01424683517564497,"active":true,"saturated":false,"log":[-0.016063900664448738,-0.032127801328897476,-0.00013365282211452723,-0.0,0.0038872244767844677,0.014246835373342037,1.0803324901331735e-09,-0.013497499749064445,0.474349707365036]},{"input":[true,15.0,0.3560024410139325,false,false,5.998888950615651e-05,false,0.0],"torque":-0.008833293737737896,"active":true,"saturated":false,"log":[-0.011566009372472763,-0.023132018744945526,-0.00013365282211452723,-0.0,0.040858279913663864,-0.008833293803036213,0.009718201123178005,0.0,0.5357257127761841]},{"input":[true,15.0,0.23505478965698698,false,false,0.00011991113086210739,false,0.0],"torque":-0.03128953812378709,"active":true,"saturated":false,"log":[-0.007055267225950956,-0.014110534451901913,-0.0001583462581038475,-0.0,0.07658563554286957,-0.031289536505937576,0.01942560449242592,0.013497499749064445,0.5926933288574219]},{"input":[true,15.0,0.11437603542967242,false,false,0.00017970014996429066,false,0.0],"torque":-0.05313701206124717,"active":true,"saturated":false,"log":[-0.002536686835810542,-0.005073373671621084,-0.00016722465807106346,-0.0,0.11106862872838974,-0.05313701182603836,0.029111424461007118,0.026980005204677582,0.6454567313194275]},{"input":[true,15.0,-0.005899746578364007,false,false,0.00023928952072016532,false,0.0],"torque":-0.07438986807829205,"active":true,"saturated":false,"log":[0.0019847117364406586,0.003969423472881317,-0.0001602781703695655,-0.0,0.14434540271759033,-0.07438986748456955,0.03876490518450737,0.040432535111904144,0.6942058205604553]},{"input":[true,15.0,-0.1256389289829289,false,false,0.000298613038848147,false,0.0],"torque":-0.09506298594861574,"active":true,"saturated":false,"log":[0.0065039051696658134,0.013007810339331627,-0.00013751450751442462,-0.0,0.1764555722475052,-0.0950629860162735,0.048375312238931656,0.053840141743421555,0.7391176819801331]},{"input":[true,15.0,-0.2447084805664071,false,false,0.00035760479543111017,false,0.0],"torque":-0.11516942389983029,"active":true,"saturated":false,"log":[0.011015873402357101,0.022031746804714203,-9.895894618239254e-05,-0.0,0.20743517577648163,-0.11516942083835602,0.05793197825551033,0.06718793511390686,0.7803572416305542]},{"input":[true,15.0,-0.36297611407653924,false,false,0.0004161992501417374,false,0.0],"torque":-0.13471983696727985,"active":true,"saturated":false,"log":[0.015515602193772793,0.031031204387545586,-4.465433812583797e-05,-0.0,0.23731540143489838,-0.1347198337316513,0.06742428243160248,0.08046107739210129,0.8180787563323975]},{"input":[true,15.0,-0.4803104331986488,false,false,0.0004743313040583631,false,0.0],"torque":-0.15372248380415995,"active":true,"saturated":false,"log":[0.01999809220433235,0.0399961844086647,2.5338986233691685e-05,-0.0,0.2661227583885193,-0.15372247993946075,0.07684167474508286,0.09364482760429382,0.8524260520935059]},{"input":[true,15.0,-0.5965810785380288,false,false,0.0005319363719904111,false,0.0],"torque":-0.17218339779311212,"active":true,"saturated":false,"log":[0.02445836551487446,0.04891673102974892,0.00011094326328020543,-0.0,0.2938792109489441,-0.17218339443206787,0.08617369085550308,0.10672454535961151,0.8835338354110718]},{"input":[true,15.0,-0.7116588724503055,false,false,0.000588950454233074,false,0.0],"torque":-0.19010661584868632,"active":true,"saturated":false,"log":[0.028891464695334435,0.05778292939066887,0.00021206338715273887,-0.0,0.32060280442237854,-0.1901066154241562,0.09540997445583344,0.11968568712472916,0.9115283489227295]},{"input":[true,15.0,-0.8254159625588602,false,false,0.0006453102076715109,false,0.0],"torque":-0.2074944535764517,"active":true,"saturated":false,"log":[0.033292464911937714,0.06658492982387543,0.0003285870188847184,-0.0,0.346308171749115,-0.20749445259571075,0.10454025119543076,0.1325138509273529,0.9365278482437134]},{"input":[true,15.0,-0.9377259637998776,false,false,0.000700953016155571,false,0.0],"torque":-0.22434777767939845,"active":true,"saturated":false,"log":[0.037656478583812714,0.07531295716762543,0.0004603847046382725,-0.0,0.37100690603256226,-0.224347785115242,0.11355438828468323,0.14519479870796204,0.9586436152458191]},{"input":[true,15.0,-1.0484640988371878,false,false,0.0007558170600668505,false,0.0],"torque":-0.24066624880856674,"active":true,"saturated":false,"log":[0.04197865352034569,0.08395730704069138,0.0006073099793866277,-0.0,0.3947083055973053,-0.2406662553548813,0.12244236469268799,0.15771442651748657,0.9779803156852722]},{"input":[true,15.0,-1.1575073366909183,false,false,0.0008098413850007994,false,0.0],"torque":-0.25644854491088886,"active":true,"saturated":false,"log":[0.04625419154763222,0.09250838309526443,0.0007691996288485825,-0.0,0.41741955280303955,-0.25644853711128235,0.1311943084001541,0.17005883157253265,0.994636595249176]},{"input":[true,15.0,-1.2647345294259222,false,false,0.0008629659694875654,false,0.0],"torque":-0.27202893533424105,"active":true,"saturated":false,"log":[0.050478339195251465,0.10095667839050293,0.0009458738495595753,-0.0,0.43981608748435974,-0.27202892303466797,0.13980048894882202,0.18221430480480194,1.008705735206604]},{"input":[true,15.0,-1.3700265467481347,false,false,0.000915131791676342,false,0.0],"torque":-0.28854780238186967,"active":true,"saturated":false,"log":[0.05464640632271767,0.10929281264543533,0.001137136248871684,-0.0,0.46417850255966187,-0.2885478138923645,0.14825135469436646,0.1941673457622528,1.0202759504318237]},{"input":[true,15.0,-1.473266408359308,false,false,0.0009662808949091303,false,0.0],"torque":-0.30448935354229645,"active":true,"saturated":false,"log":[0.058753762394189835,0.11750752478837967,0.0013427744852378964,-0.0,0.48749786615371704,-0.3044893443584442,0.1565375030040741,0.20590464770793915,1.0294311046600342]},{"input":[true,15.0,-1.5743394139230884,false,false,0.0010163564521110636,false,0.0],"torque":-0.31984768629050864,"active":true,"saturated":false,"log":[0.06279584020376205,0.1255916804075241,0.0015625598607584834,-0.0,0.5097718834877014,-0.3198476731777191,0.16464973986148834,0.21741320192813873,1.0362508296966553]},{"input":[true,15.0,-1.6731332704980326,false,false,0.0010653028289257553,false,0.0],"torque":-0.33461655349846764,"active":true,"saturated":false,"log":[0.06676815450191498,0.13353630900382996,0.0017962483689188957,-0.0,0.5309974551200867,-0.3346165418624878,0.1725790649652481,0.22868020832538605,1.0408110618591309]},{"input":[true,15.0,-1.769538217295991,false,false,0.0011130656455255266,false,0.0],"torque":-0.3487894333629678,"active":true,"saturated":false,"log":[0.07066628336906433,0.14133256673812866,0.0020435804035514593,-0.0,0.551170825958252,-0.34878942370414734,0.18031664192676544,0.23969313502311707,1.043184518814087]},{"input":[true,15.0,-1.8634471476272478,false,false,0.0011595918370278437,false,0.0],"torque":-0.3623596559860975,"active":true,"saturated":false,"log":[0.0744859054684639,0.1489718109369278,0.002304281108081341,-0.0,0.5702877640724182,-0.3623596429824829,0.1878538727760315,0.25043976306915283,1.0434406995773315]},{"input":[true,15.0,-1.954755727896935,false,false,0.0012048297124508391,false,0.0],"torque":-0.37532045781554485,"active":true,"saturated":false,"log":[0.07822276651859283,0.15644553303718567,0.0025780608411878347,-0.0,0.588343620300293,-0.3753204643726349,0.1951824128627777,0.2609081566333771,1.0416467189788818]},{"input":[true,15.0,-2.043362513520512,false,false,0.0012487290121424167,false,0.0],"torque":-0.3876651188987272,"active":true,"saturated":false,"log":[0.08187273144721985,0.1637454628944397,0.0028646152932196856,-0.0,0.605333685874939,-0.38766512274742126,0.20229409635066986,0.2710866928100586,1.0378668308258057]},{"input":[true,15.0,-2.1291690616295407,false,false,0.001291240963619141,false,0.0],"torque":-0.3993869895393674,"active":true,"saturated":false,"log":[0.08543173223733902,0.17086346447467804,0.003163626417517662,-0.0,0.621253252029419,-0.399387001991272,0.2091810405254364,0.2809640169143677,1.032163381576538]},{"input":[true,15.0,-2.2120800404425185,false,false,0.0013323183357528668,false,0.0],"torque":-0.41018655557007405,"active":true,"saturated":false,"log":[0.08889581263065338,0.17779162526130676,0.0034747617319226265,-0.0,0.6355141401290894,-0.4101865589618683,0.2158355712890625,0.2905292212963104,1.0245966911315918]},{"input":[true,15.0,-2.292003335179272,false,false,0.0013719154912449113,false,0.0],"torque":-0.4183891026295287,"active":true,"saturated":false,"log":[0.09226112812757492,0.18452225625514984,0.0037976757157593966,-0.0,0.6447898149490356,-0.41838911175727844,0.2222503125667572,0.29977163672447205,1.0152255296707153]},{"input":[true,15.0,-2.368850150401242,false,false,0.0014099884373294702,false,0.0],"torque":-0.42631854893241045,"active":true,"saturated":false,"log":[0.09552394598722458,0.19104789197444916,0.004132009577006102,-0.0,0.6537154912948608,-0.42631855607032776,0.22841812670230865,0.3086809813976288,1.004106879234314]},{"input":[true,15.0,-2.4425351086639364,false,false,0.0014464948746499401,true,0.0],"torque":-0.43396520055511817,"active":true,"saturated":false,"log":[0.09868063777685165,0.1973612755537033,0.004477391485124826,-0.0,0.662279486656189,-0.43396520614624023,0.23433217406272888,0.3172473907470703,0.9912965297698975]},{"input":[true,15.0,-2.5129763453719867,false,false,0.0014813942442538548,false,0.0],"torque":-0.44131978656869286,"active":true,"saturated":false,"log":[0.101727694272995,0.20345538854599,0.00483343843370676,-0.0,0.6704708337783813,-0.4413197934627533,0.2399858683347702,0.32546135783195496,0.9768492579460144]},{"input":[true,15.0,-2.5800955997313784,false,false,0.0015146477726542137,false,0.0],"torque":-0.44837346104138226,"active":true,"saturated":false,"log":[0.104661725461483,0.209323450922966,0.005199754610657692,-0.0,0.6782789826393127,-0.4483734667301178,0.24537293612957,0.3333137035369873,0.9608185291290283]},{"input":[true,15.0,-2.643818301697839,false,false,0.0015462185149071458,false,0.0],"torque":-0.45511781552612174,"active":true,"saturated":false,"log":[0.10747947543859482,0.21495895087718964,0.005575933028012514,-0.0,0.6856942176818848,-0.4551178216934204,0.25048738718032837,0.34079575538635254,0.9432570934295654]},{"input":[true,15.0,-2.7040736548247755,false,false,0.0015760713956580457,false,0.0],"torque":-0.4615448728976593,"active":true,"saturated":false,"log":[0.11017782241106033,0.22035564482212067,0.005961555056273937,-0.0,0.6927071809768677,-0.46154487133026123,0.2553235590457916,0.3478991687297821,0.9242169857025146]},{"input":[true,15.0,-2.760794714918706,false,false,0.0016041732481105837,false,0.0],"torque":-0.46764705569860693,"active":true,"saturated":false,"log":[0.11275375634431839,0.22550751268863678,0.006356193218380213,-0.0,0.6993092894554138,-0.4676470458507538,0.2598760724067688,0.35461607575416565,0.9037495851516724]},{"input":[true,15.0,-2.813918464414808,false,false,0.001630492850875286,false,0.0],"torque":-0.4734172118793756,"active":true,"saturated":false,"log":[0.11520441621541977,0.23040883243083954,0.006759408861398697,-0.0,0.705492377281189,-0.4734172224998474,0.2641398310661316,0.3609389662742615,0.8819056153297424]},{"input":[true,15.0,-2.863385882389958,false,false,0.0016550009626567592,false,0.0],"torque":-0.47884860076829067,"active":true,"saturated":false,"log":[0.11752709001302719,0.23505418002605438,0.007170753553509712,-0.0,0.7112489938735962,-0.4788486063480377,0.26811015605926514,0.36686089634895325,0.8587355017662048]},{"input":[true,15.0,-2.9091420101354575,false,false,0.0016776703547410072,false,0.0],"torque":-0.4839348465808393,"active":true,"saturated":false,"log":[0.11971918493509293,0.23943836987018585,0.007589770946651697,-0.0,0.7165719866752625,-0.48393484950065613,0.27178260684013367,0.3723752200603485,0.8342893123626709]},{"input":[true,15.0,-2.9511360122166144,false,false,0.0016984758412467596,false,0.0],"torque":-0.48844331240890976,"active":true,"saturated":false,"log":[0.12177827209234238,0.24355654418468475,0.008015994913876057,-0.0,0.721003532409668,-0.4884433150291443,0.27515310049057007,0.3774758279323578,0.8086166977882385]},{"input":[true,15.0,-2.989321232951325,false,false,0.0017173943071071896,false,0.0],"torque":-0.4902200318733638,"active":true,"saturated":false,"log":[0.12370206415653229,0.24740412831306458,0.008448951877653599,-0.0,0.7202596664428711,-0.4902200400829315,0.2782178819179535,0.3821570575237274,0.7817672491073608]},{"input":[true,15.0,-3.023655248244926,false,false,0.0017344047337509473,false,0.0],"torque":-0.4913821233500084,"active":true,"saturated":false,"log":[0.12548843026161194,0.2509768605232239,0.00888816174119711,-0.0,0.7185606956481934,-0.4913821220397949,0.28097355365753174,0.38641372323036194,0.7537903189659119]},{"input":[true,15.0,-3.054099912723712,false,false,0.0017494882224539628,false,0.0],"torque":-0.49192992756848514,"active":true,"saturated":false,"log":[0.12713536620140076,0.2542707324028015,0.009333135560154915,-0.0,0.7159117460250854,-0.49192991852760315,0.28341710567474365,0.3902410566806793,0.7247351408004761]},{"input":[true,15.0,-3.0806214021147578,false,false,0.0017626280153360862,false,0.0],"torque":-0.49186401009300335,"active":true,"saturated":false,"log":[0.1286410540342331,0.2572821080684662,0.009783378802239895,-0.0,0.7123181819915771,-0.49186399579048157,0.2855457365512848,0.3936348557472229,0.6946508288383484]},{"input":[true,15.0,-3.1031902508249636,false,false,0.0017738095139792282,false,0.0],"torque":-0.4911850195529054,"active":true,"saturated":false,"log":[0.13000382483005524,0.2600076496601105,0.010238392278552055,-0.0,0.7077850699424744,-0.4911850094795227,0.2873571515083313,0.39659130573272705,0.6635865569114685]},{"input":[true,15.0,-3.121781384677576,false,false,0.0017830202956463255,false,0.0],"torque":-0.4898935510873473,"active":true,"saturated":false,"log":[0.13122215867042542,0.26244431734085083,0.01069767028093338,-0.0,0.7023171186447144,-0.4898935556411743,0.2888492941856384,0.3991071283817291,0.6315913796424866]},{"input":[true,15.0,-3.136374148769799,false,false,0.0017902501270831014,false,0.0],"torque":-0.48798998964088625,"active":true,"saturated":false,"log":[0.132294699549675,0.26458939909935,0.011160701513290405,-0.0,0.6959183216094971,-0.48798999190330505,0.29002052545547485,0.40117955207824707,0.5987144708633423]},{"input":[true,15.0,-3.146952330420567,false,false,0.001795490975887298,false,0.0],"torque":-0.48547432283010455,"active":true,"saturated":false,"log":[0.13322027027606964,0.2664405405521393,0.011626971885561943,-0.0,0.688591480255127,-0.4854743182659149,0.2908695340156555,0.40280628204345703,0.5650050044059753]},{"input":[true,15.0,-3.153504177182963,false,false,0.0017987370194327387,false,0.0],"torque":-0.48234588563161107,"active":true,"saturated":false,"log":[0.1339978277683258,0.2679956555366516,0.012095964513719082,-0.0,0.6803379654884338,-0.4823458790779114,0.2913953959941864,0.40398547053337097,0.5305120944976807]},{"input":[true,15.0,-3.156022409901282,false,false,0.0017999846513383159,false,0.0],"torque":-0.4786032433165809,"active":true,"saturated":false,"log":[0.13462650775909424,0.2692530155181885,0.012567157857120037,-0.0,0.6711570620536804,-0.4786032438278198,0.29159751534461975,0.4047158360481262,0.4952850639820099]},{"input":[true,15.0,-3.154504230798238,false,false,0.001799232485474709,false,0.0],"torque":-0.47424419188509503,"active":true,"saturated":false,"log":[0.13510562479496002,0.27021124958992004,0.0130400275811553,-0.0,0.6610463261604309,-0.4742441773414612,0.29147565364837646,0.4049965441226959,0.4593731164932251]},{"input":[true,15.0,-3.1489513265833184,false,false,0.0017964813575043867,false,0.0],"torque":-0.4692660652285547,"active":true,"saturated":false,"log":[0.13543462753295898,0.27086925506591797,0.013514048419892788,-0.0,0.6500020623207092,-0.46926605701446533,0.29102998971939087,0.4048272967338562,0.4228256344795227]},{"input":[true,15.0,-3.139369866578829,false,false,0.001791734323953177,false,0.0],"torque":-0.46366674000314756,"active":true,"saturated":false,"log":[0.13561315834522247,0.27122631669044495,0.013988694176077843,-0.0,0.6380212903022766,-0.46366673707962036,0.2902609705924988,0.40420830249786377,0.3856920003890991]},{"input":[true,15.0,-3.125770495865729,false,false,0.0017849966588144434,false,0.0],"torque":-0.45744632176674316,"active":true,"saturated":false,"log":[0.13564103841781616,0.2712820768356323,0.014463437721133232,-0.0,0.625105082988739,-0.45744630694389343,0.28916946053504944,0.40314021706581116,0.34802159667015076]},{"input":[true,15.0,-3.108168323456864,false,false,0.0017762758476896352,false,0.0],"torque":-0.45060935532097846,"active":true,"saturated":false,"log":[0.13551820814609528,0.27103641629219055,0.014937751926481724,-0.0,0.6112631559371948,-0.45060935616493225,0.28775668144226074,0.40162426233291626,0.3098639249801636]},{"input":[true,15.0,-3.0865829055107197,false,false,0.0017655815794717213,false,0.0],"torque":-0.44316696256279065,"active":true,"saturated":false,"log":[0.13524481654167175,0.2704896330833435,0.01541110873222351,-0.0,0.5965179204940796,-0.44316697120666504,0.28602421283721924,0.3996620774269104,0.271268367767334]},{"input":[true,15.0,-3.0610382236043754,false,false,0.0017529257355807513,false,0.0],"torque":-0.43513745100796875,"active":true,"saturated":false,"log":[0.13482117652893066,0.26964235305786133,0.015882981941103935,-0.0,0.5809056162834167,-0.43513745069503784,0.28397396206855774,0.39725586771965027,0.23228439688682556]},{"input":[true,15.0,-3.0315626580897703,false,false,0.0017383223767634996,false,0.0],"torque":-0.42654497802315866,"active":true,"saturated":false,"log":[0.13424775004386902,0.26849550008773804,0.016352849081158638,-0.0,0.5644740462303162,-0.4265449643135071,0.2816082239151001,0.39440828561782837,0.1929614394903183]},{"input":[true,15.0,-2.9981889565628994,false,false,0.00172178772747186,false,0.0],"torque":-0.4174171538617695,"active":true,"saturated":false,"log":[0.13352516293525696,0.2670503258705139,0.016820188611745834,0.0,0.5472774505615234,-0.41741716861724854,0.27892962098121643,0.3911225199699402,0.15334883332252502]},{"input":[true,15.0,-2.9609541974809686,false,false,0.001703340157837346,false,0.0],"torque":-0.40778325233059853,"active":true,"saturated":false,"log":[0.13265423476696014,0.2653084695339203,0.01728447712957859,0.0,0.5293729305267334,-0.40778324007987976,0.2759411036968231,0.3874022364616394,0.11349587887525558]},{"input":[true,15.0,-2.919899748967918,false,false,0.001683000163261724,false,0.0],"torque":-0.3976736321934805,"active":true,"saturated":false,"log":[0.13163591921329498,0.26327183842658997,0.017745204269886017,0.0,0.5108196139335632,-0.397673636674881,0.27264603972435,0.3832515478134155,0.07345176488161087]},{"input":[true,15.0,-2.875071222854098,false,false,0.0016607903416464523,false,0.0],"torque":-0.38711970101483534,"active":true,"saturated":false,"log":[0.1304713636636734,0.2609427273273468,0.018201854079961777,0.0,0.4916782081127167,-0.38711971044540405,0.2690480351448059,0.37867504358291626,0.03326552361249924]},{"input":[true,15.0,-2.826518424001152,false,false,0.001636735368286227,false,0.0],"torque":-0.3761045659488405,"active":true,"saturated":false,"log":[0.12916183471679688,0.25832366943359375,0.01865391992032528,0.0,0.47191309928894043,-0.37610456347465515,0.26515114307403564,0.3736778199672699,-0.007013937458395958]},{"input":[true,15.0,-2.7742952949684025,false,false,0.0016108619684545263,false,0.0],"torque":-0.3644961370431579,"active":true,"saturated":false,"log":[0.1277088224887848,0.2554176449775696,0.01910090073943138,0.0,0.45125871896743774,-0.3644961416721344,0.2609596252441406,0.36826545000076294,-0.04733793064951897]},{"input":[true,15.0,-2.7184598560822226,false,false,0.001583198887711611,false,0.0],"torque":-0.3525756360069838,"active":true,"saturated":false,"log":[0.1261139214038849,0.2522278428077698,0.019542299211025238,0.0,0.4302724301815033,-0.3525756299495697,0.2564782202243805,0.3624439537525177,-0.08765798807144165]},{"input":[true,15.0,-2.659074140974979,false,false,0.0015537768599679727,false,0.0],"torque":-0.34037355521538715,"active":true,"saturated":false,"log":[0.12437891215085983,0.24875782430171967,0.019977625459432602,0.0,0.4090117812156677,-0.34037354588508606,0.2517118453979492,0.35621973872184753,-0.12792588770389557]},{"input":[true,15.0,-2.596204127665154,false,false,0.0015226285733387025,false,0.0],"torque":-0.32796533924549504,"active":true,"saturated":false,"log":[0.12250570952892303,0.24501141905784607,0.020406395196914673,0.0,0.38762372732162476,-0.3279653489589691,0.2466658353805542,0.34959977865219116,-0.168093740940094]},{"input":[true,15.0,-2.5299196652552234,true,false,0.0014897886338267289,false,0.0],"torque":-0.3160707071123525,"active":true,"saturated":false,"log":[0.12049640715122223,0.24099281430244446,0.020406395196914673,0.0,0.3679593503475189,-0.31607070565223694,0.24134576320648193,0.34259143471717834,-0.20811396837234497]},{"input":[true,15.0,-2.4602943963287256,true,false,0.001455293526875262,false,0.0],"torque":-0.30396868718336684,"active":true,"saturated":false,"log":[0.1183532327413559,0.2367064654827118,0.020406395196914673,0.0,0.3481498062610626,-0.3039686977863312,0.23575755953788757,0.3352024555206299,-0.2479393482208252]},{"input":[true,15.0,-2.387405675132754,true,false,0.0014191815768321676,false,0.0],"torque":-0.29164736406987146,"active":true,"saturated":false,"log":[0.11607856303453445,0.2321571260690689,0.020406395196914673,0.0,0.3281666934490204,-0.29164737462997437,0.22990742325782776,0.3274410367012024,-0.287523090839386]},{"input":[true,15.0,-2.311334481636741,true,false,0.0013814929043712983,false,0.0],"torque":-0.27909606762181244,"active":true,"saturated":false,"log":[0.11367493122816086,0.22734986245632172,0.020406395196914673,0.0,0.3079836070537567,-0.2790960669517517,0.223801851272583,0.3193158507347107,-0.3268188536167145]},{"input":[true,15.0,-2.232165331563051,true,false,0.0013422693819180966,false,0.0],"torque":-0.26630937534629456,"active":true,"saturated":false,"log":[0.1111450046300888,0.2222900092601776,0.020406395196914673,0.0,0.28758445382118225,-0.26630938053131104,0.21744763851165771,0.31083589792251587,-0.365780770778656]},{"input":[true,15.0,-2.149986182489314,true,false,0.0013015545871289842,false,0.0],"torque":-0.253288261232873,"active":true,"saturated":false,"log":[0.10849159210920334,0.21698318421840668,0.020406395196914673,0.0,0.26696550846099854,-0.25328826904296875,0.21085184812545776,0.3020106256008148,-0.4043635129928589]},{"input":[true,15.0,-2.0648883361268373,true,false,0.0012593937544762292,false,0.0],"torque":-0.2400389628060817,"active":true,"saturated":false,"log":[0.10571765154600143,0.21143530309200287,0.020406395196914673,0.0,0.24613326787948608,-0.24003896117210388,0.20402179658412933,0.2928497791290283,-0.4425222873687744]},{"input":[true,15.0,-1.9769663368836474,true,false,0.001215833724992072,false,0.0],"torque":-0.22657099830789323,"active":true,"saturated":false,"log":[0.10282625257968903,0.20565250515937805,0.020406395196914673,0.0,0.22510051727294922,-0.2265709936618805,0.1969650685787201,0.28336358070373535,-0.480212926864624]},{"input":[true,15.0,-1.8863178668248766,true,false,0.0011709228942279536,false,0.0],"torque":-0.21289556853155364,"active":true,"saturated":false,"log":[0.09982061386108398,0.19964122772216797,0.020406395196914673,0.0,0.203883096575737,-0.21289557218551636,0.1896895170211792,0.27356258034706116,-0.5173919200897217]},{"input":[true,15.0,-1.7930436371471818,true,false,0.0011247111584866602,false,0.0],"torque":-0.19902456002491178,"active":true,"saturated":false,"log":[0.09670407325029373,0.19340814650058746,0.020406395196914673,0.0,0.18249791860580444,-0.19902455806732178,0.18220320343971252,0.26345765590667725,-0.5540164113044739]},{"input":[true,15.0,-1.6972472762877817,true,false,0.0010772498593871215,false,0.0],"torque":-0.18497014077718524,"active":true,"saturated":false,"log":[0.09348009526729584,0.18696019053459167,0.020406395196914673,0.0,0.1609622687101364,-0.18497014045715332,0.17451447248458862,0.2530600130558014,-0.5900443196296692]},{"input":[true,15.0,-1.5990352147924034,true,false,0.0010285917268234453,false,0.0],"torque":-0.1707446564146222,"active":true,"saturated":false,"log":[0.09015225619077682,0.18030451238155365,0.020406395196914673,0.0,0.13929352164268494,-0.1707446575164795,0.16663186252117157,0.24238121509552002,-0.6254342794418335]},{"input":[true,15.0,-1.4985165670700693,true,false,0.0009787908203815724,false,0.0],"torque":-0.15636069917091056,"active":true,"saturated":false,"log":[0.08672425895929337,0.17344851791858673,0.020406395196914673,0.0,0.11750926077365875,-0.1563607007265091,0.15856412053108215,0.23143313825130463,-0.6601456999778748]},{"input":[true,15.0,-1.3958030101660999,true,false,0.0009279024692786355,false,0.0],"torque":-0.14183118175068735,"active":true,"saturated":false,"log":[0.08319991081953049,0.16639982163906097,0.020406395196914673,0.0,0.09562753140926361,-0.141831174492836,0.15032020211219788,0.2202279418706894,-0.6941389441490173]},{"input":[true,15.0,-1.2910086596879837,true,false,0.0008759832108917369,false,0.0],"torque":-0.12716943842764625,"active":true,"saturated":false,"log":[0.07958313077688217,0.15916626155376434,0.020406395196914673,0.0,0.07366697490215302,-0.1271694451570511,0.14190928637981415,0.20877805352210999,-0.7273750901222229]},{"input":[true,15.0,-1.1842499430220066,true,false,0.0008230907279444615,false,0.0],"torque":-0.11238927709914814,"active":true,"saturated":false,"log":[0.07587792724370956,0.15175585448741913,0.020406395196914673,0.0,0.05164700746536255,-0.11238927394151688,0.13334070146083832,0.19709622859954834,-0.7598162889480591]},{"input":[true,15.0,-1.0756454699814622,false,false,0.0007692837844208935,true,0.0],"torque":-0.09545560333997012,"active":true,"saturated":false,"log":[0.07208842784166336,0.14417685568332672,0.01632511615753174,0.0,0.02958790399134159,-0.095455601811409,0.12462397664785385,0.1851954162120819,-0.7914255261421204]},{"input":[true,15.0,-0.9653159010301797,false,false,0.0007146221602783498,false,0.0],"torque":-0.07943242482776063,"active":true,"saturated":false,"log":[0.06821883469820023,0.13643766939640045,0.01632511615753174,0.0,0.005420237313956022,-0.0794324278831482,0.11576879024505615,0.17308884859085083,-0.8221668601036072]},{"input":[true,15.0,-0.8533838132277445,false,false,0.0006591665850313498,false,0.0],"torque":-0.06251703114241658,"active":true,"saturated":false,"log":[0.06427344679832458,0.12854689359664917,0.01632511615753174,0.0,-0.020373279228806496,-0.06251703202724457,0.10678498446941376,0.16078998148441315,-0.8520053625106812]},{"input":[true,15.0,-0.739973564045384,false,false,0.0006029786702806292,false,0.0],"torque":-0.04551671444573497,"active":true,"saturated":false,"log":[0.060256656259298325,0.12051331251859665,0.01632511615753174,0.0,-0.046193815767765045,-0.045516714453697205,0.09768254309892654,0.14831247925758362,-0.8809070587158203]},{"input":[true,15.0,-0.6252111532037876,false,false,0.000546120841262141,false,0.0],"torque":-0.028450083355291454,"active":true,"saturated":false,"log":[0.056172918528318405,0.11234583705663681,0.01632511615753174,0.0,-0.0720132365822792,-0.02845008298754692,0.08847157657146454,0.13567019999027252,-0.908839225769043]},{"input":[true,15.0,-0.5092240826863872,false,false,0.0004886562674921086,false,0.0],"torque":-0.011336189753323596,"active":true,"saturated":false,"log":[0.05202677100896835,0.1040535420179367,0.01632511615753174,0.0,-0.09780266135931015,-0.011336189694702625,0.07916231453418732,0.12287718802690506,-0.9357701539993286]},{"input":[true,15.0,-0.39214121508359484,false,false,0.00043064879258516834,false,0.0],"torque":0.005805542840987062,"active":true,"saturated":false,"log":[0.047822821885347366,0.09564564377069473,0.01632511615753174,0.0,-0.12353259325027466,0.005805542692542076,0.069765105843544,0.10994765907526016,-0.961669385433197]},{"input":[true,15.0,-0.2740926304254076,false,false,0.0003721628633235905,false,0.0],"torque":0.02295539098625364,"active":true,"saturated":false,"log":[0.04356573894619942,0.08713147789239883,0.01632511615753174,0.0,-0.1491730511188507,0.02295539155602455,0.0602903850376606,0.0968959778547287,-0.9865075349807739]},{"input":[true,15.0,-0.1552094816614225,false,false,0.0003132634580563725,false,0.0],"torque":0.04009337377189388,"active":true,"saturated":false,"log":[0.03926025703549385,0.0785205140709877,0.01632511615753174,0.0,-0.17469362914562225,0.040093373507261276,0.05074867978692055,0.08373664319515228,-1.0102564096450806]},{"input":[true,15.0,-0.0356238489488348,false,false,0.00025401601450776096,false,0.0],"torque":0.05719930470528918,"active":true,"saturated":false,"log":[0.034911155700683594,0.06982231140136719,0.01632511615753174,0.0,-0.20006363093852997,0.057199303060770035,0.041150595992803574,0.07048428058624268,-1.0328893661499023]},{"input":[true,15.0,0.0845314070897141,false,false,0.00019448635707539666,false,0.0],"torque":0.07425278522485962,"active":true,"saturated":false,"log":[0.0305232685059309,0.0610465370118618,0.01632511615753174,0.0,-0.22525203227996826,0.07425278425216675,0.03150679171085358,0.0571536049246788,-1.0543806552886963]},{"input":[true,15.0,0.2051227929753769,false,false,0.00013474062369886897,false,0.0],"torque":0.09123319660235024,"active":true,"saturated":false,"log":[0.02610146999359131,0.05220293998718262,0.01632511615753174,0.0,-0.2502274513244629,0.09123319387435913,0.021827982738614082,0.043759431689977646,-1.074705958366394]},{"input":[true,15.0,0.3260163306854503,false,false,7.484519237992288e-05,false,0.0],"torque":0.10811967473946552,"active":true,"saturated":false,"log":[0.02165067195892334,0.04330134391784668,0.01632511615753174,0.0,-0.2749580442905426,0.1081196740269661,0.012124922126531601,0.030316639691591263,-1.0938423871994019]},{"input":[true,15.0,0.44707770650407075,false,false,1.4866607435947705e-05,false,0.0],"torque":0.12489110942455317,"active":true,"saturated":false,"log":[0.017175819724798203,0.034351639449596405,0.01632511615753174,0.0,-0.2994116544723511,0.12489110976457596,0.0024083915632218122,0.016840169206261635,-1.1117682456970215]},{"input":[true,15.0,0.5681724202458526,false,false,-4.5128494431304356e-05,false,0.0],"torque":0.1415262142744042,"active":true,"saturated":false,"log":[0.01268188375979662,0.02536376751959324,0.01632511615753174,0.0,-0.32355570793151855,0.1415262073278427,-0.007310815155506134,0.0033449865877628326,-1.1284633874893188]},{"input":[true,15.0,0.6891659346867047,false,false,-0.00010507345816964415,false,0.0],"torque":0.15798515286287607,"active":true,"saturated":false,"log":[0.008173859678208828,0.016347719356417656,0.016353724524378777,0.0,-0.347347617149353,0.1579851508140564,-0.017021900042891502,-0.01015391107648611,-1.143908977508545]},{"input":[true,15.0,0.8099238250357874,false,false,-0.0001649016844307542,false,0.0],"torque":0.17428004365042843,"active":true,"saturated":false,"log":[0.0036567538045346737,0.007313507609069347,0.016366522759199142,0.0,-0.37077775597572327,0.174280047416687,-0.02671407163143158,-0.02364152856171131,-1.1580874919891357]},{"input":[true,15.0,0.9303119282825725,false,false,-0.0002245467035626178,false,0.0],"torque":0.19039677378157835,"active":true,"saturated":false,"log":[-0.000864414672832936,-0.001728829345665872,0.01636349782347679,0.0,-0.3938286006450653,0.1903967708349228,-0.03637656569480896,-0.03710287809371948,-1.170982837677002]},{"input":[true,15.0,1.0501964922530456,false,false,-0.00028394224945784677,false,0.0],"torque":0.20631888834392825,"active":true,"saturated":false,"log":[-0.005384622607380152,-0.010769245214760303,0.016344651579856873,0.0,-0.41647735238075256,0.20631888508796692,-0.04599864408373833,-0.05052300915122032,-1.1825804710388184]},{"input":[true,15.0,1.1694443242094754,false,true,-0.0003430223331758737,false,0.0],"torque":0.2220122710500291,"active":true,"saturated":false,"log":[-0.009898848831653595,-0.01979769766330719,0.016344651579856873,0.0,-0.43870067596435547,0.22201226651668549,-0.055569615215063095,-0.06388700753450394,-1.192867398262024]},{"input":[true,15.0,1.287922938828638,false,true,-0.00040172131625720533,false,0.0],"torque":0.23746948237692245,"active":true,"saturated":false,"log":[-0.014402076601982117,-0.028804153203964233,0.016344651579856873,0.0,-0.4604749381542206,0.23746947944164276,-0.06507885456085205,-0.07718002796173096,-1.2018319368362427]},{"input":[true,15.0,1.4055007053941082,false,true,-0.0004599739836482962,false,0.0],"torque":0.25267356662761936,"active":true,"saturated":false,"log":[-0.018889304250478745,-0.03777860850095749,0.016344651579856873,0.0,-0.48177626729011536,0.25267356634140015,-0.07451578229665756,-0.09038729965686798,-1.2094638347625732]},{"input":[true,15.0,1.5220469940390662,false,true,-0.000517715616156007,false,0.0],"torque":0.26760765143674314,"active":true,"saturated":false,"log":[-0.023355543613433838,-0.046711087226867676,0.016344651579856873,0.0,-0.5025810599327087,0.2676076591014862,-0.08386992663145065,-0.10349414497613907,-1.215754508972168]},{"input":[true,15.0,1.6374323208771564,false,true,-0.0005748820623511629,false,0.0],"torque":0.2822550703366133,"active":true,"saturated":false,"log":[-0.027795838192105293,-0.05559167638421059,0.016344651579856873,0.0,-0.5228660106658936,0.282255083322525,-0.09313089400529861,-0.11648601293563843,-1.2206966876983643]},{"input":[true,15.0,1.7515284918601444,false,true,-0.0006314098098413157,false,0.0],"torque":0.2965994033369034,"active":true,"saturated":false,"log":[-0.0322052501142025,-0.064410500228405,0.016344651579856873,0.0,-0.5426082611083984,0.296599417924881,-0.1022883877158165,-0.12934845685958862,-1.2242848873138428]},{"input":[true,15.0,1.8642087452025726,false,true,-0.0006872360558335404,false,0.0],"torque":0.31062450432214866,"active":true,"saturated":false,"log":[-0.03657887876033783,-0.07315775752067566,0.016344651579856873,0.0,-0.5617856383323669,0.31062451004981995,-0.1113322377204895,-0.14206720888614655,-1.2265148162841797]},{"input":[true,15.0,1.9753478922151397,false,false,-0.0007422987769088523,false,0.0],"torque":0.32438639617007237,"active":true,"saturated":false,"log":[-0.04091187193989754,-0.08182374387979507,0.016201460734009743,0.0,-0.580376148223877,0.3243863880634308,-0.12025240063667297,-0.15462811291217804,-1.2273839712142944]},{"input":[true,15.0,2.0848224563903632,false,false,-0.0007965367979307344,false,0.0],"torque":0.3378050521649949,"active":true,"saturated":false,"log":[-0.04519940912723541,-0.09039881825447083,0.01604326255619526,0.0,-0.5983583927154541,0.3378050625324249,-0.1290389597415924,-0.16701722145080566,-1.2268911600112915]},{"input":[true,15.0,2.192510810586003,false,false,-0.000849889860011222,false,0.0],"torque":0.3508649478724433,"active":true,"saturated":false,"log":[-0.04943672940135002,-0.09887345880270004,0.0158702339977026,0.0,-0.6157112717628479,0.3508649468421936,-0.13768215477466583,-0.1792207807302475,-1.225036859512329]},{"input":[true,15.0,2.298293312153815,false,false,-0.0009022986874590205,false,0.0],"torque":0.36525627512744413,"active":true,"saturated":false,"log":[-0.053619127720594406,-0.10723825544118881,0.015682566910982132,0.0,-0.6358103156089783,0.36525627970695496,-0.1461723893880844,-0.19122521579265594,-1.2218230962753296]},{"input":[true,15.0,2.4020524358635162,false,false,-0.0009537050536352881,false,0.0],"torque":0.3793445554708436,"active":true,"saturated":false,"log":[-0.057741954922676086,-0.11548390984535217,0.015480469912290573,0.0,-0.6554102897644043,0.37934455275535583,-0.15450021624565125,-0.2030172049999237,-1.2172532081604004]},{"input":[true,15.0,2.503672904474283,false,false,-0.001004051845643908,false,0.0],"torque":0.3929945379469024,"active":true,"saturated":false,"log":[-0.061800625175237656,-0.12360125035047531,0.015264167450368404,0.0,-0.6742512583732605,0.3929945230484009,-0.16265639662742615,-0.2145836353302002,-1.2113322019577026]},{"input":[true,15.0,2.60304181680872,false,false,-0.0010532831277843902,false,0.0],"torque":0.4050293721894917,"active":true,"saturated":false,"log":[-0.06579063832759857,-0.13158127665519714,0.015033900737762451,0.0,-0.6899991631507874,0.40502938628196716,-0.17063187062740326,-0.22591166198253632,-1.2040666341781616]},{"input":[true,15.0,2.7000487731869955,false,false,-0.001101344203696894,false,0.0],"torque":0.41475006296313527,"active":true,"saturated":false,"log":[-0.06970755755901337,-0.13941511511802673,0.014789924025535583,0.0,-0.7012713551521301,0.4147500693798065,-0.17841775715351105,-0.23698870837688446,-1.1954644918441772]},{"input":[true,15.0,1.4862896171799072,false,false,-0.0005,false,0.0],"torque":0.4754750610119759,"active":true,"saturated":false,"log":[-0.1985180824995041,-0.3970361649990082,0.014095110818743706,-0.0,-0.563870370388031,0.4754750728607178,-0.08100000023841858,-0.24780245125293732,-0.9363683462142944]},{"input":[true,15.0,1.4840477698971162,false,false,-0.0004988893003505612,false,0.0],"torque":0.24055561803128325,"active":true,"saturated":false,"log":[-0.03770352154970169,-0.07540704309940338,0.014095110818743706,-0.0,-0.41775771975517273,0.24055561423301697,-0.08082006871700287,-0.11249999701976776,-0.8689656257629395]},{"input":[true,15.0,1.4773321881247061,false,false,-0.0004955621360170897,false,0.0],"torque":0.23128140934894237,"active":true,"saturated":false,"log":[-0.038047581911087036,-0.07609516382217407,0.013961944729089737,-0.0,-0.3984667956829071,0.23128141462802887,-0.08028106391429901,-0.11225008964538574,-0.8027921319007874]},{"input":[true,15.0,1.466172707839959,false,false,-0.0004900332889206208,false,0.0],"torque":0.22163069011611305,"active":true,"saturated":false,"log":[-0.03822260722517967,-0.07644521445035934,0.013828164897859097,-0.0,-0.37876367568969727,0.22163069248199463,-0.07938539236783981,-0.11150147765874863,-0.7377849817276001]},{"input":[true,15.0,1.450618908366236,false,false,-0.00048232732261528193,false,0.0],"torque":0.211592822782569,"active":true,"saturated":false,"log":[-0.03822781518101692,-0.07645563036203384,0.013694368302822113,-0.0,-0.3586292564868927,0.21159282326698303,-0.07813702523708344,-0.11025749146938324,-0.6739012002944946]},{"input":[true,15.0,1.4307398921020285,false,false,-0.00047247847315736883,false,0.0],"torque":0.2011595795858406,"active":true,"saturated":false,"log":[-0.038063183426856995,-0.07612636685371399,0.013561146333813667,-0.0,-0.3380478322505951,0.20115958154201508,-0.07654151320457458,-0.1085236445069313,-0.6111159324645996]},{"input":[true,15.0,1.40662397751292,false,false,-0.00046053049700144256,false,0.0],"torque":0.19032527187758944,"active":true,"saturated":false,"log":[-0.03772944584488869,-0.07545889168977737,0.013429094105958939,-0.0,-0.3170071244239807,0.1903252750635147,-0.07460594177246094,-0.10630765557289124,-0.5494213104248047]},{"input":[true,15.0,1.37837830675043,false,false,-0.00044653647659921467,false,0.0],"torque":0.17908949589814907,"active":true,"saturated":false,"log":[-0.03722808510065079,-0.07445617020130157,0.01329879555851221,-0.0,-0.2955039143562317,0.1790895015001297,-0.07233890891075134,-0.10361935943365097,-0.4888244867324829]},{"input":[true,15.0,1.3461283696410151,false,false,-0.00043055858456490516,false,0.0],"torque":0.1674650355527727,"active":true,"saturated":false,"log":[-0.03656132519245148,-0.07312265038490295,0.01317083090543747,-0.0,-0.2735597789287567,0.16746503114700317,-0.06975048780441284,-0.10047070682048798,-0.42934665083885193]},{"input":[true,15.0,1.3100174461600431,false,false,-0.0004126678074548392,false,0.0],"torque":0.15549157649202447,"active":true,"saturated":false,"log":[-0.03573213145136833,-0.07146426290273666,0.01304576825350523,-0.0,-0.2512483298778534,0.15549157559871674,-0.06685218214988708,-0.09687568247318268,-0.3710213303565979]},{"input":[true,15.0,1.2702059698677188,false,false,-0.00039294363038847404,false,0.0],"torque":0.143247003873808,"active":true,"saturated":false,"log":[-0.03474418818950653,-0.06948837637901306,0.012924163602292538,-0.0,-0.2287178784608841,0.14324700832366943,-0.06365686655044556,-0.09285025298595428,-0.31389307975769043]},{"input":[true,15.0,1.2268708151350876,false,false,-0.00037147368391202205,false,0.0],"torque":0.1308371005567202,"active":true,"saturated":false,"log":[-0.03360188379883766,-0.06720376759767532,0.01280655711889267,-0.0,-0.20617079734802246,0.1308370977640152,-0.0601787343621254,-0.0884123146533966,-0.2580161988735199]},{"input":[true,15.0,1.1802045113268296,false,false,-0.0003483533546735827,false,0.0],"torque":0.11835635970126865,"active":true,"saturated":false,"log":[-0.03231029212474823,-0.06462058424949646,0.012693471275269985,-0.0,-0.18378563225269318,0.1183563619852066,-0.056433241814374924,-0.0835815817117691,-0.2034534513950348]},{"input":[true,15.0,1.130414387432067,false,false,-0.0003236853616394762,false,0.0],"torque":0.10988367647982929,"active":true,"saturated":false,"log":[-0.030875151976943016,-0.06175030395388603,0.012585408054292202,-0.0,-0.16967475414276123,0.10988367348909378,-0.05243702605366707,-0.07837950438261032,-0.15027473866939545]},{"input":[true,15.0,1.0777216509434209,false,false,-0.0002975792997345639,false,0.0],"torque":0.1042916910151289,"active":true,"saturated":false,"log":[-0.029302839189767838,-0.058605678379535675,0.012482848018407822,-0.0,-0.16158071160316467,0.10429169237613678,-0.0482078455388546,-0.07282920926809311,-0.09855590760707855]},{"input":[true,15.0,1.022360405076675,false,false,-0.00027015115293406986,false,0.0],"torque":0.09838835706102975,"active":true,"saturated":false,"log":[-0.02760034054517746,-0.05520068109035492,0.012386246584355831,-0.0,-0.15313288569450378,0.09838835895061493,-0.04376448690891266,-0.06695534288883209,-0.04837757349014282]},{"input":[true,15.0,0.9645766086973364,false,false,-0.00024152277897013832,false,0.0],"torque":0.09218794685730433,"active":true,"saturated":false,"log":[-0.02577521838247776,-0.05155043676495552,0.012296034023165703,-0.0,-0.14434495568275452,0.09218794852495193,-0.0391266904771328,-0.06078400835394859,0.00017604546155780554]},{"input":[true,15.0,0.9046269835749321,false,false,-0.0002118213679424723,false,0.0],"torque":0.0857759505973018,"active":true,"saturated":false,"log":[-0.02383558452129364,-0.04767116904258728,0.012212608940899372,-0.0,-0.1353713721036911,0.0857759490609169,-0.03431506082415581,-0.05434262380003929,0.04701821133494377]},{"input":[true,15.0,0.8427778738198919,false,false,-0.00018117887723833681,false,0.0],"torque":0.07909769036970286,"active":true,"saturated":false,"log":[-0.021790051832795143,-0.043580103665590286,0.012136343866586685,-0.0,-0.12608635425567627,0.0790976881980896,-0.029350977391004562,-0.04765980690717697,0.09206075221300125]},{"input":[true,15.0,0.7793040625703076,false,false,-0.00014973144527246395,true,0.0],"torque":0.07217811847704023,"active":true,"saturated":false,"log":[-0.019647710025310516,-0.03929542005062103,0.012067576870322227,-0.0,-0.11652176827192307,0.07217811793088913,-0.024256493896245956,-0.040765248239040375,0.13521508872509003]},{"input":[true,15.0,0.7144875511857933,false,false,-0.00011761878665149471,false,0.0],"torque":0.06504018006310007,"active":true,"saturated":false,"log":[-0.01741807907819748,-0.03483615815639496,0.012006613425910473,-0.0,-0.10670457035303116,0.06504017859697342,-0.019054241478443146,-0.03368957340717316,0.17639319598674774]},{"input":[true,15.0,0.6486163063722397,false,false,-8.498357145012052e-05,false,0.0],"torque":0.052851886396194515,"active":true,"saturated":false,"log":[-0.015111061744391918,-0.030222123488783836,0.011953724548220634,-0.0,-0.0869917944073677,0.05285188555717468,-0.013767337426543236,-0.026464227586984634,0.2155085653066635]},{"input":[true,15.0,0.5819829808037364,false,false,-5.197079135667549e-05,false,0.0],"torque":0.04022052396649857,"active":true,"saturated":false,"log":[-0.0127369100227952,-0.0254738200455904,0.01190914586186409,-0.0,-0.06653901934623718,0.04022052511572838,-0.008419266901910305,-0.019121304154396057,0.25247710943222046]},{"input":[true,15.0,0.5148836129256831,false,false,-1.872711550426666e-05,false,0.0],"torque":0.02768433315761869,"active":true,"saturated":false,"log":[-0.010306170210242271,-0.020612340420484543,0.011873073875904083,-0.0,-0.046397335827350616,0.02768433280289173,-0.003033791668713093,-0.01169342827051878,0.2872179448604584]},{"input":[true,15.0,0.4476163117155989,false,false,1.4599761150644409e-05,false,0.0],"torque":0.015304323369893553,"active":true,"saturated":false,"log":[-0.007829641923308372,-0.015659283846616745,0.011845670640468597,-0.0,-0.02666681818664074,0.0153043232858181,0.0023651623632758856,-0.0042136008851230145,0.31965428590774536]},{"input":[true,15.0,0.38047993224497456,false,false,4.786177400718783e-05,false,0.0],"torque":0.003144105263759479,"active":true,"saturated":false,"log":[-0.0053183287382125854,-0.010636657476425171,0.011827056296169758,-0.0,-0.007452270481735468,0.0031441051978617907,0.0077536082826554775,0.0032849463168531656,0.34971415996551514]},{"input":[true,15.0,0.31377274792638066,false,false,8.091114664128603e-05,false,0.0],"torque":-0.008731461506191213,"active":true,"saturated":false,"log":[-0.002783386968076229,-0.005566773936152458,0.01181731466203928,-0.0,0.01113919634371996,-0.008731461130082607,0.013107607141137123,0.010768898762762547,0.3773309886455536]},{"input":[true,15.0,0.247791125344768,false,false,0.00011360104734654355,false,0.0],"torque":-0.02025700953731973,"active":true,"saturated":false,"log":[-0.00023607922776136547,-0.00047215845552273095,0.011816487647593021,-0.0,0.028999796137213707,-0.0202570091933012,0.018403369933366776,0.018205007538199425,0.4024445116519928]},{"input":[true,15.0,0.18282820756041973,false,false,0.0001457862414779463,false,0.0],"torque":-0.031377286909298,"active":true,"saturated":false,"log":[0.0023122774437069893,0.004624554887413979,0.01182458084076643,-0.0,0.04604211077094078,-0.03137728571891785,0.023617371916770935,0.02556023560464382,0.4250010848045349]},{"input":[true,15.0,0.11917261173337437,false,false,0.0001773237367001388,false,0.0],"torque":-0.04204295044519819,"active":true,"saturated":false,"log":[0.004850361030548811,0.009700722061097622,0.01184155698865652,-0.0,0.062190551310777664,-0.04204295203089714,0.028726445510983467,0.032801903784275055,0.44495439529418945]},{"input":[true,15.0,0.05710714685551141,false,false,0.0002080734182735712,false,0.0],"torque":-0.05220945871921539,"active":true,"saturated":false,"log":[0.007366895209997892,0.014733790419995785,0.011867341585457325,-0.0,0.0773790255188942,-0.052209459245204926,0.03370789438486099,0.03989784047007561,0.4622659683227539]},{"input":[true,15.0,-0.0030924427128407594,false,false,0.0002378986715560677,false,0.0],"torque":-0.06183609282986415,"active":true,"saturated":false,"log":[0.00985070038586855,0.0197014007717371,0.01190181914716959,-0.0,0.09154892712831497,-0.06183609366416931,0.0385395847260952,0.046816520392894745,0.47690549492836]},{"input":[true,15.0,-0.06115870231956194,false,false,0.00026666698895416686,false,0.0],"torque":-0.07088527086623701,"active":true,"saturated":false,"log":[0.012290740385651588,0.024581480771303177,0.011944836005568504,-0.0,0.10464764386415482,-0.07088527083396912,0.0432000532746315,0.05352720245718956,0.48885127902030945]},{"input":[true,15.0,-0.11683365526789508,false,false,0.0002942505586276729,false,0.0],"torque":-0.07932215119576423,"active":true,"saturated":false,"log":[0.014676175080239773,0.029352350160479546,0.011996203102171421,-0.0,0.11662764847278595,-0.07932215183973312,0.047668591141700745,0.06000007316470146,0.49809056520462036]},{"input":[true,15.0,-0.16986994895495067,false,false,0.0003205268323319191,false,0.0],"torque":-0.08711443919455529,"active":true,"saturated":false,"log":[0.01699640601873398,0.03399281203746796,0.01205569040030241,-0.0,0.12744608521461487,-0.08711443841457367,0.05192534625530243,0.06620637327432632,0.5046197772026062]},{"input":[true,15.0,-0.2200319538095064,false,false,0.00034537906987493823,false,0.0],"torque":-0.0942323361028052,"active":true,"saturated":false,"log":[0.019241126254200935,0.03848225250840187,0.012123034335672855,-0.0,0.13706448674201965,-0.09423233568668365,0.05595140904188156,0.07211853563785553,0.5084447860717773]},{"input":[true,15.0,-0.2670968101467332,false,false,0.0003686968577706227,false,0.0],"torque":-0.10064856232979959,"active":true,"saturated":false,"log":[0.021400362253189087,0.042800724506378174,0.012197935953736305,-0.0,0.14544880390167236,-0.10064855962991714,0.05972889065742493,0.07771029323339462,0.5095809698104858]},{"input":[true,15.0,-0.310855418288893,false,false,0.0003903765997836073,false,0.0],"torque":-0.10633842929996844,"active":true,"saturated":false,"log":[0.023464519530534744,0.04692903906106949,0.012280061841011047,-0.0,0.15256941318511963,-0.10633842647075653,0.06324101239442825,0.08295679092407227,0.5080534815788269]},{"input":[true,15.0,-0.35111336755309147,false,false,0.0004103219771864765,false,0.0],"torque":-0.11127993461176927,"active":true,"saturated":false,"log":[0.025424430146813393,0.050848860293626785,0.012369046919047832,-0.0,0.15840120613574982,-0.1112799346446991,0.06647215783596039,0.08783473819494247,0.5038970708847046]},{"input":[true,15.0,-0.38769179997878994,false,false,0.00042844437668447364,false,0.0],"torque":-0.11545385011082465,"active":true,"saturated":false,"log":[0.027271384373307228,0.054542768746614456,0.012464497238397598,-0.0,0.16292373836040497,-0.11545384675264359,0.06940799206495285,0.0923224464058876,0.4971562623977661]},{"input":[true,15.0,-0.42042820495769867,false,false,0.0004446632841065207,false,0.0],"torque":-0.1188438300558056,"active":true,"saturated":false,"log":[0.028997177258133888,0.057994354516267776,0.012565987184643745,-0.0,0.16612131893634796,-0.11884383112192154,0.07203545421361923,0.09639998525381088,0.48788532614707947]},{"input":[true,15.0,-0.4491771412356825,false,false,0.0004589066421134661,false,0.0],"torque":-0.12143651463733053,"active":true,"saturated":false,"log":[0.03059414029121399,0.06118828058242798,0.012673066928982735,-0.0,0.16798314452171326,-0.12143651396036148,0.07434287667274475,0.10004924237728119,0.4761478900909424]},{"input":[true,15.0,-0.47381088307895775,false,false,0.000471111170334329,false,0.0],"torque":-0.12322166733768415,"active":true,"saturated":false,"log":[0.03205518051981926,0.06411036103963852,0.012785259634256363,-0.0,0.1685035228729248,-0.12322166562080383,0.07632000744342804,0.10325399786233902,0.46201711893081665]},{"input":[true,15.0,-0.49421998773380665,false,false,0.00048122264650824387,false,0.0],"torque":-0.12419235565627554,"active":true,"saturated":false,"log":[0.03337380662560463,0.06674761325120926,0.012902067974209785,-0.0,0.16768214106559753,-0.12419235706329346,0.07795806974172592,0.10600001364946365,0.4455752670764923]},{"input":[true,15.0,-0.5103137816586865,false,false,0.0004891961473830436,false,0.0],"torque":-0.12434523934487844,"active":true,"saturated":false,"log":[0.03454415872693062,0.06908831745386124,0.013022972270846367,-0.0,0.16552461683750153,-0.12434524297714233,0.0792497768998146,0.10827509313821793,0.42691347002983093]},{"input":[true,15.0,-0.5220207633685157,false,false,0.0004949962483002228,false,0.0],"torque":-0.12368102728180089,"active":true,"saturated":false,"log":[0.03556104004383087,0.07112208008766174,0.013147436082363129,-0.0,0.16204328835010529,-0.12368102371692657,0.08018939197063446,0.11006913334131241,0.4061315059661865]},{"input":[true,15.0,-0.5292889211013694,false,false,0.0004985971805795595,false,0.0],"torque":-0.1222051437689514,"active":true,"saturated":false,"log":[0.03641992807388306,0.07283985614776611,0.01327490620315075,-0.0,0.15725857019424438,-0.12220514565706253,0.08077274262905121,0.1113741546869278,0.3833373785018921]},{"input":[true,15.0,-0.5320859638962641,false,false,0.0004999829460041724,false,0.0],"torque":-0.11992867032172967,"active":true,"saturated":false,"log":[0.03711701184511185,0.0742340236902237,0.013404815457761288,-0.0,0.15120072662830353,-0.11992867290973663,0.08099723607301712,0.11218436807394028,0.3586469292640686]},{"input":[true,15.0,-0.5303994650553927,false,false,0.0004991473878973766,false,0.0],"torque":-0.116869389255594,"active":true,"saturated":false,"log":[0.03764919191598892,0.07529838383197784,0.013536587357521057,-0.0,0.1439119428396225,-0.11686939001083374,0.0808618813753128,0.11249615997076035,0.3321834206581116]},{"input":[true,15.0,-0.5242369173534407,false,false,0.000496094218475557,false,0.0],"torque":-0.11305264630809161,"active":true,"saturated":false,"log":[0.03801410645246506,0.07602821290493011,0.013669637031853199,-0.0,0.13544799387454987,-0.11305264383554459,0.08036726713180542,0.11230815947055817,0.3040769696235657]},{"input":[true,15.0,-0.5136256997486968,false,false,0.0004908370023555395,false,0.0],"torque":-0.10851136096674094,"active":true,"saturated":false,"log":[0.038210127502679825,0.07642025500535965,0.013803372159600258,-0.0,0.12587828934192657,-0.10851135849952698,0.07951559871435165,0.1116212010383606,0.2744641900062561]},{"input":[true,15.0,-0.49861295574385617,false,false,0.00048339909628973055,false,0.0],"torque":-0.1032845798197124,"active":true,"saturated":false,"log":[0.03823639079928398,0.07647278159856796,0.013937199488282204,-0.0,0.11528290063142776,-0.10328458249568939,0.07831065356731415,0.11043832451105118,0.24348755180835724]},{"input":[true,15.0,-0.47926538393693124,false,false,0.00047381354539676926,false,0.0],"torque":-0.09741430840981592,"active":true,"saturated":false,"log":[0.038092777132987976,0.07618555426597595,0.014070524834096432,-0.0,0.10374629497528076,-0.09741430729627609,0.07675779610872269,0.10876479744911194,0.21129480004310608]},{"input":[true,15.0,-0.4556689416928165,false,false,0.0004621229363487218,false,0.0],"torque":-0.09120948768739967,"active":true,"saturated":false,"log":[0.037779923528432846,0.07555984705686569,0.014202754013240337,-0.0,0.09188303351402283,-0.09120948612689972,0.07486391812562943,0.10660804808139801,0.17803837358951569]},{"input":[true,15.0,-0.42792846325202555,false,false,0.00044837920816707353,false,0.0],"torque":-0.08881160893727484,"active":true,"saturated":false,"log":[0.037299223244190216,0.07459844648838043,0.014333301223814487,-0.0,0.08793878555297852,-0.08881160616874695,0.07263743132352829,0.10397765785455704,0.14387477934360504]},{"input":[true,15.0,-0.3961671939732769,false,false,0.00043264342146811797,false,0.0],"torque":-0.0859375130248252,"active":true,"saturated":false,"log":[0.036652810871601105,0.07330562174320221,0.014461586251854897,0.0,0.0833798497915268,-0.0859375149011612,0.07008823752403259,0.10088532418012619,0.1089639663696289]},{"input":[true,15.0,-0.36052624277918427,false,false,0.00041498548718293574,false,0.0],"torque":-0.08260223153815774,"active":true,"saturated":false,"log":[0.03584355488419533,0.07168710976839066,0.0145870391279459,0.0,0.07823099195957184,-0.08260223269462585,0.06722764670848846,0.097344771027565,0.07346858084201813]},{"input":[true,15.0,-0.3211639552377363,false,false,0.00039548385595720845,false,0.0],"torque":-0.07882009473216964,"active":true,"saturated":false,"log":[0.0348750539124012,0.0697501078248024,0.014709101058542728,0.0,0.07251408696174622,-0.07882009446620941,0.06406838446855545,0.09337173402309418,0.03755338490009308]},{"input":[true,15.0,-0.27825521006482573,false,false,0.00037422516961079937,false,0.0],"torque":-0.07460563978844856,"active":true,"saturated":false,"log":[0.03375161066651344,0.06750322133302689,0.014827231876552105,0.0,0.06625000387430191,-0.07460563629865646,0.06062448024749756,0.08898387104272842,0.0013844863278791308]},{"input":[true,15.0,-0.23199064217333892,false,false,0.0003513038762055989,false,0.0],"torque":-0.07004487573300766,"active":true,"saturated":false,"log":[0.032478217035532,0.064956434071064,0.014940906316041946,0.0,0.05960056558251381,-0.0700448751449585,0.05691123008728027,0.0842006653547287,-0.034871336072683334]},{"input":[true,15.0,-0.18257579572062937,false,false,0.00032682181043180595,false,0.0],"torque":-0.06508829164382102,"active":true,"saturated":false,"log":[0.03106052801012993,0.06212105602025986,0.015049617737531662,0.0,0.052456341683864594,-0.06508829444646835,0.05294513329863548,0.07904337346553802,-0.0710473358631134]},{"input":[true,15.0,-0.13023021091722609,false,false,0.0003008877411769109,false,0.0],"torque":-0.05975506628858747,"active":true,"saturated":false,"log":[0.029504843056201935,0.05900968611240387,0.015152884647250175,0.0,0.0448434054851532,-0.05975506454706192,0.04874381422996521,0.07353490591049194,-0.10697753727436066]},{"input":[true,15.0,-0.07518644865389124,false,false,0.00027361688828743294,false,0.0],"torque":-0.05406688049020673,"active":true,"saturated":false,"log":[0.027818074449896812,0.055636148899793625,0.015250247903168201,0.0,0.036791544407606125,-0.05406688153743744,0.04432593658566475,0.06769974529743195,-0.14249739050865173]},{"input":[true,15.0,-0.017689057280424858,false,false,0.0002451304106703497,false,0.0],"torque":-0.048044496291574076,"active":true,"saturated":false,"log":[0.026007715612649918,0.052015431225299835,0.015341275371611118,0.0,0.02832755073904991,-0.04804449528455734,0.039711128920316696,0.061563801020383835,-0.17744456231594086]},{"input":[true,15.0,0.04200651387340487,false,false,0.00021555486800847306,false,0.0],"torque":-0.03737613160159038,"active":true,"saturated":false,"log":[0.02408180944621563,0.04816361889243126,0.015425560995936394,0.0,0.010848459787666798,-0.03737613186240196,0.03491989150643349,0.05515434220433235,-0.2116595357656479]},{"input":[false,15.0,0.10363504940778356,false,false,0.00018502165848126905,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.16692274615145833,false,false,0.0001536664349892097,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.23158842961407955,false,false,0.00012162850247525857,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.2973448031878373,false,false,8.905019902106331e-05,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.3638997245484247,false,false,5.6076263467527434e-05,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.430957503584548,false,false,2.2853192369290225e-05,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.4982202160895353,false,false,-1.0471410859959058e-05,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.5653890273785891,false,false,-4.37494917197232e-05,true,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.6321655199511014,false,false,-7.683320239902244e-05,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.6982530192995089,false,false,-0.00010957555863444268,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.7633579119743754,false,false,-0.00014183109273161314,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.8271909500498194,false,false,-0.00017345649984887746,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.8894685361937891,false,false,-0.00020431127467184204,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.9499139836338912,false,false,-0.00023425833565018857,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.0082587454209988,false,false,-0.00026316463402339594,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,15.0,1.064243607529258,false,false,-0.00029090174492958534,false,0.0],"torque":0.08564503836454129,"active":true,"saturated":false,"log":[-0.014383972622454166,-0.028767945244908333,-5.034390414948575e-05,0.0,-0.1417504996061325,0.08564503490924835,-0.047126080840826035,-0.05921204388141632,-0.04375579208135605]},{"input":[true,15.0,1.1176198404917992,false,false,-0.00031734643797131734,false,0.0],"torque":0.09246644836203795,"active":true,"saturated":false,"log":[-0.016712848097085953,-0.033425696194171906,-0.00010883887443924323,0.0,-0.1506190299987793,0.09246645122766495,-0.05141012370586395,-0.06545289605855942,-0.0824049711227417]},{"input":[true,15.0,1.1681503044556611,false,false,-0.00034238122470342736,false,0.0],"torque":0.09902974845715615,"active":true,"saturated":false,"log":[-0.01896747015416622,-0.03793494030833244,-0.00017522502457723022,0.0,-0.1591140478849411,0.09902974963188171,-0.055465757846832275,-0.07140295207500458,-0.11612022668123245]},{"input":[true,15.0,1.2156105027464028,false,false,-0.0003658948806105272,false,0.0],"torque":0.1053029653756371,"active":true,"saturated":false,"log":[-0.021137826144695282,-0.042275652289390564,-0.00024920739815570414,0.0,-0.16719220578670502,0.10530296713113785,-0.05927496775984764,-0.07703577727079391,-0.14507202804088593]},{"input":[true,15.0,1.2597895792616083,false,false,-0.00038778293925512483,false,0.0],"torque":0.1112535773262389,"active":true,"saturated":false,"log":[-0.023214269429445267,-0.04642853885889053,-0.00033045734744518995,0.0,-0.17480836808681488,0.11125357449054718,-0.06282083690166473,-0.08232634514570236,-0.16942955553531647]},{"input":[true,15.0,1.3004912552620864,false,false,-0.0004079481564009665,false,0.0],"torque":0.11812849784721488,"active":true,"saturated":false,"log":[-0.025187576189637184,-0.05037515237927437,-0.0004186138685327023,0.0,-0.18446466326713562,0.11812850087881088,-0.06608760356903076,-0.08725116401910782,-0.18936152756214142]},{"input":[true,15.0,1.3375347013987748,false,false,-0.00042630094204959286,false,0.0],"torque":0.125491786984182,"active":true,"saturated":false,"log":[-0.027048980817198753,-0.05409796163439751,-0.0005132852820679545,0.0,-0.1953108012676239,0.12549178302288055,-0.06906075030565262,-0.09178833663463593,-0.20503687858581543]},{"input":[true,15.0,1.3707553411011222,false,false,-0.00044275975847065947,false,0.0],"torque":0.13189068197294168,"active":true,"saturated":false,"log":[-0.02879021130502224,-0.05758042261004448,-0.0006140510668046772,0.0,-0.20447053015232086,0.1318906843662262,-0.07172708213329315,-0.095917709171772,-0.2166254222393036]},{"input":[true,15.0,1.400005581757642,false,false,-0.00045725148245765016,false,0.0],"torque":0.13731627284943246,"active":true,"saturated":false,"log":[-0.030403533950448036,-0.06080706790089607,-0.0007204634021036327,0.0,-0.21194206178188324,0.1373162716627121,-0.07407473772764206,-0.09962094575166702,-0.224298357963562]},{"input":[true,15.0,1.4251554704401619,false,false,-0.00046971173019955657,false,0.0],"torque":0.14176531231799305,"active":true,"saturated":false,"log":[-0.03188177943229675,-0.0637635588645935,-0.0008320496417582035,0.0,-0.21773375570774078,0.1417653113603592,-0.07609330117702484,-0.10288158059120178,-0.2282286286354065]},{"input":[true,15.0,1.446093271258509,false,false,-0.000480085143325183,false,0.0],"torque":0.14524011911925003,"active":true,"saturated":false,"log":[-0.0332183800637722,-0.0664367601275444,-0.0009483139729127288,0.0,-0.22186389565467834,0.14524011313915253,-0.0777737945318222,-0.10568513721227646,-0.22859133780002594]},{"input":[true,15.0,1.4627259617805681,false,false,-0.0004883256348492412,false,0.0],"torque":0.1477483000947938,"active":true,"saturated":false,"log":[-0.03440739959478378,-0.06881479918956757,-0.0010687399189919233,0.0,-0.22436006367206573,0.14774830639362335,-0.07910875231027603,-0.108019158244133,-0.2255638837814331]},{"input":[true,15.0,1.4749796463122105,false,false,-0.0004943965939275431,false,0.0],"torque":0.14930247123774143,"active":true,"saturated":false,"log":[-0.03544355183839798,-0.07088710367679596,-0.0011927923187613487,0.0,-0.225258469581604,0.14930246770381927,-0.08009224385023117,-0.10987326502799988,-0.2193262130022049]},{"input":[true,15.0,1.4827998842009815,false,false,-0.0004982710485116088,false,0.0],"torque":0.14992003958078867,"active":true,"saturated":false,"log":[-0.03632223606109619,-0.07264447212219238,-0.001319920178502798,0.0,-0.2246035784482956,0.1499200463294983,-0.08071991056203842,-0.1112392321228981,-0.21006086468696594]},{"input":[true,15.0,1.486151931704955,false,false,-0.0004999317851800449,false,0.0],"torque":0.14962294471449034,"active":true,"saturated":false,"log":[-0.03703954815864563,-0.07407909631729126,-0.0014495585346594453,0.0,-0.222447469830513,0.14962294697761536,-0.08098895102739334,-0.11211098730564117,-0.19795292615890503]},{"input":[true,15.0,1.4850208963521807,false,false,-0.0004993714256143098,false,0.0],"torque":0.14843729246248544,"active":true,"saturated":false,"log":[-0.03759229928255081,-0.07518459856510162,-0.0015811315970495343,0.0,-0.2188490629196167,0.1484372913837433,-0.08089817315340042,-0.1124846488237381,-0.18319009244441986]},{"input":[true,15.0,1.4794118031049377,false,false,-0.0004965924593790963,false,0.0],"torque":0.14830791270813282,"active":true,"saturated":false,"log":[-0.03797803819179535,-0.0759560763835907,-0.0017140547279268503,0.0,-0.21768704056739807,0.1483079195022583,-0.0804479792714119,-0.11235857009887695,-0.16596241295337677]},{"input":[true,15.0,1.4693495720348397,false,false,-0.0004916072328616975,false,0.0],"torque":0.14808446957043336,"active":true,"saturated":false,"log":[-0.03819504752755165,-0.0763900950551033,-0.0018477373523637652,0.0,-0.21667447686195374,0.14808447659015656,-0.07964037358760834,-0.11173330247402191,-0.14646218717098236]},{"input":[true,15.0,1.4548789076079756,false,false,-0.0004844378944194944,false,0.0],"torque":0.14733549293013565,"active":true,"saturated":false,"log":[-0.038242362439632416,-0.07648472487926483,-0.0019815857522189617,0.0,-0.21495462954044342,0.1473354995250702,-0.07847893983125687,-0.11061162501573563,-0.12488368898630142]},{"input":[true,15.0,1.4360641000719692,false,false,-0.0004751162959792648,false,0.0],"torque":0.1460682168977092,"active":true,"saturated":false,"log":[-0.03811977803707123,-0.07623955607414246,-0.0021150049287825823,0.0,-0.21254292130470276,0.14606821537017822,-0.07696884125471115,-0.10899852961301804,-0.10142286866903305]}]},{"name":"delay_clamped","frames":[{"input":[false,18.0,3.451966396693357,false,false,-0.0013323183357528668,true,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,3.3602463304795016,false,false,-0.001291240963619141,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,3.2653230478597743,false,false,-0.0012487290121424167,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,3.167302009382789,false,false,-0.0012048297124508391,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,3.066292117229424,false,false,-0.0011595918370278437,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.962405594221605,false,false,-0.0011130656455255266,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.8557578591418222,false,false,-0.0010653028289257553,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.7464673985019137,false,false,-0.0010163564521110636,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.6346556349035777,false,false,-0.0009662808949091303,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.52044679213686,false,false,-0.000915131791676342,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.403967757166498,false,false,-0.0008629659694875654,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.2853479391594655,false,false,-0.0008098413850007994,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.1647191257103127,false,false,-0.0007558170600668505,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.0422153364240723,false,false,-0.000700953016155571,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.9179726740193692,false,false,-0.0006453102076715109,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.792129173117182,false,false,-0.000588950454233074,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.6648246468832417,false,false,-0.0005319363719904111,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.5362005316944531,false,false,-0.0004743313040583631,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.4063997300019155,false,false,-0.0004161992501417374,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.275566451565123,false,false,-0.00035760479543111017,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,18.0,1.1438460532337351,false,false,-0.000298613038848147,false,1.4],"torque":-0.02581284375409776,"active":true,"saturated":false,"log":[0.0710289403796196,0.12216977775096893,0.0,-0.0,-0.07077128440141678,-0.025812843814492226,-0.06966044753789902,-0.006783371325582266,-0.004845265299081802]},{"input":[true,18.0,1.0113848774549183,false,false,-0.00023928952072016532,false,1.4],"torque":-0.004487866168858736,"active":true,"saturated":false,"log":[0.049792513251304626,0.08564312011003494,0.00017427379498258233,-0.0,-0.07688116282224655,-0.004487866070121527,-0.055821459740400314,-0.01174353901296854,-0.00838824175298214]},{"input":[true,18.0,0.8783300896856829,false,false,-0.00017970014996429066,false,1.4],"torque":0.014134469732006027,"active":true,"saturated":false,"log":[0.03040803223848343,0.05230181664228439,0.0002807019045576453,-0.0,-0.08072704076766968,0.01413446944206953,-0.04192044958472252,-0.015002291649580002,-0.010715922340750694]},{"input":[true,18.0,0.7448295148908484,false,false,-0.00011991113086210739,false,1.4],"torque":0.030218138669178368,"active":true,"saturated":false,"log":[0.012763302773237228,0.021952880546450615,0.00032537346123717725,-0.0,-0.08244854211807251,0.03021813929080963,-0.027972867712378502,-0.016674384474754333,-0.011910274624824524]},{"input":[true,18.0,0.6110314733082908,false,false,-5.998888950615651e-05,false,1.4],"torque":0.04391777717783566,"active":true,"saturated":false,"log":[-0.003246413078159094,-0.005583830643445253,0.00031401103478856385,-0.0,-0.08217915147542953,0.04391777887940407,-0.013994206674396992,-0.01686803437769413,-0.012048596516251564]},{"input":[true,18.0,0.4770846156639367,false,false,0.0,false,1.4],"torque":0.055379579624798815,"active":true,"saturated":false,"log":[-0.017718957737088203,-0.030476607382297516,0.00025199467199854553,-0.0,-0.08004707843065262,0.055379580706357956,1.4062664366321087e-09,-0.01568538509309292,-0.011203845962882042]},{"input":[true,18.0,0.34313775801958263,false,false,5.998888950615651e-05,false,1.4],"torque":0.06474199420580118,"active":true,"saturated":false,"log":[-0.030745774507522583,-0.05288273096084595,0.0001443844666937366,-0.0,-0.07617577910423279,0.06474199146032333,0.013994209468364716,-0.01322292909026146,-0.009444949217140675]},{"input":[true,18.0,0.2093397164370251,false,false,0.00011991113086210739,false,1.4],"torque":0.07213628113380037,"active":true,"saturated":false,"log":[-0.042412370443344116,-0.07294927537441254,-4.058836111653363e-06,-0.0,-0.07068429887294769,0.0721362829208374,0.02797286957502365,-0.009571912698447704,-0.006837080232799053]},{"input":[true,18.0,0.07583914164219055,false,false,0.00017970014996429066,false,1.4],"torque":0.0776869794667122,"active":true,"saturated":false,"log":[-0.052798762917518616,-0.09081387519836426,-0.00018885450845118612,-0.0,-0.06368748098611832,0.07768698036670685,0.04192045331001282,-0.004818692337721586,-0.0034419228322803974]},{"input":[true,18.0,-0.057215646127044995,false,false,0.00023928952072016532,false,1.4],"torque":0.08152017500638474,"active":true,"saturated":false,"log":[-0.061979856342077255,-0.10660535097122192,-0.00040578399784862995,-0.0,-0.05531175434589386,0.08152017742395401,0.055821459740400314,0.0009549183305352926,0.0006820845301263034]},{"input":[true,18.0,-0.18967682190586188,false,false,0.000298613038848147,false,1.4],"torque":0.08379612694408334,"active":true,"saturated":false,"log":[-0.07002585381269455,-0.12044446915388107,-0.0006508745136670768,-0.0,-0.04575937241315842,0.08379612863063812,0.06966044753789902,0.00767133804038167,0.005479527171701193]},{"input":[true,18.0,-0.32139722023724987,false,false,0.00035760479543111017,false,1.4],"torque":0.08458669634330396,"active":true,"saturated":false,"log":[-0.07700256258249283,-0.13244441151618958,-0.0009203834342770278,-0.0,-0.03506404906511307,0.08458669483661652,0.08342204988002777,0.015256929211318493,0.010897806845605373]},{"input":[true,18.0,-0.45223049867404214,false,false,0.0004161992501417374,false,1.4],"torque":0.08399163295010417,"active":true,"saturated":false,"log":[-0.08297175168991089,-0.14271141588687897,-0.0012107845395803452,-0.0,-0.023321686312556267,0.0839916318655014,0.0970909595489502,0.02364172600209713,0.01688694767653942]},{"input":[true,18.0,-0.58203130036658,false,false,0.0004743313040583631,false,1.4],"torque":0.08210494290058526,"active":true,"saturated":false,"log":[-0.08799143135547638,-0.15134526789188385,-0.001518754637800157,-0.0,-0.010623016394674778,0.08210494369268417,0.11065200716257095,0.03275919333100319,0.02339942380785942]},{"input":[true,18.0,-0.7106554155553682,false,false,0.0005319363719904111,false,1.4],"torque":0.07901524102899361,"active":true,"saturated":false,"log":[-0.09211613237857819,-0.1584397405385971,-0.0018411610508337617,-0.0,0.0029461656231433153,0.07901524007320404,0.12409012019634247,0.04254598915576935,0.030389990657567978]},{"input":[true,18.0,-0.8379599417893087,false,false,0.000588950454233074,false,1.4],"torque":0.07480604718435144,"active":true,"saturated":false,"log":[-0.09539715945720673,-0.1640831083059311,-0.0021750510204583406,-0.0,0.01730484329164028,0.07480604946613312,0.1373903602361679,0.052941758185625076,0.03781554102897644]},{"input":[true,18.0,-0.9638034426914959,false,false,0.0006453102076715109,false,1.4],"torque":0.06955606567601881,"active":true,"saturated":false,"log":[-0.09788285940885544,-0.16835851967334747,-0.002517641056329012,-0.0,0.032376669347286224,0.06955606490373611,0.15053796768188477,0.06388894468545914,0.045634958893060684]},{"input":[true,18.0,-1.088046105096199,false,false,0.000700953016155571,false,1.4],"torque":0.06333934959021295,"active":true,"saturated":false,"log":[-0.09961878508329391,-0.1713443100452423,-0.002866306807845831,-0.0,0.04808991029858589,0.06333935260772705,0.16351832449436188,0.07533259689807892,0.05380899831652641]},{"input":[true,18.0,-1.2105498943824393,false,false,0.0007558170600668505,false,1.4],"torque":0.05622553736195456,"active":true,"saturated":false,"log":[-0.10064796358346939,-0.17311449348926544,-0.003218574682250619,-0.0,0.06437742710113525,0.05622553825378418,0.1763170063495636,0.0872202217578888,0.06230015680193901]},{"input":[true,18.0,-1.3311787078315918,false,false,0.0008098413850007994,false,1.4],"torque":0.048279971758310364,"active":true,"saturated":false,"log":[-0.10101103782653809,-0.1737389862537384,-0.0035721133463084698,-0.0,0.08117667585611343,0.04827997088432312,0.188919797539711,0.0995016098022461,0.07107257843017578]},{"input":[true,18.0,-1.449798525838625,false,false,0.0008629659694875654,false,1.4],"torque":0.03956387556614357,"active":true,"saturated":false,"log":[-0.10074646770954132,-0.1732839196920395,-0.003924726042896509,-0.0,0.09842969477176666,0.039563875645399094,0.20131270587444305,0.11212871968746185,0.0800919458270073]},{"input":[true,18.0,-1.5662775608089865,false,false,0.000915131791676342,false,1.4],"torque":0.03013450401907356,"active":true,"saturated":false,"log":[-0.09989067912101746,-0.17181196808815002,-0.004274343606084585,-0.0,0.1160830482840538,0.030134504660964012,0.21348194777965546,0.12505553662776947,0.08932538330554962]},{"input":[true,18.0,-1.6804864035757048,false,false,0.0009662808949091303,false,1.4],"torque":0.0200453575203487,"active":true,"saturated":false,"log":[-0.09847823530435562,-0.16938257217407227,-0.004619017243385315,-0.0,0.1340876966714859,0.020045356824994087,0.22541400790214539,0.13823793828487396,0.09874138236045837]},{"input":[true,18.0,-1.7922981671740408,false,false,0.0010163564521110636,false,1.4],"torque":0.00934640343488617,"active":true,"saturated":false,"log":[-0.09654194861650467,-0.16605214774608612,-0.004956914111971855,-0.0,0.1523987352848053,0.009346403181552887,0.2370956391096115,0.1516336351633072,0.10830973833799362]},{"input":[true,18.0,-1.9015886278139493,false,false,0.0010653028289257553,false,1.4],"torque":-0.0019155987409258093,"active":true,"saturated":false,"log":[-0.09411302208900452,-0.16187439858913422,-0.005286309402436018,-0.0,0.170974999666214,-0.0019155987538397312,0.2485138475894928,0.16520200669765472,0.11800143122673035]},{"input":[true,18.0,-2.0082363628937325,false,false,0.0011130656455255266,false,1.4],"torque":-0.013696690718292076,"active":true,"saturated":false,"log":[-0.09122118353843689,-0.15690043568611145,-0.005605583544820547,-0.0,0.18977844715118408,-0.013696691021323204,0.2596559524536133,0.17890405654907227,0.12778861820697784]},{"input":[true,18.0,-2.112122885901551,false,false,0.0011595918370278437,false,1.4],"torque":-0.02595527309873171,"active":true,"saturated":false,"log":[-0.08789476007223129,-0.1511789858341217,-0.005913215223699808,-0.0,0.20877346396446228,-0.02595527283847332,0.2705095708370209,0.19270233809947968,0.13764452934265137]},{"input":[true,18.0,-2.213132778054916,false,false,0.0012048297124508391,false,1.4],"torque":-0.03865153284221994,"active":true,"saturated":false,"log":[-0.08416081964969635,-0.14475661516189575,-0.0062077781185507774,-0.0,0.22792589664459229,-0.03865153342485428,0.28106266260147095,0.20656083524227142,0.14754346013069153]},{"input":[true,18.0,-2.311153816531901,false,false,0.0012487290121424167,false,1.4],"torque":-0.05454346722962158,"active":true,"saturated":false,"log":[-0.08004523813724518,-0.13767780363559723,-0.0064879367128014565,-0.0,0.2527705132961273,-0.05454346537590027,0.29130351543426514,0.220444917678833,0.15746065974235535]},{"input":[true,18.0,-2.406077099151629,false,false,0.001291240963619141,false,1.4],"torque":-0.07141409228760662,"active":true,"saturated":false,"log":[-0.07557278126478195,-0.12998518347740173,-0.006752441171556711,-0.0,0.2789342999458313,-0.07141409069299698,0.30122068524360657,0.23432126641273499,0.16737233102321625]},{"input":[true,18.0,-2.4977971653654834,false,false,0.0013323183357528668,false,1.4],"torque":-0.08862168608278763,"active":true,"saturated":false,"log":[-0.07076720148324966,-0.12171958386898041,-0.007000126410275698,-0.0,0.3051791787147522,-0.08862168341875076,0.31080323457717896,0.24815784394741058,0.17725560069084167]},{"input":[true,18.0,-2.5862121134246556,false,false,0.0013719154912449113,false,1.4],"torque":-0.10611916205465059,"active":true,"saturated":false,"log":[-0.0656513199210167,-0.11292026937007904,-0.007229906041175127,-0.0,0.3314495384693146,-0.10611916333436966,0.32004043459892273,0.26192381978034973,0.18708842992782593]},{"input":[true,18.0,-2.67122371359382,false,false,0.0014099884373294702,false,1.4],"torque":-0.12385866134552634,"active":true,"saturated":false,"log":[-0.06024705618619919,-0.10362493991851807,-0.007440770976245403,-0.0,0.3576868176460266,-0.12385866045951843,0.3289220929145813,0.2755894958972931,0.19684964418411255]},{"input":[true,18.0,-2.75273751728518,false,false,0.0014464948746499401,true,1.4],"torque":-0.1417915670546662,"active":true,"saturated":false,"log":[-0.054575517773628235,-0.0938698872923851,-0.007631785236299038,-0.0,0.38382959365844727,-0.14179156720638275,0.33743831515312195,0.28912633657455444,0.20651881396770477]},{"input":[true,18.0,-2.8306629619916284,false,false,0.0014813942442538548,false,1.4],"torque":-0.15986872290883034,"active":true,"saturated":false,"log":[-0.04865705966949463,-0.08369014412164688,-0.007802085019648075,-0.0,0.40981411933898926,-0.15986871719360352,0.3455796539783478,0.30250686407089233,0.2160763442516327]},{"input":[true,18.0,-2.9049134719024385,false,false,0.0015146477726542137,false,1.4],"torque":-0.17804082176226058,"active":true,"saturated":false,"log":[-0.04251130670309067,-0.07311944663524628,-0.007950874045491219,-0.0,0.4355751872062683,-0.17804081737995148,0.35333701968193054,0.31570467352867126,0.2255033403635025]},{"input":[true,18.0,-2.975406554089701,false,false,0.0015462185149071458,false,1.4],"torque":-0.19625881893496547,"active":true,"saturated":false,"log":[-0.03615724295377731,-0.0621904581785202,-0.008077424950897694,-0.0,0.46104711294174194,-0.1962588131427765,0.36070185899734497,0.32869431376457214,0.2347816526889801]},{"input":[true,18.0,-3.042063890158645,false,false,0.0015760713956580457,false,1.4],"torque":-0.2144744410357306,"active":true,"saturated":false,"log":[-0.02961321920156479,-0.05093473568558693,-0.008181070908904076,-0.0,0.4861646592617035,-0.21447443962097168,0.36766594648361206,0.34145137667655945,0.2438938319683075]},{"input":[true,18.0,-3.104811423260028,false,false,0.0016041732481105837,false,1.4],"torque":-0.23204743012815893,"active":true,"saturated":false,"log":[-0.022897005081176758,-0.039382848888635635,-0.008261210285127163,-0.0,0.5096829533576965,-0.2320474237203598,0.374221533536911,0.3539523780345917,0.2528231143951416]},{"input":[true,18.0,-3.1635794403678914,false,false,0.001630492850875286,false,1.4],"torque":-0.2482804181738912,"active":true,"saturated":false,"log":[-0.016025828197598457,-0.02756442502140999,-0.00831730104982853,-0.0,0.5302425026893616,-0.24828042089939117,0.3803613781929016,0.3661747872829437,0.26155343651771545]},{"input":[true,18.0,-3.218302649731319,false,false,0.0016550009626567592,false,1.4],"torque":-0.2644151028400645,"active":true,"saturated":false,"log":[-0.009016407653689384,-0.015508221462368965,-0.0083488579839468,-0.0,0.5503440499305725,-0.26441511511802673,0.3860786259174347,0.3780970275402069,0.27006930112838745]},{"input":[true,18.0,-3.2689202534141004,false,false,0.0016776703547410072,false,1.4],"torque":-0.2804142988114991,"active":true,"saturated":false,"log":[-0.001884971046820283,-0.003242150181904435,-0.008355455473065376,-0.0,0.569940984249115,-0.28041431307792664,0.3913669288158417,0.3896982967853546,0.2783559262752533]},{"input":[true,18.0,-3.3153760148417484,false,false,0.0016984758412467596,false,1.4],"torque":-0.2962427332786564,"active":true,"saturated":false,"log":[0.00535270432010293,0.009206651709973812,-0.008336720988154411,-0.0,0.5889898538589478,-0.29624274373054504,0.39622044563293457,0.4009588360786438,0.28639915585517883]},{"input":[true,18.0,-3.357618321280787,false,false,0.0017173943071071896,false,1.4],"torque":-0.3118670786022565,"active":true,"saturated":false,"log":[0.012681275606155396,0.021811794489622116,-0.008292336948215961,-0.0,0.6074503660202026,-0.3118670880794525,0.4006337523460388,0.4118596017360687,0.29418542981147766]},{"input":[true,18.0,-3.3956002411809245,false,false,0.0017344047337509473,false,1.4],"torque":-0.32725591614785965,"active":true,"saturated":false,"log":[0.020085809752345085,0.0345475934445858,-0.008222036994993687,-0.0,0.6252853870391846,-0.32725590467453003,0.40460193157196045,0.42238253355026245,0.3017018139362335]},{"input":[true,18.0,-3.429279576316404,false,false,0.0017494882224539628,false,1.4],"torque":-0.3423796645503393,"active":true,"saturated":false,"log":[0.027551770210266113,0.04738904535770416,-0.008125605061650276,-0.0,0.642460823059082,-0.3423796594142914,0.40812060236930847,0.4325103163719177,0.3089359402656555]},{"input":[true,18.0,-3.4586189086685617,false,false,0.0017626280153360862,false,1.4],"torque":-0.3572104642777345,"active":true,"saturated":false,"log":[0.03506498783826828,0.060311779379844666,-0.008002878166735172,-0.0,0.6589453816413879,-0.3572104573249817,0.4111858606338501,0.44222649931907654,0.3158760666847229]},{"input":[true,18.0,-3.4835856419975695,false,false,0.0017738095139792282,false,1.4],"torque":-0.37172206572663274,"active":true,"saturated":false,"log":[0.042611654847860336,0.07329204678535461,-0.007853737100958824,-0.0,0.6747103929519653,-0.3717220723628998,0.41379427909851074,0.45151546597480774,0.32251104712486267]},{"input":[true,18.0,-3.504152038057115,false,false,0.0017830202956463255,false,1.4],"torque":-0.38588970587481064,"active":true,"saturated":false,"log":[0.05017831176519394,0.08630669862031937,-0.00767811294645071,-0.0,0.6897297501564026,-0.3858897089958191,0.4159429669380188,0.46036240458488464,0.3288303017616272]},{"input":[true,18.0,-3.5202952474118256,false,false,0.0017902501270831014,false,1.4],"torque":-0.3996899876630494,"active":true,"saturated":false,"log":[0.05775183439254761,0.09933315217494965,-0.007475981488823891,-0.0,0.7039792537689209,-0.39969000220298767,0.41762953996658325,0.46875330805778503,0.3348237872123718]},{"input":[true,18.0,-3.5319973348231826,false,false,0.001795490975887298,false,1.4],"torque":-0.4131007595061459,"active":true,"saturated":false,"log":[0.06531941145658493,0.11234939098358154,-0.007247363682836294,-0.0,0.7174370288848877,-0.4131007492542267,0.41885215044021606,0.47667497396469116,0.34048211574554443]},{"input":[true,18.0,-3.539245299175714,false,false,0.0017987370194327387,false,1.4],"torque":-0.42610104162836626,"active":true,"saturated":false,"log":[0.07286856323480606,0.12533393502235413,-0.006992323789745569,-0.0,0.7300826907157898,-0.42610102891921997,0.4196093678474426,0.48411494493484497,0.3457963764667511]},{"input":[true,18.0,-3.542031087921341,false,false,0.0017999846513383159,false,1.4],"torque":-0.4386709136202027,"active":true,"saturated":false,"log":[0.08038710057735443,0.13826581835746765,-0.006710968911647797,-0.0,0.7418976426124573,-0.43867090344429016,0.41990041732788086,0.49106162786483765,0.35075831413269043]},{"input":[true,18.0,-3.540351606025828,false,false,0.001799232485474709,false,1.4],"torque":-0.45079145671075094,"active":true,"saturated":false,"log":[0.08786313235759735,0.15112458169460297,-0.006403448060154915,-0.0,0.7528649568557739,-0.45079144835472107,0.4197249412536621,0.49750420451164246,0.35536015033721924]},{"input":[true,18.0,-3.5342087194073923,false,false,0.0017964813575043867,false,1.4],"torque":-0.4624447210415221,"active":true,"saturated":false,"log":[0.0952850878238678,0.16389034688472748,-0.006069950293749571,-0.0,0.7629688382148743,-0.4624447226524353,0.4190831780433655,0.50343257188797,0.3595947027206421]},{"input":[true,18.0,-3.5236092528636482,false,false,0.001791734323953177,false,1.4],"torque":-0.47361363284142366,"active":true,"saturated":false,"log":[0.10264165699481964,0.17654365301132202,-0.005710704252123833,-0.0,0.772195041179657,-0.4736136198043823,0.4179757833480835,0.5088374614715576,0.36345532536506653]},{"input":[true,18.0,-3.5085649824892045,false,false,0.0017849966588144434,false,1.4],"torque":-0.4842820010587539,"active":true,"saturated":false,"log":[0.10992183536291122,0.18906556069850922,-0.005325977690517902,-0.0,0.7805305123329163,-0.484281986951828,0.41640400886535645,0.5137103199958801,0.36693593859672546]},{"input":[true,18.0,-3.4890926225923256,false,false,0.0017762758476896352,false,1.4],"torque":-0.4944344608260226,"active":true,"saturated":false,"log":[0.11711488664150238,0.20143760740756989,-0.004916075617074966,-0.0,0.7879634499549866,-0.4944344460964203,0.4143696427345276,0.5180434584617615,0.3700310289859772]},{"input":[true,18.0,-3.465213807125197,false,false,0.0017655815794717213,false,1.4],"torque":-0.5040564810885101,"active":true,"saturated":false,"log":[0.12421037256717682,0.21364183723926544,-0.0044813393615186214,-0.0,0.7944831848144531,-0.504056453704834,0.4118748605251312,0.5218298435211182,0.3727356195449829]},{"input":[true,18.0,-3.4369550656484287,false,false,0.0017529257355807513,false,1.4],"torque":-0.5131343255145818,"active":true,"saturated":false,"log":[0.13119812309741974,0.2256607711315155,-0.004022146109491587,-0.0,0.8000802397727966,-0.5131343007087708,0.4089225232601166,0.5250632762908936,0.37504521012306213]},{"input":[true,18.0,-3.4043477938564894,false,false,0.0017383223767634996,false,1.4],"torque":-0.5216550621318112,"active":true,"saturated":false,"log":[0.13806825876235962,0.2374774068593979,-0.003538907039910555,-0.0,0.8047462701797485,-0.5216550827026367,0.4055158495903015,0.5277382731437683,0.37695589661598206]},{"input":[true,18.0,-3.3674282186968383,false,false,0.00172178772747186,false,1.4],"torque":-0.5296065498368212,"active":true,"saturated":false,"log":[0.14481118321418762,0.24907523393630981,-0.003032068023458123,0.0,0.808474063873291,-0.5296065211296082,0.40165865421295166,0.5298501253128052,0.37846437096595764]},{"input":[true,18.0,-3.3262373581214817,false,false,0.001703340157837346,false,1.4],"torque":-0.5369773944510556,"active":true,"saturated":false,"log":[0.151417538523674,0.2604381740093231,-0.0025021065957844257,0.0,0.8112574815750122,-0.5369774103164673,0.39735519886016846,0.5313948392868042,0.3795677423477173]},{"input":[true,18.0,-3.280820975515697,false,false,0.001683000163261724,false,1.4],"torque":-0.5437570348195285,"active":true,"saturated":false,"log":[0.1578783094882965,0.2715506851673126,-0.0019495325395837426,0.0,0.8130915760993958,-0.5437570214271545,0.3926102817058563,0.5323691964149475,0.38026368618011475]},{"input":[true,18.0,-3.2312295288545285,false,false,0.0016607903416464523,false,1.4],"torque":-0.5499356362325396,"active":true,"saturated":false,"log":[0.16418470442295074,0.28239768743515015,-0.0013748860219493508,0.0,0.8139723539352417,-0.5499356389045715,0.3874291777610779,0.5327706933021545,0.3805505037307739]},{"input":[true,18.0,-3.1775181146435654,false,false,0.001636735368286227,false,1.4],"torque":-0.5555041902172408,"active":true,"saturated":false,"log":[0.1703282743692398,0.2929646372795105,-0.000778737070504576,0.0,0.8138969540596008,-0.5555042028427124,0.38181763887405396,0.5325976610183716,0.38042688369750977]},{"input":[true,18.0,-3.119746406706272,false,false,0.0016108619684545263,false,1.4],"torque":-0.5604544080673286,"active":true,"saturated":false,"log":[0.1763007938861847,0.3032373785972595,-0.00016168429283425212,0.0,0.8128637671470642,-0.5604544281959534,0.3757818937301636,0.5318489670753479,0.37989211082458496]},{"input":[true,18.0,-3.0579785898858716,false,false,0.001583198887711611,false,1.4],"torque":-0.5647788073137705,"active":true,"saturated":false,"log":[0.18209436535835266,0.3132023215293884,0.00047564596752636135,0.0,0.810871958732605,-0.5647788047790527,0.36932864785194397,0.5305243730545044,0.3789459764957428]},{"input":[true,18.0,-2.992283288735465,false,false,0.0015537768599679727,false,1.4],"torque":-0.5684706560190093,"active":true,"saturated":false,"log":[0.18770135939121246,0.32284635305404663,0.0011326007079333067,0.0,0.8079219460487366,-0.5684706568717957,0.3624650537967682,0.5286242961883545,0.37758877873420715]},{"input":[true,18.0,-2.9227334912755842,false,false,0.0015226285733387025,false,1.4],"torque":-0.5715239937780919,"active":true,"saturated":false,"log":[0.19311444461345673,0.33215683698654175,0.0018085013143718243,0.0,0.8040151596069336,-0.5715239644050598,0.35519880056381226,0.5261498689651489,0.3758213222026825]},{"input":[true,18.0,-2.849406467903896,true,false,0.0014897886338267289,false,1.4],"torque":-0.5735850092906568,"active":true,"saturated":false,"log":[0.19832658767700195,0.34112173318862915,0.0018085013143718243,0.0,0.7991540431976318,-0.573585033416748,0.34753790497779846,0.523102879524231,0.37364494800567627]},{"input":[true,18.0,-2.7723836855471617,true,false,0.001455293526875262,false,1.4],"torque":-0.5749890758522208,"active":true,"saturated":false,"log":[0.20333103835582733,0.34972938895225525,0.0018085013143718243,0.0,0.7933420538902283,-0.5749890804290771,0.3394908607006073,0.5194860100746155,0.37106141448020935]},{"input":[true,18.0,-2.691750717150806,true,false,0.0014191815768321676,false,1.4],"torque":-0.5757328892816161,"active":true,"saturated":false,"log":[0.20812135934829712,0.3579687476158142,0.0018085013143718243,0.0,0.7865837812423706,-0.5757328867912292,0.33106666803359985,0.515302300453186,0.36807310581207275]},{"input":[true,18.0,-2.6075971466066696,true,false,0.0013814929043712983,false,1.4],"torque":-0.5758138768170245,"active":true,"saturated":false,"log":[0.21269138157367706,0.3658291697502136,0.0018085013143718243,0.0,0.7788846492767334,-0.575813889503479,0.32227465510368347,0.5105558633804321,0.36468273401260376]},{"input":[true,18.0,-2.52001646922457,true,false,0.0013422693819180966,false,1.4],"torque":-0.5752302533357241,"active":true,"saturated":false,"log":[0.21703527867794037,0.3733006715774536,0.0018085013143718243,0.0,0.7702510952949524,-0.5752302408218384,0.3131245970726013,0.5052511692047119,0.3608936667442322]},{"input":[true,18.0,-2.4291059878582395,true,false,0.0013015545871289842,false,1.4],"torque":-0.5739809613959242,"active":true,"saturated":false,"log":[0.22114750742912292,0.3803737163543701,0.0018085013143718243,0.0,0.7606906294822693,-0.573980987071991,0.3036266565322876,0.4993934631347656,0.35670962929725647]},{"input":[true,18.0,-2.334966704801058,true,false,0.0012593937544762292,false,1.4],"torque":-0.5708406502780272,"active":true,"saturated":false,"log":[0.2250228375196457,0.38703927397727966,0.0018085013143718243,0.0,0.7477723360061646,-0.5708406567573547,0.29379138350486755,0.4929887652397156,0.3521348237991333]},{"input":[true,18.0,-2.2377032095716713,true,false,0.001215833724992072,false,1.4],"torque":-0.5666474605527649,"active":true,"saturated":false,"log":[0.2286563366651535,0.39328891038894653,0.0018085013143718243,0.0,0.7331736087799072,-0.566647469997406,0.2836296856403351,0.4860435724258423,0.3471739888191223]},{"input":[true,18.0,-2.13742356271417,true,false,0.0011709228942279536,false,1.4],"torque":-0.56201334666027,"active":true,"saturated":false,"log":[0.23204341530799866,0.3991146683692932,0.0018085013143718243,0.0,0.7181207537651062,-0.5620133280754089,0.2731528878211975,0.4785650968551636,0.3418322205543518]},{"input":[true,18.0,-2.034239175741941,true,false,0.0011247111584866602,false,1.4],"torque":-0.5569435683906253,"active":true,"saturated":false,"log":[0.23517975211143494,0.40450918674468994,0.0018085013143718243,0.0,0.7026317715644836,-0.5569435954093933,0.26237261295318604,0.4705612361431122,0.33611515164375305]},{"input":[true,18.0,-1.9282646873585614,true,false,0.0010772498593871215,false,1.4],"torque":0.15248368575235985,"active":true,"saturated":false,"log":[-0.21538576483726501,-0.3704635202884674,0.0018085013143718243,0.0,0.06503063440322876,0.15248368680477142,0.2513008415699005,0.060634512454271317,0.35164690017700195]},{"input":[true,18.0,-1.8196178360932456,true,false,0.0010285917268234453,false,1.4],"torque":0.08609505785838216,"active":true,"saturated":false,"log":[-0.1669899970293045,-0.28722280263900757,0.0018085013143718243,0.0,0.11398287862539291,0.08609505742788315,0.23994988203048706,0.09212501347064972,0.36463361978530884]},{"input":[true,18.0,-1.7084193294923729,true,false,0.0009787908203815724,false,1.4],"torque":0.024465683399728606,"active":true,"saturated":false,"log":[-0.12157690525054932,-0.2091122716665268,0.0018085013143718243,0.0,0.15858814120292664,0.024465683847665787,0.22833232581615448,0.12070857733488083,0.3752119839191437]},{"input":[true,18.0,-1.59479271001242,true,false,0.0009279024692786355,false,1.4],"torque":-0.032607427474047615,"active":true,"saturated":false,"log":[-0.07897479832172394,-0.1358366459608078,0.0018085013143718243,0.0,0.19895531237125397,-0.03260742872953415,0.21646109223365784,0.14655008912086487,0.38351064920425415]},{"input":[true,18.0,-1.4788642177632574,true,false,0.0008759832108917369,false,1.4],"torque":-0.08532278134682159,"active":true,"saturated":false,"log":[-0.03902445361018181,-0.06712205708026886,0.0018085013143718243,0.0,0.23520585894584656,-0.0853227823972702,0.2043493688106537,0.1698036789894104,0.38965103030204773]},{"input":[true,18.0,-1.36076265025436,true,false,0.0008230907279444615,false,1.4],"torque":-0.1354682633969002,"active":true,"saturated":false,"log":[-0.0015781880356371403,-0.0027144835330545902,0.0018085013143718243,0.0,0.2706458866596222,-0.1354682594537735,0.19201061129570007,0.1906135380268097,0.3937477171421051]},{"input":[true,18.0,-1.2406192192997079,false,false,0.0007692837844208935,true,1.4],"torque":-0.1827083161224007,"active":true,"saturated":false,"log":[0.033500999212265015,0.05762171745300293,0.0015640545170754194,0.0,0.30461618304252625,-0.18270832300186157,0.17945852875709534,0.209114670753479,0.3959091305732727]},{"input":[true,18.0,-1.1185674052403884,false,false,0.0007146221602783498,false,1.4],"torque":-0.22608030540449653,"active":true,"saturated":false,"log":[0.06634022295475006,0.11410517990589142,0.0017962453421205282,0.0,0.33426040410995483,-0.2260802984237671,0.16670705378055573,0.22543352842330933,0.39623787999153137]},{"input":[true,18.0,-0.9947428086468251,false,false,0.0006591665850313498,false,1.4],"torque":-0.26559711031789246,"active":true,"saturated":false,"log":[0.09705743193626404,0.16693878173828125,0.0021359464153647423,0.0,0.3597705662250519,-0.2655971050262451,0.15377038717269897,0.2396887093782425,0.39483124017715454]},{"input":[true,18.0,-0.8692829996654388,false,false,0.0006029786702806292,false,1.4],"torque":-0.30144675157795847,"active":true,"saturated":false,"log":[0.12576210498809814,0.21631081402301788,0.0025761136785149574,0.0,0.3813398778438568,-0.3014467656612396,0.14066286385059357,0.25199148058891296,0.3917815685272217]},{"input":[true,18.0,-0.7423273651770748,false,false,0.000546120841262141,false,1.4],"torque":-0.33380943257745127,"active":true,"saturated":false,"log":[0.152555912733078,0.2623961567878723,0.00311005930416286,0.0,0.3991590142250061,-0.33380943536758423,0.12739907205104828,0.26244643330574036,0.3871767222881317]},{"input":[true,18.0,-0.6140169539370393,false,false,0.0004886562674921086,false,1.4],"torque":-0.36285653484108793,"active":true,"saturated":false,"log":[0.17753323912620544,0.3053571581840515,0.003731425618752837,0.0,0.41341328620910645,-0.36285653710365295,0.11399373412132263,0.2711518108844757,0.38110029697418213]},{"input":[true,18.0,-0.4844943198687595,false,false,0.00043064879258516834,false,1.4],"torque":-0.3887501867508187,"active":true,"saturated":false,"log":[0.20078183710575104,0.34534475207328796,0.0044341622851789,0.0,0.42428064346313477,-0.38875019550323486,0.10046175122261047,0.2782002389431,0.3736321032047272]},{"input":[true,18.0,-0.3539033636852092,false,false,0.0003721628633235905,false,1.4],"torque":-0.4116430193449961,"active":true,"saturated":false,"log":[0.22238323092460632,0.38249915838241577,0.005212503485381603,0.0,0.4319306015968323,-0.41164302825927734,0.0868181511759758,0.2836788594722748,0.3648483157157898]},{"input":[true,18.0,-0.22238917301402666,false,false,0.0003132634580563725,false,1.4],"torque":-0.42984036972535705,"active":true,"saturated":false,"log":[0.24241326749324799,0.4169508218765259,0.006060949992388487,0.0,0.43286386132240295,-0.4298403561115265,0.07307810336351395,0.2876700460910797,0.3548218309879303]},{"input":[true,18.0,-0.0900978612039735,false,false,0.00025401601450776096,false,1.4],"torque":-0.43653683815938815,"active":true,"saturated":false,"log":[0.2609425485134125,0.44882118701934814,0.006974248681217432,0.0,0.4134136736392975,-0.43653684854507446,0.05925685912370682,0.2902515232563019,0.34362247586250305]},{"input":[true,18.0,0.042823595008209536,false,false,0.00019448635707539666,false,1.4],"torque":-0.4419782382028055,"active":true,"saturated":false,"log":[0.27803686261177063,0.47822341322898865,0.007947377860546112,0.0,0.3938728868961334,-0.44197824597358704,0.04536977782845497,0.2914968430995941,0.33131730556488037]},{"input":[true,18.0,0.17622751879016224,false,false,0.00013474062369886897,false,1.4],"torque":-0.44623021626915227,"active":true,"saturated":false,"log":[0.29375743865966797,0.5052627921104431,0.008975529111921787,0.0,0.3742716610431671,-0.4462302029132843,0.031432293355464935,0.29147571325302124,0.31797075271606445]},{"input":[true,18.0,0.30996569728407647,false,false,7.484519237992288e-05,false,1.4],"torque":-0.44935482333025617,"active":true,"saturated":false,"log":[0.30816149711608887,0.5300377607345581,0.010054093785583973,0.0,0.3546397387981415,-0.4493548274040222,0.017459888011217117,0.29025423526763916,0.3036448657512665]},{"input":[true,18.0,0.44388954627241184,false,false,1.4866607435947705e-05,false,1.4],"torque":-0.45141059163617725,"active":true,"saturated":false,"log":[0.3213024437427521,0.5526401996612549,0.011178652755916119,0.0,0.3350062370300293,-0.45141059160232544,0.003468083683401346,0.28789523243904114,0.28839942812919617]},{"input":[true,18.0,0.577850275256178,false,false,-4.5128494431304356e-05,false,1.4],"torque":-0.4524527436195594,"active":true,"saturated":false,"log":[0.33323025703430176,0.5731560587882996,0.012344958260655403,0.0,0.3153994083404541,-0.45245274901390076,-0.010527573525905609,0.2844584584236145,0.27229219675064087]},{"input":[true,18.0,0.7116990527624083,false,false,-0.00010507345816964415,false,1.4],"torque":-0.4525332126528427,"active":true,"saturated":false,"log":[0.3439916968345642,0.5916657447814941,0.013548929244279861,0.0,0.2958463132381439,-0.45253321528434753,-0.024511534720659256,0.28000086545944214,0.2553790509700775]},{"input":[true,18.0,0.8452871716971433,false,false,-0.0001649016844307542,false,1.4],"torque":-0.4517007449405652,"active":true,"saturated":false,"log":[0.3536306321620941,0.6082446575164795,0.01478663645684719,0.0,0.2763724625110626,-0.45170074701309204,-0.038468264043331146,0.2745768427848816,0.23771405220031738]},{"input":[true,18.0,0.9784662145602487,false,false,-0.0002245467035626178,false,1.4],"torque":-0.4500008977826669,"active":true,"saturated":false,"log":[0.3621882498264313,0.6229637861251831,0.016054295003414154,0.0,0.2570015490055084,-0.45000091195106506,-0.05238225311040878,0.2682383358478546,0.219349667429924]},{"input":[true,18.0,1.1110882183384723,false,false,-0.00028394224945784677,false,1.4],"torque":-0.44747611799727394,"active":true,"saturated":false,"log":[0.3697033226490021,0.6358897089958191,0.017348257824778557,0.0,0.23775500059127808,-0.4474761188030243,-0.06623804569244385,0.26103511452674866,0.20033679902553558]},{"input":[true,18.0,1.2430058388935739,false,true,-0.0003430223331758737,false,1.4],"torque":-0.44350450741442493,"active":true,"saturated":false,"log":[0.37621238827705383,0.6470853090286255,0.017348257824778557,0.0,0.21865198016166687,-0.4435045123100281,-0.0800202488899231,0.2530149519443512,0.18072494864463806]},{"input":[true,18.0,1.374072514662869,false,true,-0.00040172131625720533,false,1.4],"torque":-0.4387741329194778,"active":true,"saturated":false,"log":[0.3817499279975891,0.6566098928451538,0.017348257824778557,0.0,0.19970925152301788,-0.43877413868904114,-0.09371354430913925,0.24422365427017212,0.16056232154369354]},{"input":[true,18.0,1.5041426294903417,false,true,-0.0004599739836482962,false,1.4],"torque":-0.4333205895423811,"active":true,"saturated":false,"log":[0.3863486349582672,0.6645196676254272,0.017348257824778557,0.0,0.18094146251678467,-0.4333205819129944,-0.10730273276567459,0.2347053736448288,0.13989582657814026]},{"input":[true,18.0,1.6330716744073874,false,true,-0.000517715616156007,false,1.4],"torque":-0.4271770991399633,"active":true,"saturated":false,"log":[0.39003944396972656,0.6708678603172302,0.017348257824778557,0.0,0.1623615324497223,-0.4271771013736725,-0.12077269703149796,0.22450262308120728,0.1187712699174881]},{"input":[true,18.0,1.76071640818347,false,true,-0.0005748820623511629,false,1.4],"torque":-0.42037507210626296,"active":true,"saturated":false,"log":[0.3928518295288086,0.6757051348686218,0.017348257824778557,0.0,0.14398129284381866,-0.4203750789165497,-0.13410848379135132,0.2136564552783966,0.09723332524299622]},{"input":[true,18.0,1.8869350164682996,false,true,-0.0006314098098413157,false,1.4],"torque":-0.4129445128897329,"active":true,"saturated":false,"log":[0.3948138654232025,0.6790798306465149,0.017348257824778557,0.0,0.12581220269203186,-0.4129445254802704,-0.147295281291008,0.2022065371274948,0.07532565295696259]},{"input":[true,18.0,2.0115872693487566,false,true,-0.0006872360558335404,false,1.4],"torque":-0.4049145560425804,"active":true,"saturated":false,"log":[0.395952433347702,0.6810382008552551,0.017348257824778557,0.0,0.10786601155996323,-0.4049145579338074,-0.16031841933727264,0.19019128382205963,0.05309094488620758]},{"input":[true,18.0,2.1345346771454725,false,false,-0.0007422987769088523,false,1.4],"torque":-0.3963137500916231,"active":true,"saturated":false,"log":[0.39629319310188293,0.6816242933273315,0.017348257824778557,0.0,0.09015540778636932,-0.39631375670433044,-0.17316345870494843,0.1776479035615921,0.030570965260267258]},{"input":[true,18.0,2.2556406442760015,false,false,-0.0007965367979307344,false,1.4],"torque":-0.3871705656963335,"active":true,"saturated":false,"log":[0.3958609104156494,0.6808807849884033,0.017348257824778557,0.0,0.07269446551799774,-0.38717055320739746,-0.18581610918045044,0.1646125763654709,0.007806598674505949]},{"input":[true,18.0,2.3747706210136443,false,false,-0.000849889860011222,false,1.4],"torque":-0.377533994776348,"active":true,"saturated":false,"log":[0.39467936754226685,0.6788485050201416,0.017348257824778557,0.0,0.05553979054093361,-0.37753400206565857,-0.1982623040676117,0.15112042427062988,-0.015162110328674316]},{"input":[true,18.0,2.4917922529732985,false,false,-0.0009022986874590205,false,1.4],"torque":-0.3674157247149549,"active":true,"saturated":false,"log":[0.39277157187461853,0.675567090511322,0.017348257824778557,0.0,0.038675062358379364,-0.36741572618484497,-0.2104882299900055,0.13720566034317017,-0.0382959246635437]},{"input":[true,18.0,2.6065755281582703,false,false,-0.0009537050536352881,false,1.4],"torque":-0.3568324167238081,"active":true,"saturated":false,"log":[0.39015981554985046,0.6710748672485352,0.017348257824778557,0.0,0.0220950935035944,-0.3568324148654938,-0.22248031198978424,0.1229015663266182,-0.0615563839673996]},{"input":[true,18.0,2.7189929214046713,false,false,-0.001004051845643908,false,1.4],"torque":-0.3458133123031047,"active":true,"saturated":false,"log":[0.3868657648563385,0.6654090881347656,0.017348257824778557,0.0,0.005820816848427057,-0.3458133041858673,-0.23422521352767944,0.10824067890644073,-0.08490578830242157]},{"input":[true,18.0,2.8289195360629313,false,false,-0.0010532831277843902,false,1.4],"torque":-0.33438793931137684,"active":true,"saturated":false,"log":[0.38291051983833313,0.6586061120033264,0.017348257824778557,0.0,-0.010125278495252132,-0.3343879282474518,-0.2457098811864853,0.0932547077536583,-0.10830715298652649]},{"input":[true,18.0,2.936233242759002,false,false,-0.001101344203696894,false,1.4],"torque":-0.32258585822132657,"active":true,"saturated":false,"log":[0.378314733505249,0.6507013440132141,0.017348257824778557,0.0,-0.025719866156578064,-0.3225858509540558,-0.25692155957221985,0.07797467708587646,-0.13172423839569092]},{"input":[true,18.0,1.5935151642791718,false,false,-0.0005,false,1.4],"torque":-0.22151272000275102,"active":true,"saturated":false,"log":[0.21892023086547852,0.3765428066253662,0.018114477396011353,-0.0,0.046416666358709335,-0.22151271998882294,-0.11664000153541565,0.07715516537427902,-0.1446041464805603]},{"input":[true,18.0,1.5910351262412323,false,false,-0.0004988893003505612,false,1.4],"torque":-0.22020147157200506,"active":true,"saturated":false,"log":[0.21746888756752014,0.3740464746952057,0.018875619396567345,-0.0,0.04554092511534691,-0.22020147740840912,-0.11638089269399643,0.0761294811964035,-0.1574094444513321]},{"input":[true,18.0,1.583606030436931,false,false,-0.0004955621360170897,false,1.4],"torque":-0.21812615108876307,"active":true,"saturated":false,"log":[0.2152540236711502,0.37023693323135376,0.019629009068012238,-0.0,0.04446478188037872,-0.21812614798545837,-0.11560473591089249,0.07494497299194336,-0.1700928807258606]},{"input":[true,18.0,1.57126088284269,false,false,-0.0004900332889206208,false,1.4],"torque":-0.21500349524577045,"active":true,"saturated":false,"log":[0.21232926845550537,0.3652063310146332,0.020372159779071808,-0.0,0.04253445193171501,-0.21500349044799805,-0.114314965903759,0.07364566624164581,-0.18260987102985382]},{"input":[true,18.0,1.5540545304629303,false,false,-0.00048232732261528193,false,1.4],"torque":-0.20993473483644146,"active":true,"saturated":false,"log":[0.20874714851379395,0.3590450882911682,0.021102774888277054,-0.0,0.03787225857377052,-0.20993474125862122,-0.11251731961965561,0.07227230817079544,-0.1949184387922287]},{"input":[true,18.0,1.5320634176558765,false,false,-0.00047247847315736883,false,1.4],"torque":-0.2043299559109498,"active":true,"saturated":false,"log":[0.20455901324748993,0.3518415093421936,0.021818732842803,-0.0,0.033199749886989594,-0.2043299525976181,-0.11021977663040161,0.07086237519979477,-0.20697921514511108]},{"input":[true,18.0,1.5053852465066715,false,false,-0.00046053049700144256,false,1.4],"torque":-0.1982551683442038,"active":true,"saturated":false,"log":[0.19981509447097778,0.3436819612979889,0.022518085315823555,-0.0,0.02856391854584217,-0.19825516641139984,-0.10743255168199539,0.06945013254880905,-0.21875543892383575]},{"input":[true,18.0,1.4741385427566873,false,false,-0.00044653647659921467,false,1.4],"torque":-0.19177346556674152,"active":true,"saturated":false,"log":[0.19456449151039124,0.3346509337425232,0.023199060931801796,-0.0,0.024007707834243774,-0.1917734593153,-0.10416802763938904,0.06806664913892746,-0.2302129864692688]},{"input":[true,18.0,1.438462129217529,false,false,-0.00043055858456490516,false,1.4],"torque":-0.18494517459044144,"active":true,"saturated":false,"log":[0.18885508179664612,0.3248307406902313,0.023860054090619087,-0.0,0.019570501521229744,-0.18494518101215363,-0.10044070333242416,0.06673982739448547,-0.24132026731967926]},{"input":[true,18.0,1.3985145090092412,false,false,-0.0004126678074548392,false,1.4],"torque":-0.17782810488731565,"active":true,"saturated":false,"log":[0.18273361027240753,0.31430181860923767,0.024499621242284775,-0.0,0.015288446098566055,-0.1778281033039093,-0.09626714140176773,0.06549446284770966,-0.2520482838153839]},{"input":[true,18.0,1.354473161362869,false,false,-0.00039294363038847404,false,1.4],"torque":-0.17047751402499434,"active":true,"saturated":false,"log":[0.1762455254793167,0.3031423091888428,0.025116480886936188,-0.0,0.011194683611392975,-0.1704775094985962,-0.09166588634252548,0.06435225903987885,-0.26237061619758606]},{"input":[true,18.0,1.3065337531159789,false,false,-0.00037147368391202205,false,1.4],"torque":-0.16294617713316992,"active":true,"saturated":false,"log":[0.16943500936031342,0.29142820835113525,0.025709502398967743,-0.0,0.007319434080272913,-0.1629461795091629,-0.08665738254785538,0.06333188712596893,-0.27226337790489197]},{"input":[true,18.0,1.2549092694043076,false,false,-0.0003483533546735827,false,1.4],"torque":-0.15528433653604437,"active":true,"saturated":false,"log":[0.16234493255615234,0.27923327684402466,0.026277709752321243,-0.0,0.003689981298521161,-0.1552843302488327,-0.08126387000083923,0.06244904175400734,-0.2817051410675049]},{"input":[true,18.0,1.1998290674116991,false,false,-0.0003236853616394762,false,1.4],"torque":-0.14753960919826734,"active":true,"saturated":false,"log":[0.15501680970191956,0.2666289210319519,0.026820268481969833,-0.0,0.0003305674181319773,-0.14753961563110352,-0.07550931721925735,0.06171649321913719,-0.2906770408153534]},{"input":[true,18.0,1.14153785738233,false,false,-0.0002975792997345639,false,1.4],"torque":-0.13975681602430803,"active":true,"saturated":false,"log":[0.1474906951189041,0.25368398427963257,0.027336485683918,-0.0,-0.002737760543823242,-0.13975681364536285,-0.06941929459571838,0.061144158244132996,-0.2991625666618347]},{"input":[true,18.0,1.0802946154223803,false,false,-0.00027015115293406986,false,1.4],"torque":-0.13197790313876115,"active":true,"saturated":false,"log":[0.13980519771575928,0.2404649406671524,0.02782580442726612,-0.0,-0.005497328471392393,-0.1319779008626938,-0.06302086263895035,0.060739148408174515,-0.30714765191078186]},{"input":[true,18.0,1.016371432921352,false,false,-0.00024152277897013832,false,1.4],"torque":-0.12424177532736369,"active":true,"saturated":false,"log":[0.13199739158153534,0.2270355075597763,0.028287796303629875,-0.0,-0.007934008724987507,-0.12424177676439285,-0.056342434138059616,0.06050586700439453,-0.3146206736564636]},{"input":[true,18.0,0.9500523077048246,false,false,-0.0002118213679424723,false,1.4],"torque":-0.11658422595620943,"active":true,"saturated":false,"log":[0.12410282343626022,0.21345685422420502,0.028722155839204788,-0.0,-0.010037366300821304,-0.11658422648906708,-0.049413688480854034,0.060446083545684814,-0.3215721845626831]},{"input":[true,18.0,0.8816318822893141,false,false,-0.00018117887723833681,false,1.4],"torque":-0.10903775129017418,"active":true,"saturated":false,"log":[0.11615536361932755,0.1997872292995453,0.029128698632121086,-0.0,-0.011800765059888363,-0.10903774946928024,-0.042265407741069794,0.060559019446372986,-0.32799509167671204]},{"input":[true,18.0,0.8114141348449146,false,false,-0.00014973144527246395,true,1.4],"torque":-0.10163154947186083,"active":true,"saturated":false,"log":[0.1081872433423996,0.18608206510543823,0.029507353901863098,-0.0,-0.013221431523561478,-0.10163155198097229,-0.03492935001850128,0.06084144115447998,-0.33388444781303406]},{"input":[true,18.0,0.7397110286815097,false,false,-0.00011761878665149471,false,1.4],"torque":-0.09439144139044256,"active":true,"saturated":false,"log":[0.10022897273302078,0.1723938286304474,0.02985815517604351,-0.0,-0.014300462789833546,-0.09439144283533096,-0.02743810974061489,0.0612877681851387,-0.3392375409603119]},{"input":[true,18.0,0.6668411262586169,false,false,-8.498357145012052e-05,false,1.4],"torque":-0.08733986096067399,"active":true,"saturated":false,"log":[0.09230931103229523,0.15877202153205872,0.030181238427758217,-0.0,-0.015042788349092007,-0.0873398631811142,-0.01982496678829193,0.06189017370343208,-0.34405359625816345]},{"input":[true,18.0,0.5931281738765389,false,false,-5.197079135667549e-05,false,1.4],"torque":-0.08049585501550166,"active":true,"saturated":false,"log":[0.0844552218914032,0.14526298642158508,0.030476832762360573,-0.0,-0.015457089059054852,-0.08049585670232773,-0.012123744934797287,0.06263870745897293,-0.3483338952064514]},{"input":[true,18.0,0.5188996633367553,false,false,-1.872711550426666e-05,false,1.4],"torque":-0.0738751224813481,"active":true,"saturated":false,"log":[0.07669185101985931,0.13190998136997223,0.0307452529668808,-0.0,-0.015555673278868198,-0.07387512177228928,-0.004368660040199757,0.06352140754461288,-0.35208168625831604]},{"input":[true,18.0,0.44448537696180607,false,false,1.4599761150644409e-05,false,1.4],"torque":-0.06749003200099227,"active":true,"saturated":false,"log":[0.06904245913028717,0.11875303089618683,0.030986901372671127,-0.0,-0.015354319475591183,-0.06749003380537033,0.0034058336168527603,0.06452441960573196,-0.35530194640159607]},{"input":[true,18.0,0.3702159224388505,false,false,4.786177400718783e-05,false,1.4],"torque":-0.061349734397666875,"active":true,"saturated":false,"log":[0.06152844801545143,0.10582893341779709,0.031202251091599464,-0.0,-0.014872090891003609,-0.06134973466396332,0.0111651960760355,0.06563213467597961,-0.3580014705657959]},{"input":[true,18.0,0.2964212639962993,false,false,8.091114664128603e-05,false,1.4],"torque":-0.05546021593494854,"active":true,"saturated":false,"log":[0.05416930466890335,0.09317120164632797,0.031391844153404236,-0.0,-0.01413112785667181,-0.055460214614868164,0.01887495443224907,0.06682734936475754,-0.36018866300582886]},{"input":[true,18.0,0.22342925643920286,false,false,0.00011360104734654355,false,1.4],"torque":-0.049824409136955,"active":true,"saturated":false,"log":[0.046982601284980774,0.0808100774884224,0.03155628219246864,-0.0,-0.01315641775727272,-0.04982440918684006,0.026500854641199112,0.06809134781360626,-0.3618735373020172]},{"input":[true,18.0,0.15156418855638323,false,false,0.0001457862414779463,false,1.4],"torque":-0.04444230112184051,"active":true,"saturated":false,"log":[0.03998398408293724,0.06877245008945465,0.03169622644782066,-0.0,-0.01197555661201477,-0.044442299753427505,0.034009017050266266,0.06940411031246185,-0.36306747794151306]},{"input":[true,18.0,0.08114534237065779,false,false,0.0001773237367001388,false,1.4],"torque":-0.039311050507838785,"active":true,"saturated":false,"log":[0.03318716585636139,0.0570819266140461,0.031812380999326706,-0.0,-0.010618493892252445,-0.03931105136871338,0.041366081684827805,0.07074441760778427,-0.3637832999229431]},{"input":[true,18.0,0.012485574633115948,false,false,0.0002080734182735712,false,1.4],"torque":-0.03442512746271336,"active":true,"saturated":false,"log":[0.02660394087433815,0.04575878009200096,0.03190549463033676,-0.0,-0.00911727361381054,-0.03442512825131416,0.04853936657309532,0.0720900222659111,-0.364035040140152]},{"input":[true,18.0,-0.054110073136415915,false,false,0.0002378986715560677,false,1.4],"torque":-0.029776444974717056,"active":true,"saturated":false,"log":[0.020244186744093895,0.03482000157237053,0.03197634965181351,-0.0,-0.007505771704018116,-0.029776444658637047,0.055497001856565475,0.07341780513525009,-0.3638378381729126]},{"input":[true,18.0,-0.11834572988740993,false,false,0.00026666698895416686,false,1.4],"torque":-0.025354508405970446,"active":true,"saturated":false,"log":[0.014115885831415653,0.02427932433784008,0.03202575817704201,-0.0,-0.0058194235898554325,-0.025354508310556412,0.06220807507634163,0.07470390945672989,-0.36320793628692627]},{"input":[true,18.0,-0.17993600953412786,false,false,0.0002942505586276729,false,1.4],"torque":-0.02114656305764999,"active":true,"saturated":false,"log":[0.008225143887102604,0.014147247187793255,0.03205454349517822,-0.0,-0.004094960168004036,-0.021146563813090324,0.06864277273416519,0.07592392712831497,-0.36216241121292114]},{"input":[true,18.0,-0.23860727886851912,false,false,0.0003205268323319191,false,1.4],"torque":-0.01713775747644919,"active":true,"saturated":false,"log":[0.0025762272998690605,0.004431110806763172,0.0320635624229908,-0.0,-0.002370138419792056,-0.017137756571173668,0.07477249950170517,0.07705306261777878,-0.3607192039489746]},{"input":[true,18.0,-0.29409887325745715,false,false,0.00034537906987493823,false,1.4],"torque":-0.013311302922134666,"active":true,"saturated":false,"log":[-0.0028284029103815556,-0.004864852875471115,0.03205366060137749,-0.0,-0.0006834756932221353,-0.013311303220689297,0.0805700272321701,0.07806623727083206,-0.3588969111442566]},{"input":[true,18.0,-0.3461642547232026,false,false,0.0003686968577706227,false,1.4],"torque":-0.0096486385246455,"active":true,"saturated":false,"log":[-0.00798804871737957,-0.013739443384110928,0.03202570229768753,-0.0,0.0009260112419724464,-0.009648638777434826,0.08600960671901703,0.078938327729702,-0.3567146956920624]},{"input":[true,18.0,-0.3945721072619888,false,false,0.0003903765997836073,false,1.4],"torque":-0.006129604543543661,"active":true,"saturated":false,"log":[-0.012903721071779728,-0.022194400429725647,0.03198054060339928,-0.0,0.0024190652184188366,-0.006129604764282703,0.09106705337762833,0.07964427024126053,-0.35419222712516785]},{"input":[true,18.0,-0.43910736453443516,false,false,0.0004103219771864765,false,1.4],"torque":-0.0027326117497034606,"active":true,"saturated":false,"log":[-0.017578087747097015,-0.030234310775995255,0.03191901743412018,-0.0,0.0037564418744295835,-0.0027326117269694805,0.09571991115808487,0.08015923202037811,-0.35134944319725037]},{"input":[true,18.0,-0.47957216536198205,false,false,0.00042844437668447364,false,1.4],"torque":0.0005651799824064672,"active":true,"saturated":false,"log":[-0.02201540768146515,-0.0378665030002594,0.03184196352958679,-0.0,0.004899157211184502,0.0005651799729093909,0.0999475046992302,0.08045876771211624,-0.3482065498828888]},{"input":[true,18.0,-0.5157867327842534,false,false,0.0004446632841065207,false,1.4],"torque":0.003787682431063564,"active":true,"saturated":false,"log":[-0.026221461594104767,-0.04510091245174408,0.031750187277793884,-0.0,0.005808732006698847,0.003787682391703129,0.10373105108737946,0.08051898330450058,-0.3447839021682739]},{"input":[true,18.0,-0.5475901727718879,false,false,0.0004589066421134661,false,1.4],"torque":0.006959698766747262,"active":true,"saturated":false,"log":[-0.030203478410840034,-0.05194998160004616,0.03164447471499443,-0.0,0.006447426974773407,0.006959698628634214,0.107053741812706,0.08031666278839111,-0.34110182523727417]},{"input":[true,18.0,-0.5748411890463041,false,false,0.000471111170334329,false,1.4],"torque":0.010106742660526958,"active":true,"saturated":false,"log":[-0.03397005796432495,-0.05842849984765053,0.03152558207511902,-0.0,0.0067784772254526615,0.01010674238204956,0.1099008172750473,0.07982943952083588,-0.33718055486679077]},{"input":[true,18.0,-0.5974187108306115,false,false,0.00048122264650824387,false,1.4],"torque":0.013254849461145132,"active":true,"saturated":false,"log":[-0.03753108158707619,-0.06455346196889877,0.031394220888614655,-0.0,0.006766316946595907,0.01325484924018383,0.11225961893796921,0.07903590798377991,-0.3330400884151459]},{"input":[true,18.0,-0.615222430742685,false,false,0.0004891961473830436,false,1.4],"torque":0.016430389860929154,"active":true,"saturated":false,"log":[-0.04089762270450592,-0.07034391164779663,0.031251080334186554,-0.0,0.006376802455633879,0.016430389136075974,0.11411967873573303,0.07791580259799957,-0.3287001848220825]},{"input":[true,18.0,-0.6281732504406652,false,false,0.0004949962483002228,false,1.4],"torque":0.01965988120829436,"active":true,"saturated":false,"log":[-0.044081851840019226,-0.07582078874111176,0.031096793711185455,-0.0,0.005577427800744772,0.01965988054871559,0.11547272652387619,0.07645006477832794,-0.32418012619018555]},{"input":[true,18.0,-0.6362136320409575,false,false,0.0004985971805795595,false,1.4],"torque":0.022969807802976798,"active":true,"saturated":false,"log":[-0.04709694907069206,-0.08100675046443939,0.030931955203413963,-0.0,0.004337534308433533,0.0229698084294796,0.11631274968385696,0.0746210366487503,-0.319498747587204]},{"input":[true,18.0,-0.6393078537474626,false,false,0.0004999829460041724,false,1.4],"torque":0.026386410039517905,"active":true,"saturated":false,"log":[-0.04995697736740112,-0.08592600375413895,0.030757104977965355,-0.0,0.002628517569974065,0.026386409997940063,0.11663602292537689,0.07241251319646835,-0.3146742284297943]},{"input":[true,18.0,-0.6374421685563227,false,false,0.0004991473878973766,false,1.4],"torque":0.02993551839017734,"active":true,"saturated":false,"log":[-0.05267680436372757,-0.09060410410165787,0.030572736635804176,-0.0,0.0004240251728333533,0.02993551827967167,0.11644110083580017,0.0698099210858345,-0.3097241222858429]},{"input":[true,18.0,-0.6306248653310882,false,false,0.000496094218475557,false,1.4],"torque":0.03364235213375112,"active":true,"saturated":false,"log":[-0.05527197942137718,-0.09506780654191971,0.0303792841732502,-0.0,-0.002299851505085826,0.033642351627349854,0.11572886258363724,0.06680034101009369,-0.3046652376651764]},{"input":[true,18.0,-0.6188862319769681,false,false,0.0004908370023555395,false,1.4],"torque":0.037531331712407884,"active":true,"saturated":false,"log":[-0.057758621871471405,-0.09934482723474503,0.030177129432559013,-0.0,-0.005564393475651741,0.037531331181526184,0.11450245976448059,0.06337268650531769,-0.29951345920562744]},{"input":[true,18.0,-0.602278420877769,false,false,0.00048339909628973055,false,1.4],"torque":0.04162590548974894,"active":true,"saturated":false,"log":[-0.06015332043170929,-0.10346370935440063,0.02996659278869629,-0.0,-0.009388080798089504,0.04162590578198433,0.11276734620332718,0.05951771140098572,-0.2942838668823242]},{"input":[true,18.0,-0.5808752171933529,false,false,0.00047381354539676926,false,1.4],"torque":0.04594835313433784,"active":true,"saturated":false,"log":[-0.06247299909591675,-0.10745355486869812,0.029747936874628067,-0.0,-0.013786430470645428,0.04594835266470909,0.11053122580051422,0.055228136479854584,-0.28899049758911133]},{"input":[true,18.0,-0.5547717110470369,false,false,0.0004621229363487218,false,1.4],"torque":0.05051963751963168,"active":true,"saturated":false,"log":[-0.06473483145236969,-0.11134391278028488,0.029521364718675613,-0.0,-0.01877184957265854,0.050519637763500214,0.10780403763055801,0.050498705357313156,-0.28364643454551697]},{"input":[true,18.0,-0.5240838750593246,false,false,0.00044837920816707353,false,1.4],"torque":0.05535921279371826,"active":true,"saturated":false,"log":[-0.06695609539747238,-0.11516448110342026,0.02928701974451542,-0.0,-0.024353496730327606,0.055359214544296265,0.10459790378808975,0.04532623291015625,-0.2782635986804962]},{"input":[true,18.0,-0.48894804910491,false,false,0.00043264342146811797,false,1.4],"torque":0.060484889556517325,"active":true,"saturated":false,"log":[-0.06915408372879028,-0.11894502490758896,0.02904498018324375,0.0,-0.030537178739905357,0.06048488989472389,0.10092706233263016,0.0397096611559391,-0.2728528678417206]},{"input":[true,18.0,-0.44952033458207435,false,false,0.00041498548718293574,false,1.4],"torque":0.06591267765189081,"active":true,"saturated":false,"log":[-0.0713459774851799,-0.12271507829427719,0.0287952683866024,0.0,-0.037325240671634674,0.06591267883777618,0.0968078151345253,0.03365008533000946,-0.26742392778396606]},{"input":[true,18.0,-0.40597590088561303,false,false,0.00039548385595720845,false,1.4],"torque":0.07165664608933404,"active":true,"saturated":false,"log":[-0.07354872673749924,-0.1265038102865219,0.028537848964333534,0.0,-0.04471650719642639,0.07165664434432983,0.09225847572088242,0.027150796726346016,-0.26198524236679077]},{"input":[true,18.0,-0.3585082071644918,false,false,0.00037422516961079937,false,1.4],"torque":0.07772882659026653,"active":true,"saturated":false,"log":[-0.07577896863222122,-0.1303398311138153,0.02827262133359909,0.0,-0.05270622298121452,0.07772883027791977,0.08729925006628036,0.020217295736074448,-0.2565440833568573]},{"input":[true,18.0,-0.30732814282181414,false,false,0.0003513038762055989,false,1.4],"torque":0.08413905807531818,"active":true,"saturated":false,"log":[-0.07805286347866058,-0.1342509239912033,0.027999436482787132,0.0,-0.06128603219985962,0.08413905650377274,0.08195216953754425,0.012857288122177124,-0.25110653042793274]},{"input":[true,18.0,-0.2526630905756743,false,false,0.00032682181043180595,false,1.4],"torque":0.090894959117018,"active":true,"saturated":false,"log":[-0.08038606494665146,-0.1382640302181244,0.027718085795640945,0.0,-0.07044397294521332,0.09089495986700058,0.07624099403619766,0.005080690141767263,-0.24567733705043793]},{"input":[true,18.0,-0.19475591624353833,false,false,0.0003008877411769109,false,1.4],"torque":0.09801668506831597,"active":true,"saturated":false,"log":[-0.08279354125261307,-0.14240488409996033,0.027428308501839638,0.0,-0.08019412308931351,0.09801668673753738,0.07019109278917313,-0.003100387053564191,-0.2402600795030594]},{"input":[true,18.0,-0.1338638897383278,false,false,0.00027361688828743294,false,1.4],"torque":0.10552026548172701,"active":true,"saturated":false,"log":[-0.08528953790664673,-0.146697998046875,0.027129795402288437,0.0,-0.09054353833198547,0.10552026331424713,0.06382934749126434,-0.01167166419327259,-0.23485706746578217]},{"input":[true,18.0,-0.07025754207001611,false,false,0.0002451304106703497,false,1.4],"torque":0.11338306591102187,"active":true,"saturated":false,"log":[-0.08788742870092392,-0.1511663794517517,0.02682218886911869,0.0,-0.10142385959625244,0.11338306963443756,0.057184021919965744,-0.02061672881245613,-0.22946937382221222]},{"input":[true,18.0,-0.004219463430831627,false,false,0.00021555486800847306,false,1.4],"torque":0.12160389802652546,"active":true,"saturated":false,"log":[-0.0905996784567833,-0.155831441283226,0.026505090296268463,0.0,-0.11281087249517441,0.12160389870405197,0.05028464272618294,-0.029917078092694283,-0.22409687936306]},{"input":[false,18.0,0.06395695229604892,false,false,0.00018502165848126905,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.13396881102643524,false,false,0.0001536664349892097,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.2055050641725319,false,false,0.00012162850247525857,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.27824789056917354,false,false,8.905019902106331e-05,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.35187410848924844,false,false,5.6076263467527434e-05,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.4260566114750243,false,false,2.2853192369290225e-05,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.5004658216062559,false,false,-1.0471410859959058e-05,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.5747711537485132,false,false,-4.37494917197232e-05,true,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.6486424842763487,false,false,-7.683320239902244e-05,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.7217516177460801,false,false,-0.00010957555863444268,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.793773745002043,false,false,-0.00014183109273161314,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.864388886238258,false,false,-0.00017345649984887746,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.9332833126042621,false,false,-0.00020431127467184204,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.0001509400392006,false,false,-0.00023425833565018857,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.0646946891416718,false,false,-0.00026316463402339594,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,18.0,1.1266278050336689,false,false,-0.00029090174492958534,false,1.4],"torque":-0.0019330168494814677,"active":true,"saturated":false,"log":[0.05383547395467758,0.09259701520204544,0.0,0.0,-0.08874800056219101,-0.0019330168142914772,-0.06786155700683594,-0.020204681903123856,-0.003987892065197229]},{"input":[true,18.0,1.1856751313547538,false,false,-0.00031734643797131734,false,1.4],"torque":0.03151480672121062,"active":true,"saturated":false,"log":[0.03385905921459198,0.05823758244514465,0.00011850670853164047,0.0,-0.12110820412635803,0.03151480853557587,-0.07403057813644409,-0.0440574586391449,-0.007152613252401352]},{"input":[true,18.0,1.2415743327263438,false,false,-0.00034238122470342736,false,1.4],"torque":0.06347797568276892,"active":true,"saturated":false,"log":[0.014797583222389221,0.025451842695474625,0.0001702982553979382,0.0,-0.1520189642906189,0.06347797811031342,-0.07987069338560104,-0.06677139550447464,-0.00953089538961649]},{"input":[true,18.0,1.2940770602549705,false,false,-0.0003658948806105272,false,1.4],"torque":0.09402587279386257,"active":true,"saturated":false,"log":[-0.003406801726669073,-0.005859699100255966,0.00015837444516364485,0.0,-0.18152189254760742,0.0940258726477623,-0.0853559598326683,-0.08837176859378815,-0.011156166903674603]},{"input":[true,18.0,1.3429500548963915,false,false,-0.00038778293925512483,false,1.4],"torque":0.12321840149279453,"active":true,"saturated":false,"log":[-0.02080555446445942,-0.035785552114248276,8.555500244256109e-05,0.0,-0.20965059101581573,0.12321840226650238,-0.09046199917793274,-0.10887974500656128,-0.012058895081281662]},{"input":[true,18.0,1.3879761837785463,false,false,-0.0004079481564009665,false,1.4],"torque":0.15110608151105606,"active":true,"saturated":false,"log":[-0.03744415193796158,-0.0644039437174797,-4.549953155219555e-05,0.0,-0.2364300787448883,0.1511060744524002,-0.0951661467552185,-0.12831290066242218,-0.012266903184354305]},{"input":[true,18.0,1.4289554048791737,false,false,-0.00042630094204959286,false,1.4],"torque":0.1777302984425586,"active":true,"saturated":false,"log":[-0.05336248129606247,-0.09178347140550613,-0.00023226821213029325,0.0,-0.2618767023086548,0.17773029208183289,-0.09944748133420944,-0.1466856449842453,-0.011805674061179161]},{"input":[true,18.0,1.4657056557722314,false,false,-0.00044275975847065947,false,1.4],"torque":0.20312377886602134,"active":true,"saturated":false,"log":[-0.06859521567821503,-0.11798377335071564,-0.00047235147212632,0.0,-0.2859984338283539,0.2031237781047821,-0.1032869964838028,-0.16400966048240662,-0.01069863885641098]},{"input":[true,18.0,1.4980636624945847,false,false,-0.00045725148245765016,false,1.4],"torque":0.22731132411680324,"active":true,"saturated":false,"log":[-0.08317218720912933,-0.14305616915225983,-0.0007634541252627969,0.0,-0.308795690536499,0.22731132805347443,-0.10666762292385101,-0.18029429018497467,-0.008967451751232147]},{"input":[true,18.0,1.525885664939341,false,false,-0.00046971173019955657,false,1.4],"torque":0.25031073023634637,"active":true,"saturated":false,"log":[-0.09711871296167374,-0.16704419255256653,-0.0011033696355298162,0.0,-0.33026257157325745,0.2503107190132141,-0.1095743477344513,-0.1955469250679016,-0.006632243748754263]},{"input":[true,18.0,1.5490480555530524,false,false,-0.000480085143325183,false,1.4],"torque":0.27213388510104236,"active":true,"saturated":false,"log":[-0.1104559525847435,-0.18998423218727112,-0.0014899654779583216,0.0,-0.3503885567188263,0.27213388681411743,-0.11199425905942917,-0.20977339148521423,-0.0037118655163794756]},{"input":[true,18.0,1.5674479284991791,false,false,-0.0004883256348492412,false,1.4],"torque":0.2927878804203019,"active":true,"saturated":false,"log":[-0.12320122122764587,-0.21190610527992249,-0.0019211696926504374,0.0,-0.36916011571884155,0.2927878797054291,-0.11391660571098328,-0.22297824919223785,-0.00022411915415432304]},{"input":[true,18.0,1.5810035368479978,false,false,-0.0004943965939275431,false,1.4],"torque":0.3122901418455085,"active":true,"saturated":false,"log":[-0.13536830246448517,-0.23283347487449646,-0.0023949588648974895,0.0,-0.38659027218818665,0.31229013204574585,-0.11533283442258835,-0.23516516387462616,0.0038140302058309317]},{"input":[true,18.0,1.5896546557617444,false,false,-0.0004982710485116088,false,1.4],"torque":0.33062941446255345,"active":true,"saturated":false,"log":[-0.14696772396564484,-0.25278449058532715,-0.0029093457851558924,0.0,-0.4026404917240143,0.3306294083595276,-0.11623667180538177,-0.2463371753692627,0.008386251516640186]},{"input":[true,18.0,1.5933628500614396,false,false,-0.0004999317851800449,false,1.4],"torque":0.3478030440118411,"active":true,"saturated":false,"log":[-0.15800708532333374,-0.271772176027298,-0.003462370717898011,0.0,-0.4172944724559784,0.3478030562400818,-0.11662408709526062,-0.25649699568748474,0.013476659543812275]},{"input":[true,18.0,1.5921116449866486,false,false,-0.0004993714256143098,false,1.4],"torque":0.3638082356851894,"active":true,"saturated":false,"log":[-0.16849134862422943,-0.2898051142692566,-0.004052090458571911,0.0,-0.4305399954319,0.3638082444667816,-0.11649336665868759,-0.2656472623348236,0.019069641828536987]},{"input":[true,18.0,1.5859065993895234,false,false,-0.0004965924593790963,false,1.4],"torque":0.37864173209506863,"active":true,"saturated":false,"log":[-0.17842306196689606,-0.3068876564502716,-0.004676571115851402,0.0,-0.44236817955970764,0.3786417245864868,-0.11584508419036865,-0.2737908661365509,0.025149695575237274]},{"input":[true,18.0,1.574775281037942,false,false,-0.0004916072328616975,false,1.4],"torque":0.39230023557341265,"active":true,"saturated":false,"log":[-0.18780264258384705,-0.3230205476284027,-0.005333880428224802,0.0,-0.45277366042137146,0.39230024814605713,-0.11468213051557541,-0.2809309959411621,0.03170127794146538]},{"input":[true,18.0,1.5587671441374678,false,false,-0.0004844378944194944,false,1.4],"torque":0.40478069921900367,"active":true,"saturated":false,"log":[-0.1966286301612854,-0.33820125460624695,-0.006022080313414335,0.0,-0.46175482869148254,0.40478068590164185,-0.11300966888666153,-0.2870716154575348,0.03870866447687149]},{"input":[true,18.0,1.5379533096162747,false,false,-0.0004751162959792648,false,1.4],"torque":0.41608058274309045,"active":true,"saturated":false,"log":[-0.20489798486232758,-0.3524245321750641,-0.006739223375916481,0.0,-0.4693138301372528,0.4160805940628052,-0.11083512753248215,-0.29221734404563904,0.046155836433172226]}]}],"corrections":{"pull_request_heads":{"180":"e4fdb7cbbeb616ea9a7c293df0d56911b80a2ce7"},"behavior":"Magnitude-based unwind.","original_output_sha256":"f1bd257259d332c3ee7d26de19db6112ad2d70016e5d248965d0a27798fe58db"}} diff --git a/openpilot/starpilot/lateral/tests/fixtures/genesis_g70_ba901b5f.json b/openpilot/starpilot/lateral/tests/fixtures/genesis_g70_ba901b5f.json new file mode 100644 index 0000000000..50440b88b2 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/fixtures/genesis_g70_ba901b5f.json @@ -0,0 +1 @@ +{"source_commit":"ba901b5f551984c331db3dadab9b70af75597d35","source_sha256":{"selfdrive/controls/lib/latcontrol_torque.py":"3561d5ac43a72f9b68bc9e44042b8f5cbdca406dec940e7648c64607862a60f7","selfdrive/controls/lib/latcontrol_vehicle_tunes.py":"f7096ba7208c9d49e410a904909d0ccb1846494b9dc6f413293c6c8096113321","common/pid.py":"67ae4b84e38a219a8997654c16832c2863d56c9458832678dee76497ba36145a","opendbc/car/lateral.py":"8c9fb186ed9a43487478a76e0d1ec2b311d76d44c09e99889a0950cd5f6a2528"},"oracle_boundary":"Frozen original update + PID + friction + default family helpers; current matched CP, VM and torque conversion callbacks; no FLM, trailer or deprecated deadzone boost.","car_params":{"carFingerprint":"GENESIS_G70_2020","mass":1776.0,"wheelbase":2.8399999141693115,"centerToFront":1.1359999179840088,"steerRatio":13.5600004196167,"tireStiffnessFront":233418.890625,"tireStiffnessRear":245991.78125,"minSteerSpeed":0.0,"lateralTuning":{"torque":{"deprecated":{"useSteeringAngle":false,"kp":0.0,"ki":0.0,"kf":0.0,"kd":0.0},"friction":0.07813665270805359,"steeringAngleDeadzoneDeg":0.0,"latAccelFactor":2.963873863220215,"latAccelOffset":0.0}}},"log_fields":["error","p","i","d","f","output","actualLateralAccel","desiredLateralAccel","desiredLateralJerk"],"cases":[{"name":"pull_away","frames":[{"input":[false,0.0,2.4933139152262838,false,false,-0.0013323183357528668,true,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.428055606948259,false,false,-0.001291240963619141,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.3605182283170976,false,false,-0.0012487290121424167,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.290776813916581,false,false,-0.0012048297124508391,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.218908847032376,false,false,-0.0011595918370278437,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.1449941735674725,false,false,-0.0011130656455255266,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,2.0691149133327222,false,false,-0.0010653028289257553,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.9913553688110548,false,false,-0.0010163564521110636,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.911801931496727,false,false,-0.0009662808949091303,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.830542985913659,false,false,-0.000915131791676342,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.7476688114195011,false,false,-0.0008629659694875654,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.663271481904524,false,false,-0.0008098413850007994,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.5774447634967683,false,false,-0.0007558170600668505,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.4902840103871058,false,false,-0.000700953016155571,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.4018860588899444,false,false,-0.0006453102076715109,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.312349119857291,false,false,-0.000588950454233074,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.2217726695656803,false,false,-0.0005319363719904111,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.1302573391972177,false,false,-0.0004743313040583631,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,1.037904803037503,false,false,-0.0004161992501417374,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,0.0,0.944817665514665,false,false,-0.00035760479543111017,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,0.0,0.8510993472049957,false,false,-0.000298613038848147,false,0.2],"torque":0.001395057194823898,"active":true,"saturated":false,"log":[0.0,0.0,0.0,-0.0,-2.7755575615628914e-17,0.001395057188346982,-0.0,0.0,0.0]},{"input":[true,0.045,0.7568544245739397,false,false,-0.00023928952072016532,false,0.2],"torque":0.001618190136718334,"active":true,"saturated":false,"log":[-3.3342744245601352e-06,-0.0008335686288774014,0.0,0.0,-3.050297891604714e-06,0.0016181900864467025,-3.4888410027633654e-07,-2.465410489094211e-06,7.46762566450343e-07]},{"input":[true,0.09,0.6621876067825287,false,false,-0.00017970014996429066,false,0.2],"torque":0.002354113340185872,"active":true,"saturated":false,"log":[-1.3102652701491024e-05,-0.003275663126260042,0.0,0.0,-1.1537645150383469e-05,0.002354113385081291,-1.048011199600296e-06,-9.368715836899355e-06,3.7299475934560178e-06]},{"input":[true,0.135,0.5672033877362129,false,false,-0.00011991113086210739,false,0.2],"torque":0.0035650398530351957,"active":true,"saturated":false,"log":[-2.8812437449232675e-05,-0.007203109562397003,0.0,0.0,-2.4354594643227756e-05,0.003565039951354265,-1.5734738099126844e-06,-1.9878036255249754e-05,1.0399921848147642e-05]},{"input":[true,0.18,0.4720066111918346,false,false,-5.998888950615651e-05,false,0.2],"torque":0.005208909819821549,"active":true,"saturated":false,"log":[-4.993898619432002e-05,-0.012484746985137463,0.0,0.0,-4.03664744226262e-05,0.005208909977227449,-1.3994206256029429e-06,-3.3138734579551965e-05,2.2160196749609895e-05]},{"input":[true,0.22499999999999998,0.37670235604520796,false,false,0.0,false,0.2],"torque":0.007241309182872282,"active":true,"saturated":false,"log":[-7.592821930302307e-05,-0.0189820546656847,0.0,-0.0,-5.841602251166478e-05,0.007241309154778719,2.8879928505066987e-13,-4.8276913730660453e-05,4.03601661673747e-05]},{"input":[true,0.27,0.2813958213608316,false,false,5.998888950615651e-05,false,0.2],"torque":0.006101958093430658,"active":true,"saturated":false,"log":[-0.00010619919339660555,-0.026549797505140305,0.0,-0.0,-7.732806989224628e-05,0.006101957987993956,3.1486972602579044e-06,-6.440294237108901e-05,6.628815754083917e-05]},{"input":[true,0.315,0.1861922112683742,false,false,0.00011991113086210739,false,0.2],"torque":0.008727051328358168,"active":true,"saturated":false,"log":[-0.00014014662883710116,-0.03503665700554848,-4.905131731902657e-07,-0.0,-9.591421257937327e-05,0.008727051317691803,8.566691576561425e-06,-8.061500557232648e-05,0.00010116480552824214]},{"input":[true,0.36,0.09119661985072017,false,false,0.00017970014996429066,false,0.2],"torque":0.011596522145564732,"active":true,"saturated":false,"log":[-0.0001771435490809381,-0.044285885989665985,-1.110515654545452e-06,-0.0,-0.0001129775118897669,0.01159652229398489,1.676818101259414e-05,-9.600265184417367e-05,0.00014613675011787564]},{"input":[true,0.40499999999999997,-0.0034860838516289627,false,false,0.00023928952072016532,false,0.2],"torque":0.014654437919112959,"active":true,"saturated":false,"log":[-0.0002165438636438921,-0.054135967046022415,-1.8684191900320002e-06,-0.0,-0.0001273171801585704,0.014654438011348248,2.825961564667523e-05,-0.0001096503619919531,0.00020227066124789417]},{"input":[true,0.44999999999999996,-0.09775137059511979,false,false,0.000298613038848147,false,0.2],"torque":0.01784766852356485,"active":true,"saturated":false,"log":[-0.00025768502382561564,-0.06442125886678696,-2.7703167688741814e-06,-0.0,-0.00013773326645605266,0.017847668379545212,4.35377805843018e-05,-0.00012064108886988834,0.0002705476072151214]},{"input":[true,0.495,-0.19149516532225763,false,false,0.00035760479543111017,false,0.2],"torque":0.02112217607994741,"active":true,"saturated":false,"log":[-0.00029989058384671807,-0.07497264444828033,-3.819933681370458e-06,-0.0,-0.000143031298648566,0.021122176200151443,6.308792217168957e-05,-0.00012805982260033488,0.00035185780143365264]},{"input":[true,0.54,-0.28461396106123077,false,false,0.0004161992501417374,false,0.2],"torque":0.02479689572678665,"active":true,"saturated":false,"log":[-0.0003424729220569134,-0.08561822772026062,-5.018589035898913e-06,-0.0,-0.0013192269252613187,0.024796895682811737,8.738186443224549e-05,-0.00013099714124109596,0.00044699557474814355]},{"input":[true,0.585,-0.3770049322125378,false,false,0.0004743313040583631,false,0.2],"torque":0.028489568116841734,"active":true,"saturated":false,"log":[-0.00038473590393550694,-0.09618397802114487,-6.365164608723717e-06,-0.0,-0.0026351006235927343,0.02848956733942032,0.00011687618098221719,-0.00012855263776145875,0.0005566549953073263]},{"input":[true,0.63,-0.46856604709976396,false,false,0.0005319363719904111,false,0.2],"torque":0.03209689511132452,"active":true,"saturated":false,"log":[-0.0004259773704688996,-0.10649434477090836,-7.8560851761722e-06,-0.0,-0.003942351788282394,0.032096896320581436,0.00015201039786916226,-0.00011983851436525583,0.0006814255029894412]},{"input":[true,0.6749999999999999,-0.5591961796624336,false,false,0.000588950454233074,false,0.2],"torque":0.03556248483543484,"active":true,"saturated":false,"log":[-0.00046549199032597244,-0.11637299507856369,-9.485307600698434e-06,-0.0,-0.005239853635430336,0.035562485456466675,0.00019320519641041756,-0.00010398291487945244,0.0008217881550081074]},{"input":[true,0.72,-0.6487952201697926,false,false,0.0006453102076715109,false,0.2],"torque":0.0388299762007114,"active":true,"saturated":false,"log":[-0.0005025736754760146,-0.12564341723918915,-1.1244314919167664e-05,-0.0,-0.006526508368551731,0.038829974830150604,0.00024086075427476317,-8.013338810997084e-05,0.0009781121043488383]},{"input":[true,0.765,-0.7372641848354138,false,false,0.000700953016155571,false,0.2],"torque":0.04184318666357658,"active":true,"saturated":false,"log":[-0.0005365182878449559,-0.1341295689344406,-1.3122129530529492e-05,-0.0,-0.007801249157637358,0.04184318706393242,0.0002953549555968493,-4.7460198402404785e-05,0.0011506517184898257]},{"input":[true,0.8099999999999999,-0.8245053242136979,false,false,0.0007558170600668505,false,0.2],"torque":0.044546326744141475,"active":true,"saturated":false,"log":[-0.0005666263168677688,-0.14165657758712769,-1.5105321836017538e-05,-0.0,-0.009063046425580978,0.044546328485012054,0.0003570419503375888,-5.159617558092577e-06,0.0013395434943959117]},{"input":[true,0.855,-0.9104222302606537,false,false,0.0008098413850007994,false,0.2],"torque":0.04688449393582544,"active":true,"saturated":false,"log":[-0.0005922052077949047,-0.14805130660533905,-1.71780393429799e-05,-0.0,-0.010310913436114788,0.04688449576497078,0.00042625030619092286,4.7542846004944295e-05,0.001544804428704083]},{"input":[true,0.8999999999999999,-0.9949199419427575,false,false,0.0008629659694875654,false,0.2],"torque":0.048803107918144135,"active":true,"saturated":false,"log":[-0.0006125721265561879,-0.1531430333852768,-1.9322042135172524e-05,-0.0,-0.01154390536248684,0.04880310967564583,0.0005032817716710269,0.00011138931586174294,0.0017663293983787298]},{"input":[true,0.945,-1.077905049279263,false,false,0.000915131791676342,false,0.2],"torque":0.05024872197928329,"active":true,"saturated":false,"log":[-0.0006270563462749124,-0.15676409006118774,-2.151674016204197e-05,-0.0,-0.012761128135025501,0.05024872347712517,0.0005884096026420593,0.00018708647985477,0.002003889996558428]},{"input":[true,0.99,-1.1592857957050058,false,false,0.0009662808949091303,false,0.2],"torque":0.051169093835604106,"active":true,"saturated":false,"log":[-0.0006350015755742788,-0.15875039994716644,-2.373924507992342e-05,-0.0,-0.013961739838123322,0.05116909369826317,0.0006818773690611124,0.00027530259103514254,0.002257133601233363]},{"input":[true,1.035,-1.238972178642541,false,false,0.0010163564521110636,false,0.2],"torque":0.0491280930129253,"active":true,"saturated":false,"log":[-0.0006286325515247881,-0.15143758058547974,-2.5939458282664418e-05,-0.0,-0.015139470808207989,0.04912809282541275,0.0007838974706828594,0.0003766645968426019,0.002525581978261471]},{"input":[true,1.08,-1.3168760481743809,false,false,0.0010653028289257553,false,0.2],"torque":0.04600510679401422,"active":true,"saturated":false,"log":[-0.0006140905315987766,-0.14074954390525818,-2.8088776161894202e-05,-0.0,-0.016298791393637657,0.04600510746240616,0.0008946498273871839,0.0004917553742416203,0.0028086320962756872]},{"input":[true,1.125,-1.3929112037081404,false,false,0.0011130656455255266,false,0.2],"torque":0.04253823461729041,"active":true,"saturated":false,"log":[-0.0005930997431278229,-0.12899918854236603,-3.016462505911477e-05,-0.0,-0.017440825700759888,0.04253823310136795,0.0010142810642719269,0.0006211110157892108,0.003105555195361376]},{"input":[true,1.17,2.023899046256842,false,false,0.0011595918370278437,false,0.2],"torque":-0.17482651634518942,"active":true,"saturated":false,"log":[0.0026728669181466103,0.5500760078430176,-2.0809589841519482e-05,0.0,-0.01607675664126873,-0.174826517701149,-0.0010210935724899173,0.0007652182248421013,0.003415497252717614]},{"input":[true,1.2149999999999999,5.38826997698619,false,false,0.0012048297124508391,false,0.2],"torque":-0.2311267551383678,"active":true,"saturated":false,"log":[0.0063579631969332695,1.2340806722640991,1.4432806665354292e-06,0.0,-0.014378353022038937,-0.23112675547599792,-0.003350017126649618,0.0009245119872502983,0.0037374808453023434]},{"input":[true,1.26,8.646657224127619,false,false,0.0012487290121424167,false,0.2],"torque":-0.23231269311157698,"active":true,"saturated":false,"log":[0.010437577031552792,1.9038140773773193,3.797480167122558e-05,0.0,-0.012364540249109268,-0.23231269419193268,-0.005944790318608284,0.0010993729811161757,0.004070404917001724]},{"input":[true,1.305,11.74720094837511,false,false,0.001291240963619141,false,0.2],"torque":-0.23343460417593342,"active":true,"saturated":false,"log":[0.014878037385642529,2.5396809577941895,9.00479280971922e-05,0.0,-0.010061252862215042,-0.23343460261821747,-0.008767248131334782,0.001290125772356987,0.0044130487367510796]},{"input":[true,1.3499999999999999,14.64053621487128,false,false,0.0013323183357528668,false,0.2],"torque":-0.23459751645058985,"active":true,"saturated":false,"log":[0.019637608900666237,3.122379779815674,9.00479280971922e-05,0.0,-0.007500665262341499,-0.23459751904010773,-0.011768901720643044,0.001497036311775446,0.004764070268720388]},{"input":[true,1.395,17.280564405626656,false,false,0.0013719154912449113,false,0.2],"torque":-0.23580256408968656,"active":true,"saturated":false,"log":[0.024668168276548386,3.6336212158203125,9.00479280971922e-05,0.0,-0.004719906952232122,-0.235802561044693,-0.014891376718878746,0.0017203104216605425,0.005122012458741665]},{"input":[true,1.44,19.62517362612556,false,false,0.0014099884373294702,false,0.2],"torque":-0.2370506484447301,"active":true,"saturated":false,"log":[0.02991778776049614,4.056851863861084,9.00479280971922e-05,0.0,-0.0017590795177966356,-0.23705065250396729,-0.018067123368382454,0.0019600915256887674,0.005485304165631533]},{"input":[true,1.4849999999999999,21.636896863255973,false,false,0.0014464948746499401,true,0.2],"torque":-0.23834263530041208,"active":true,"saturated":false,"log":[0.03533460944890976,4.377958297729492,9.00479280971922e-05,0.0,0.0013417286099866033,-0.23834262788295746,-0.021220432594418526,0.0022164597176015377,0.005852262023836374]},{"input":[true,1.53,23.283497623111042,false,false,0.0014813942442538548,false,0.2],"torque":-0.23967935312072491,"active":true,"saturated":false,"log":[0.040328506380319595,4.706336498260498,9.00479280971922e-05,0.0,0.0041289678774774075,-0.23967935144901276,-0.024268701672554016,0.00248942943289876,0.006221097428351641]},{"input":[true,1.575,24.538473908903594,false,false,0.0015146477726542137,false,0.2],"torque":-0.24106159077597178,"active":true,"saturated":false,"log":[0.04483075812458992,5.0098371505737305,9.00479280971922e-05,0.0,0.006549592595547438,-0.24106159806251526,-0.027123982086777687,0.002778948750346899,0.006589917466044426]},{"input":[true,1.6199999999999999,25.381472673553247,false,false,0.0015462185149071458,false,0.2],"torque":-0.24249009513470293,"active":true,"saturated":false,"log":[0.04894540086388588,5.2273688316345215,9.00479280971922e-05,0.0,0.008683179505169392,-0.24249009788036346,-0.029694747179746628,0.003084897994995117,0.006956728640943766]},{"input":[true,1.665,25.798608278562234,false,false,0.0015760713956580457,false,0.2],"torque":-0.24396556852029389,"active":true,"saturated":false,"log":[0.05255402252078056,5.352627277374268,9.00479280971922e-05,0.0,0.010438424535095692,-0.24396556615829468,-0.03188786655664444,0.0034070885740220547,0.007319445721805096]},{"input":[true,1.71,25.782679988790818,false,false,0.0016041732481105837,false,0.2],"torque":-0.24548866603418232,"active":true,"saturated":false,"log":[0.05553791671991348,5.381624221801758,9.00479280971922e-05,0.0,0.011723760515451431,-0.24548867344856262,-0.033610742539167404,0.0037452627439051867,0.007675890810787678]},{"input":[true,1.755,25.333285108300288,false,false,0.001630492850875286,false,0.2],"torque":-0.2470599927511369,"active":true,"saturated":false,"log":[0.057780176401138306,5.312887191772461,9.00479280971922e-05,0.0,0.012448973022401333,-0.24705998599529266,-0.034773580729961395,0.004099092446267605,0.008023803122341633]},{"input":[true,1.7999999999999998,24.45682599096456,false,false,0.0016550009626567592,false,0.2],"torque":-0.24868010079551414,"active":true,"saturated":false,"log":[0.059167731553316116,5.147592544555664,9.00479280971922e-05,0.0,0.01252677571028471,-0.24868009984493256,-0.03529172018170357,0.004468178376555443,0.008360842242836952]},{"input":[true,1.845,23.1664108156483,false,false,0.0016776703547410072,false,0.2],"torque":-0.25034948631130505,"active":true,"saturated":false,"log":[0.05959325283765793,4.889626502990723,9.00479280971922e-05,0.0,0.01187425572425127,-0.2503494918346405,-0.03508799895644188,0.0048520504496991634,0.008684595115482807]},{"input":[true,1.89,21.481649673566555,false,false,0.0016984758412467596,false,0.2],"torque":-0.25206858634281604,"active":true,"saturated":false,"log":[0.05895674601197243,4.545565128326416,9.00479280971922e-05,0.0,0.01041410118341446,-0.25206857919692993,-0.034095101058483124,0.005250168032944202,0.008992581628262997]},{"input":[true,1.9349999999999998,19.428349149108854,false,false,0.0017173943071071896,false,0.2],"torque":-0.25383777564710885,"active":true,"saturated":false,"log":[0.05716664716601372,4.124573707580566,9.00479280971922e-05,0.0,0.008075455203652382,-0.2538377642631531,-0.03225778043270111,0.00566191878169775,0.009282258339226246]},{"input":[true,1.98,17.03811015943694,false,false,0.0017344047337509473,false,0.2],"torque":-0.2556573634637362,"active":true,"saturated":false,"log":[0.05414023622870445,3.638223886489868,9.00479280971922e-05,0.0,0.004794210661202669,-0.2556573748588562,-0.029535003006458282,0.006086619105190039,0.009551030583679676]},{"input":[true,2.025,14.347835327830536,false,false,0.0017494882224539628,false,0.2],"torque":-0.2575275902718907,"active":true,"saturated":false,"log":[0.04929478466510773,3.1610281467437744,9.00479280971922e-05,0.0,0.0001225313317263499,-0.25752758979797363,-0.025901833549141884,0.006523516029119492,0.009796255268156528]},{"input":[true,2.07,11.399153577503347,false,false,0.0017626280153360862,false,0.2],"torque":-0.25944862456974466,"active":true,"saturated":false,"log":[0.04276271536946297,2.6748077869415283,0.00023971743939910084,-0.0,-0.005838556215167046,-0.2594486176967621,-0.02135111577808857,0.006971784867346287,0.010015249252319336]},{"input":[true,2.1149999999999998,8.237770924410393,false,false,0.0017738095139792282,false,0.2],"torque":-0.26142055971544215,"active":true,"saturated":false,"log":[0.03499738126993179,2.133965253829956,0.00036220825859345496,-0.0,-0.01274184975773096,-0.2614205479621887,-0.015894828364253044,0.00743053387850523,0.01020529679954052]},{"input":[true,2.16,4.912757599261163,false,false,0.0017830202956463255,false,0.2],"torque":-0.26344341087388257,"active":true,"saturated":false,"log":[0.026054758578538895,1.5476526021957397,0.0004533999308478087,-0.0,-0.02054518461227417,-0.2634434103965759,-0.009565114043653011,0.007898801006376743,0.010363655164837837]},{"input":[true,2.205,1.4757826225676565,false,false,0.0017902501270831014,false,0.2],"torque":-0.2655171121179991,"active":true,"saturated":false,"log":[0.016016755253076553,0.9261688590049744,0.0005094585940241814,-0.0,-0.0291865523904562,-0.26551711559295654,-0.0024149082601070404,0.008375559002161026,0.010487564839422703]},{"input":[true,2.25,-2.4842652058334065,false,false,0.001795490975887298,false,0.2],"torque":-0.05196680240563652,"active":true,"saturated":false,"log":[0.003420943394303322,0.19242806732654572,0.0005214318516664207,-0.0,-0.03978865221142769,-0.05196680128574371,0.006544564850628376,0.008859713561832905,0.01057425793260336]},{"input":[true,2.295,-2.4897826814759663,false,false,0.0017987370194327387,false,0.2],"torque":-0.0547877838753116,"active":true,"saturated":false,"log":[0.0037218460347503424,0.2034919261932373,0.0005344583187252283,-0.0,-0.04049810767173767,-0.054787784814834595,0.006821274757385254,0.009350105188786983,0.010620961897075176]},{"input":[true,2.34,-2.4921231707811162,false,false,0.0017999846513383159,false,0.2],"torque":-0.05794585441930089,"active":true,"saturated":false,"log":[0.004032288212329149,0.2141145020723343,0.0005485713481903076,-0.0,-0.0412026084959507,-0.0579458549618721,0.007096317131072283,0.009845515713095665,0.01062491349875927]},{"input":[true,2.385,-2.491283305227617,false,false,0.001799232485474709,false,0.2],"torque":-0.06094669023300071,"active":true,"saturated":false,"log":[0.004352097865194082,0.22424183785915375,0.0005638037109747529,-0.0,-0.041903529316186905,-0.060946691781282425,0.007368796039372683,0.010344660840928555,0.010583360679447651]},{"input":[true,2.4299999999999997,-2.4872632116030746,false,false,0.0017964813575043867,false,0.2],"torque":-0.06377166347619595,"active":true,"saturated":false,"log":[0.004681101534515619,0.23382101953029633,0.0005801875377073884,-0.0,-0.04260228946805,-0.06377166509628296,0.007637790869921446,0.01084620039910078,0.01049357745796442]},{"input":[true,2.475,-2.4800665127715793,false,false,0.001791734323953177,false,0.2],"torque":-0.06640386358003109,"active":true,"saturated":false,"log":[0.005019127856940031,0.24280031025409698,0.000597754493355751,-0.0,-0.043300334364175797,-0.06640386581420898,0.007902354933321476,0.01134873740375042,0.010352867655456066]},{"input":[true,2.52,-2.469700324632262,false,false,0.0017849966588144434,false,0.2],"torque":-0.0690136849574318,"active":true,"saturated":false,"log":[0.005366016179323196,0.2511295676231384,0.0006165355443954468,-0.0,-0.0434105359017849,-0.06901368498802185,0.008161518722772598,0.011850820854306221,0.01015857420861721]},{"input":[true,2.565,-2.45617524927095,false,false,0.0017762758476896352,false,0.2],"torque":-0.07163257970834597,"active":true,"saturated":false,"log":[0.005721623543649912,0.2587604224681854,0.0006365612498484552,-0.0,-0.042787231504917145,-0.07163257896900177,0.008414293639361858,0.01235094666481018,0.009908091276884079]},{"input":[true,2.61,-2.439505364311297,false,false,0.0017655815794717213,false,0.2],"torque":-0.07401203917326808,"active":true,"saturated":false,"log":[0.006085832603275776,0.2656466066837311,0.0006578616448678076,-0.0,-0.04216766729950905,-0.07401204109191895,0.008659669198095798,0.012847562320530415,0.009598866105079651]},{"input":[true,2.655,-2.41970820847584,false,false,0.0017529257355807513,false,0.2],"torque":-0.07613694500013134,"active":true,"saturated":false,"log":[0.006458566524088383,0.2717441916465759,0.0006804666481912136,-0.0,-0.041553329676389694,-0.07613694667816162,0.008896620944142342,0.013339067809283733,0.009228415787220001]},{"input":[true,2.6999999999999997,-2.3968047633714984,false,false,0.0017383223767634996,false,0.2],"torque":-0.07799371136959775,"active":true,"saturated":false,"log":[0.006839799229055643,0.2770118713378906,0.000704405945725739,-0.0,-0.040945686399936676,-0.0779937133193016,0.009124106727540493,0.01382382307201624,0.008794328197836876]},{"input":[true,2.745,-2.3708194315181697,false,false,0.00172178772747186,false,0.2],"torque":-0.07956969297112977,"active":true,"saturated":false,"log":[0.007229574955999851,0.28141120076179504,0.0007297094562090933,-0.0,-0.0403461679816246,-0.07956968992948532,0.009341074153780937,0.014300143346190453,0.008294275961816311]},{"input":[true,2.79,-2.3417800106430895,false,false,0.001703340157837346,false,0.2],"torque":-0.0808532786091096,"active":true,"saturated":false,"log":[0.007628030609339476,0.28490695357322693,0.0007564075640402734,-0.0,-0.039756156504154205,-0.08085327595472336,0.009546458721160889,0.014766310341656208,0.007726022507995367]},{"input":[true,2.835,-2.3097176642676813,false,false,0.001683000163261724,false,0.2],"torque":-0.08183394779197206,"active":true,"saturated":false,"log":[0.008035420440137386,0.2874671518802643,0.0007845315267331898,-0.0,-0.03917694464325905,-0.08183395117521286,0.009739188477396965,0.015220570378005505,0.007087429985404015]},{"input":[true,2.88,-2.2746668886176393,false,false,0.0016607903416464523,false,0.2],"torque":-0.08250237635372729,"active":true,"saturated":false,"log":[0.008452151902019978,0.28906360268592834,0.0008141140569932759,-0.0,-0.03860972076654434,-0.08250238001346588,0.00991818681359291,0.015661140903830528,0.006376469507813454]},{"input":[true,2.925,-2.236665475890944,false,false,0.001636735368286227,false,0.2],"torque":-0.08285051431934007,"active":true,"saturated":false,"log":[0.008878828957676888,0.28967180848121643,0.0008451899630017579,-0.0,-0.038055527955293655,-0.08285051584243774,0.010082371532917023,0.016086211428046227,0.005591228138655424]},{"input":[true,2.9699999999999998,-2.1957544739224786,false,false,0.0016108619684545263,false,0.2],"torque":-0.08287169312308656,"active":true,"saturated":false,"log":[0.009316311217844486,0.2892714738845825,0.0008777970215305686,-0.0,-0.03751521557569504,-0.0828716903924942,0.010230661369860172,0.01649394817650318,0.004729917738586664]},{"input":[true,3.0149999999999997,-2.15197814228781,false,false,0.001583198887711611,false,0.2],"torque":-0.0833496864381211,"active":true,"saturated":false,"log":[0.009723806753754616,0.2903650104999542,0.000911830342374742,-0.0,-0.03702152892947197,-0.08334968984127045,0.01036197692155838,0.016882503405213356,0.0037908819504082203]},{"input":[true,3.06,-2.1053839048925376,false,false,0.0015537768599679727,false,0.2],"torque":-0.08519415062715165,"active":true,"saturated":false,"log":[0.010047625750303268,0.29585233330726624,0.0009469970245845616,-0.0,-0.03661706671118736,-0.08519414812326431,0.010475240647792816,0.01725000888109207,0.0027726045809686184]},{"input":[true,3.105,-2.0560222990974646,false,false,0.0015226285733387025,false,0.2],"torque":-0.08688529996213405,"active":true,"saturated":false,"log":[0.010364742018282413,0.3008755147457123,0.0009832736104726791,-0.0,-0.036244358867406845,-0.08688530325889587,0.010569384321570396,0.017594588920474052,0.0016737171681597829]},{"input":[true,3.15,-2.0039469214335552,true,false,0.0014897886338267289,false,0.2],"torque":-0.08617453611079971,"active":true,"saturated":false,"log":[0.010674218647181988,0.305416077375412,0.0009832736104726791,-0.0,-0.03590559586882591,-0.08617453277111053,0.010643348097801208,0.017914360389113426,0.0004930063150823116]},{"input":[true,3.195,-1.949214369964386,true,false,0.001455293526875262,false,0.2],"torque":-0.08756278453795616,"active":true,"saturated":false,"log":[0.010975101962685585,0.3094567060470581,0.0009832736104726791,-0.0,-0.03560292720794678,-0.08756278455257416,0.01069608423858881,0.018207436427474022,-0.000770578917581588]},{"input":[true,3.2399999999999998,-1.891884183357426,true,false,0.0014191815768321676,false,0.2],"torque":-0.08877449224203933,"active":true,"saturated":false,"log":[0.011266428045928478,0.31298136711120605,0.0009832736104726791,-0.0,-0.03533853217959404,-0.08877449482679367,0.010726560838520527,0.018471933901309967,-0.002117920434102416]},{"input":[true,3.2849999999999997,-1.8320187767290184,true,false,0.0013814929043712983,false,0.2],"torque":-0.08980416599978357,"active":true,"saturated":false,"log":[0.011547215282917023,0.3159750998020172,0.0009832736104726791,-0.0,-0.0351145900785923,-0.08980416506528854,0.01073376089334488,0.018705977126955986,-0.0035497238859534264]},{"input":[true,3.33,-1.7696833743315332,true,false,0.0013422693819180966,false,0.2],"torque":-0.09064679801793676,"active":true,"saturated":false,"log":[0.011816476471722126,0.3184244930744171,0.0009832736104726791,-0.0,-0.03493323549628258,-0.09064679592847824,0.010716689750552177,0.01890769973397255,-0.005066513083875179]},{"input":[true,3.375,-1.7049459391544952,true,false,0.0013015545871289842,false,0.2],"torque":-0.09129781837487602,"active":true,"saturated":false,"log":[0.0120732132345438,0.32031744718551636,0.0009832736104726791,-0.0,-0.0347965806722641,-0.09129782021045685,0.010674374178051949,0.019075248390436172,-0.00666862353682518]},{"input":[true,3.42,-1.6378770995149279,true,false,0.0012593937544762292,false,0.2],"torque":-0.09175312787278045,"active":true,"saturated":false,"log":[0.012316418811678886,0.32164326310157776,0.0009832736104726791,-0.0,-0.034706708043813705,-0.09175312519073486,0.010605868883430958,0.01920679584145546,-0.008356194943189621]},{"input":[true,3.465,-1.5685500727153858,true,false,0.001215833724992072,false,0.2],"torque":-0.09200913629111507,"active":true,"saturated":false,"log":[0.012545081786811352,0.32239291071891785,0.0009832736104726791,-0.0,-0.03466567024588585,-0.09200913459062576,0.01051025278866291,0.019300533458590508,-0.010129169560968876]},{"input":[true,3.51,-1.4970405858513838,true,false,0.0011709228942279536,false,0.2],"torque":-0.09206278264941707,"active":true,"saturated":false,"log":[0.01275818794965744,0.3225588798522949,0.0009832736104726791,-0.0,-0.034675467759370804,-0.09206278622150421,0.010386639274656773,0.01935468427836895,-0.01198728196322918]},{"input":[true,3.5549999999999997,-1.4234267938530083,true,false,0.0011247111584866602,false,0.2],"torque":-0.09191152442515989,"active":true,"saturated":false,"log":[0.012954718433320522,0.3221352696418762,0.0009832736104726791,-0.0,-0.03473806753754616,-0.09191152453422546,0.010234172455966473,0.019367506727576256,-0.013930060900747776]},{"input":[true,3.5999999999999996,-1.3477891948485543,true,false,0.0010772498593871215,false,0.2],"torque":-0.09155337899618203,"active":true,"saturated":false,"log":[0.013133654370903969,0.3211178481578827,0.0009832736104726791,-0.0,-0.034855376929044724,-0.09155338257551193,0.010052033700048923,0.01933729462325573,-0.01595681719481945]},{"input":[true,3.645,-1.2702105429409059,true,false,0.0010285917268234453,false,0.2],"torque":-0.09098693990672636,"active":true,"saturated":false,"log":[0.0132939787581563,0.31950417160987854,0.0009832736104726791,-0.0,-0.03502925485372543,-0.0909869372844696,0.009839444421231747,0.019262386485934258,-0.018066642805933952]},{"input":[true,3.69,-1.190775758490279,true,false,0.0009787908203815724,false,0.2],"torque":-0.09021137753958611,"active":true,"saturated":false,"log":[0.013434676453471184,0.3172934651374817,0.0009832736104726791,-0.0,-0.035261500626802444,-0.09021137654781342,0.009595666080713272,0.019141167402267456,-0.02025841549038887]},{"input":[true,3.735,-1.1095718359996316,true,false,0.0009279024692786355,false,0.2],"torque":-0.08922645345584122,"active":true,"saturated":false,"log":[0.013554736040532589,0.3144868314266205,0.0009832736104726791,0.0,-0.03555384278297424,-0.08922645449638367,0.009320002980530262,0.01897207647562027,-0.022530777379870415]},{"input":[true,3.78,-1.0266877497016649,true,false,0.0008759832108917369,false,0.2],"torque":-0.08803253989612811,"active":true,"saturated":false,"log":[0.013653152622282505,0.31108707189559937,0.0009832736104726791,0.0,-0.03590794652700424,-0.08803253620862961,0.009011806920170784,0.01875360682606697,-0.024882148951292038]},{"input":[true,3.8249999999999997,-0.9422143569489272,true,false,0.0008230907279444615,false,0.2],"torque":-0.08663062364242223,"active":true,"saturated":false,"log":[0.013728928752243519,0.30709898471832275,0.0009832736104726791,0.0,-0.036325398832559586,-0.08663062006235123,0.00867047905921936,0.018484316766262054,-0.027310719713568687]},{"input":[true,3.8699999999999997,-0.856244299510883,false,false,0.0007692837844208935,true,0.2],"torque":-0.08640344865602707,"active":true,"saturated":false,"log":[0.01378107350319624,0.3025290071964264,0.0008348526898771524,0.0,-0.03680771589279175,-0.08640345185995102,0.008295469917356968,0.01816282980144024,-0.029814448207616806]},{"input":[true,3.915,-0.7688719028841533,false,false,0.0007146221602783498,false,0.2],"torque":-0.08456445003862659,"active":true,"saturated":false,"log":[0.013808609917759895,0.29738566279411316,0.000883182801771909,0.0,-0.03735632449388504,-0.0845644474029541,0.00788628589361906,0.017787834629416466,-0.032391052693128586]},{"input":[true,3.96,-0.6801930737243109,false,false,0.0006591665850313498,false,0.2],"torque":-0.08251927031818447,"active":true,"saturated":false,"log":[0.013810567557811737,0.29167917370796204,0.0009315197821706533,0.0,-0.03797256574034691,-0.08251927047967911,0.007442486472427845,0.017358101904392242,-0.03503802791237831]},{"input":[true,4.005,-0.5903051955097167,false,false,0.0006029786702806292,false,0.2],"torque":-0.08027707198559271,"active":true,"saturated":false,"log":[0.013785991817712784,0.2854217290878296,0.0009797707898542285,0.0,-0.038657695055007935,-0.08027707040309906,0.006963691208511591,0.016872476786375046,-0.03775262087583542]},{"input":[true,4.05,-1.3874831980697506,false,false,0.000546120841262141,false,0.2],"torque":-0.012691443642545553,"active":true,"saturated":false,"log":[0.0046443091705441475,0.09422142058610916,0.0009960258612409234,-0.0,-0.04635513201355934,-0.012691443786025047,0.012988733127713203,0.016329888254404068,-0.04053185135126114]},{"input":[true,4.095,-1.2021884027341248,false,false,0.0004886562674921086,false,0.2],"torque":-0.01632428582217393,"active":true,"saturated":false,"log":[0.005344503093510866,0.10620195418596268,0.0010147315915673971,-0.0,-0.0465843491256237,-0.01632428541779518,0.011881719343364239,0.015729352831840515,-0.0433725044131279]},{"input":[true,4.14,-1.0150627705567117,false,false,0.00043064879258516834,false,0.2],"torque":-0.02017691159233112,"active":true,"saturated":false,"log":[0.006063177715986967,0.11795911937952042,0.0010359527077525854,-0.0,-0.04684928059577942,-0.02017691172659397,0.010702664963901043,0.01506997924298048,-0.046271126717329025]},{"input":[true,4.185,-0.8263121532470052,false,false,0.0003721628633235905,false,0.2],"torque":-0.023931427006437113,"active":true,"saturated":false,"log":[0.0067999945022165775,0.12946338951587677,0.001059752656146884,0.0,-0.0471503771841526,-0.02393142692744732,0.009451309219002724,0.014350971207022667,-0.04922403022646904]},{"input":[true,4.2299999999999995,-0.6361442807061402,false,false,0.0003132634580563725,false,0.2],"torque":-0.02757675509178103,"active":true,"saturated":false,"log":[0.007554598618298769,0.14068551361560822,0.001086193835362792,0.0,-0.047488000243902206,-0.027576755732297897,0.008127528242766857,0.013571630232036114,-0.05222730711102486]},{"input":[true,4.2749999999999995,-0.44476853441841674,false,false,0.00025401601450776096,false,0.2],"torque":-0.03110369499427415,"active":true,"saturated":false,"log":[0.008326621726155281,0.1515965610742569,0.0011153370141983032,0.0,-0.047862425446510315,-0.031103694811463356,0.0067313374020159245,0.012731361202895641,-0.05527681112289429]},{"input":[true,4.32,-0.2523957189363883,false,false,0.00019448635707539666,false,0.2],"torque":-0.03450327458894552,"active":true,"saturated":false,"log":[0.009115686640143394,0.16216807067394257,0.0011472419137135148,0.0,-0.04827384278178215,-0.034503273665905,0.005262894090265036,0.011829675175249577,-0.058368172496557236]},{"input":[true,4.365,-0.05923783170664654,false,false,0.00013474062369886897,false,0.2],"torque":-0.03776676447434238,"active":true,"saturated":false,"log":[0.00992141105234623,0.17237211763858795,0.001181966857984662,0.0,-0.048722341656684875,-0.03776676580309868,0.0037225030828267336,0.010866193100810051,-0.061496805399656296]},{"input":[true,4.41,0.13449216851470233,false,false,7.484519237992288e-05,false,0.2],"torque":-0.04088587074377475,"active":true,"saturated":false,"log":[0.010743408463895321,0.18218134343624115,0.0012195687741041183,0.0,-0.049207903444767,-0.04088586941361427,0.002110615372657776,0.009840646758675575,-0.06465789675712585]},{"input":[true,4.455,0.328580594406243,false,false,1.4866607435947705e-05,false,0.2],"torque":-0.043829914609288735,"active":true,"saturated":false,"log":[0.01158129796385765,0.19156914949417114,0.001260103308595717,0.0,-0.04973042011260986,-0.043829914182424545,0.00042783410754054785,0.008752886205911636,-0.06784641742706299]},{"input":[true,4.5,0.5228132637330412,false,false,-4.5128494431304356e-05,false,0.2],"torque":-0.04921178021887969,"active":true,"saturated":false,"log":[0.012434708885848522,0.2005096822977066,0.001303624827414751,0.0,-0.05028965696692467,-0.0492117814719677,-0.0013250852935016155,0.00760287931188941,-0.07105714827775955]},{"input":[true,4.545,0.7169757336529099,false,false,-0.00010507345816964415,false,0.2],"torque":-0.05180580126908914,"active":true,"saturated":false,"log":[0.013303284533321857,0.20897796750068665,0.001350186299532652,0.0,-0.05088526010513306,-0.051805801689624786,-0.003147232113406062,0.006390714086592197,-0.07428465783596039]},{"input":[true,4.59,0.9108535354231275,false,false,-0.0001649016844307542,false,0.2],"torque":-0.05421669831460363,"active":true,"saturated":false,"log":[0.014186692424118519,0.21694999933242798,0.0013998396461829543,0.0,-0.051516756415367126,-0.05421669781208038,-0.0050375391729176044,0.005116603337228298,-0.07752329856157303]},{"input":[true,4.635,1.104232409253086,false,false,-0.0002245467035626178,false,0.2],"torque":-0.05645407569003719,"active":true,"saturated":false,"log":[0.015084633603692055,0.2244027853012085,0.0014526358572766185,0.0,-0.05218353867530823,-0.05645407736301422,-0.0069947815500199795,0.003780885599553585,-0.08076725900173187]},{"input":[true,4.68,1.2968985390479664,false,false,-0.00028394224945784677,false,0.2],"torque":-0.05851130781270546,"active":true,"saturated":false,"log":[0.01599685102701187,0.23131446540355682,0.0015086248749867082,0.0,-0.052884846925735474,-0.05851130932569504,-0.009017573669552803,0.002384028397500515,-0.0840105339884758]},{"input":[true,4.725,1.4886387867886037,false,true,-0.0003430223331758737,false,0.2],"torque":-0.06036332416267537,"active":true,"saturated":false,"log":[0.016923144459724426,0.23766441643238068,0.0015086248749867082,0.0,-0.053619783371686935,-0.060363322496414185,-0.011104372330009937,0.0009266287670470774,-0.08724693208932877]},{"input":[true,4.77,1.6792409262928938,false,true,-0.00040172131625720533,false,0.2],"torque":-0.062026503854664655,"active":true,"saturated":false,"log":[0.017863377928733826,0.2434331774711609,0.0015086248749867082,0.0,-0.054387155920267105,-0.062026504427194595,-0.0132534708827734,-0.0005905844154767692,-0.09047010540962219]},{"input":[true,4.8149999999999995,1.8684938761046819,false,true,-0.0004599739836482962,false,0.2],"torque":-0.06349908071711,"active":true,"saturated":false,"log":[0.018817508593201637,0.24860280752182007,0.0015086248749867082,0.0,-0.05518561229109764,-0.0634990781545639,-0.01546300295740366,-0.002166748745366931,-0.09367354959249496]},{"input":[true,4.859999999999999,2.0561879312568,false,true,-0.000517715616156007,false,0.2],"torque":-0.06476885970731253,"active":true,"saturated":false,"log":[0.019785594195127487,0.2531566917896271,0.0015086248749867082,0.0,-0.056013889610767365,-0.06476885825395584,-0.017730941995978355,-0.00380086712539196,-0.09685058891773224]},{"input":[true,4.905,2.2421149936560703,false,true,-0.0005748820623511629,false,0.2],"torque":-0.06583126727560387,"active":true,"saturated":false,"log":[0.020767824724316597,0.25707972049713135,0.0015086248749867082,0.0,-0.05687033385038376,-0.0658312663435936,-0.020055098459124565,-0.0054918075911700726,-0.09999442845582962]},{"input":[true,4.95,2.4260688008393636,false,true,-0.0006314098098413157,false,0.2],"torque":-0.06668231141534398,"active":true,"saturated":false,"log":[0.021764546632766724,0.26035839319229126,0.0015086248749867082,0.0,-0.0577530674636364,-0.06668230891227722,-0.022433122619986534,-0.007238300051540136,-0.10309813171625137]},{"input":[true,4.995,2.607845152851522,false,true,-0.0006872360558335404,false,0.2],"torque":-0.0673185657168807,"active":true,"saturated":false,"log":[0.022776298224925995,0.2629808187484741,0.0015086248749867082,0.0,-0.05865994840860367,-0.06731856614351273,-0.024862507358193398,-0.009038940072059631,-0.10615462809801102]},{"input":[true,5.04,2.7872421369977003,false,false,-0.0007422987769088523,false,0.2],"torque":-0.06914927478250922,"active":true,"saturated":false,"log":[0.02362712472677231,0.26944372057914734,0.0015913198003545403,0.0,-0.0597231388092041,-0.06914927810430527,-0.027340585365891457,-0.010892183519899845,-0.10915675759315491]},{"input":[true,5.085,2.964060350224957,false,false,-0.0007965367979307344,false,0.2],"torque":-0.07105434063939342,"active":true,"saturated":false,"log":[0.02444724179804325,0.2761560380458832,0.0016768851783126593,0.0,-0.06084149703383446,-0.07105433940887451,-0.029864534735679626,-0.012796349823474884,-0.11209724843502045]},{"input":[true,5.13,3.1381031188902972,false,false,-0.000849889860011222,false,0.2],"torque":-0.0728546180190634,"active":true,"saturated":false,"log":[0.025255946442484856,0.28256353735923767,0.0017652809619903564,0.0,-0.06199827045202255,-0.07285461574792862,-0.03243137523531914,-0.014749621972441673,-0.11496872454881668]},{"input":[true,5.175,3.3091767156750307,false,false,-0.0009022986874590205,false,0.2],"torque":-0.07454499961998123,"active":true,"saturated":false,"log":[0.026051834225654602,0.2886543273925781,0.0018564624479040504,0.0,-0.06319236755371094,-0.0745449960231781,-0.03503797948360443,-0.016750046983361244,-0.11776376515626907]},{"input":[true,5.22,3.4770905734082684,false,false,-0.0009537050536352881,false,0.2],"torque":-0.07612189079555404,"active":true,"saturated":false,"log":[0.026833489537239075,0.29441705346107483,0.0019503795774653554,0.0,-0.0644225999712944,-0.07612188905477524,-0.037681058049201965,-0.01879553124308586,-0.12047486752271652]},{"input":[true,5.265,3.6416574955656,false,false,-0.001004051845643908,false,0.2],"torque":-0.07753811533283618,"active":true,"saturated":false,"log":[0.027599478140473366,0.2998407185077667,0.0020469778683036566,0.0,-0.06582549214363098,-0.07753811776638031,-0.040357187390327454,-0.020883850753307343,-0.12309446185827255]},{"input":[true,5.31,3.802693863212409,false,false,-0.0010532831277843902,false,0.2],"torque":-0.07876405288936833,"active":true,"saturated":false,"log":[0.028348363935947418,0.30491501092910767,0.0021461970172822475,0.0,-0.06748145073652267,-0.07876405119895935,-0.04306279122829437,-0.02301264926791191,-0.1256149560213089]},{"input":[true,5.3549999999999995,3.960019838165007,false,false,-0.001101344203696894,false,0.2],"torque":-0.07987168758122147,"active":true,"saturated":false,"log":[0.02907869592308998,0.3096299469470978,0.0022479724138975143,0.0,-0.06915147602558136,-0.07987168431282043,-0.04579415172338486,-0.025179430842399597,-0.1280287355184555]},{"input":[true,5.3999999999999995,2.003952328994093,false,false,-0.0005,false,0.2],"torque":0.04457462740354087,"active":true,"saturated":false,"log":[-0.006917976774275303,-0.0729154720902443,0.0022237596567720175,-0.0,-0.07874832302331924,0.04457462579011917,-0.02114100009202957,-0.02605639398097992,-0.12370223551988602]},{"input":[true,5.444999999999999,2.0007976045806934,false,false,-0.0004988893003505612,false,0.2],"torque":0.04814922328100697,"active":true,"saturated":false,"log":[-0.007713632192462683,-0.08046860992908478,0.002196761779487133,-0.0,-0.07951749861240387,0.048149224370718,-0.021447069942951202,-0.02693968452513218,-0.11912231892347336]},{"input":[true,5.49,1.9904270520872334,false,false,-0.0004955621360170897,false,0.2],"torque":0.0512850916673107,"active":true,"saturated":false,"log":[-0.008640372194349766,-0.08920320123434067,0.00216652057133615,-0.0,-0.08036595582962036,0.051285091787576675,-0.02165762335062027,-0.027822984382510185,-0.11426415294408798]},{"input":[true,5.535,1.9728825615415588,false,false,-0.0004900332889206208,false,0.2],"torque":0.054784513784988595,"active":true,"saturated":false,"log":[-0.009694716893136501,-0.09904123097658157,0.0021325889974832535,-0.0,-0.08124232292175293,0.054784514009952545,-0.021768515929579735,-0.028700094670057297,-0.10910414904356003]},{"input":[true,5.58,1.948237847936072,false,false,-0.00048232732261528193,false,0.2],"torque":0.05862793156295098,"active":true,"saturated":false,"log":[-0.010872889310121536,-0.10990316420793533,0.0020945339929312468,-0.0,-0.08214106410741806,0.058627933263778687,-0.02177600748836994,-0.029564950615167618,-0.10362005978822708]},{"input":[true,5.625,1.9165981432782868,false,false,-0.00047247847315736883,false,0.2],"torque":0.06278775461916021,"active":true,"saturated":false,"log":[-0.01217078696936369,-0.12170787155628204,0.0020519362296909094,-0.0,-0.08305670320987701,0.06278775632381439,-0.021676795557141304,-0.03041163645684719,-0.09779097139835358]},{"input":[true,5.67,1.8780997490370968,false,false,-0.00046053049700144256,false,0.2],"torque":0.06723573637988858,"active":true,"saturated":false,"log":[-0.013583960942924023,-0.1343725472688675,0.002004392445087433,-0.0,-0.08398383855819702,0.06723573803901672,-0.021468045189976692,-0.031234387308359146,-0.09159744530916214]},{"input":[true,5.715,1.8329094508797739,false,false,-0.00044653647659921467,false,0.2],"torque":0.07194302458088808,"active":true,"saturated":false,"log":[-0.015107589773833752,-0.14781266450881958,0.0019515157910063863,-0.0,-0.0849171057343483,0.0719430223107338,-0.02114742062985897,-0.03202761709690094,-0.08502151817083359]},{"input":[true,5.76,1.781223798203781,false,false,-0.00043055858456490516,false,0.2],"torque":0.07688019657891566,"active":true,"saturated":false,"log":[-0.01673646830022335,-0.16194206476211548,0.0018929381622001529,-0.0,-0.08585122972726822,0.07688019424676895,-0.02071310579776764,-0.03278592228889465,-0.07804679870605469]},{"input":[true,5.805,1.723268251565888,false,false,-0.0004126678074548392,false,0.2],"torque":0.0820172780719638,"active":true,"saturated":false,"log":[-0.018464989960193634,-0.176673024892807,0.0018283106619492173,-0.0,-0.08678101748228073,0.08201728016138077,-0.020163830369710922,-0.033504094928503036,-0.07065852731466293]},{"input":[true,5.85,1.6592962016961597,false,false,-0.00039294363038847404,false,0.2],"torque":0.0873237969881585,"active":true,"saturated":false,"log":[-0.02028713934123516,-0.1919163316488266,0.0017573056975379586,-0.0,-0.08770134299993515,0.08732379972934723,-0.01949889399111271,-0.03417715057730675,-0.06284363567829132]},{"input":[true,5.895,1.589587864353586,false,false,-0.00037147368391202205,false,0.2],"torque":0.09276882336044244,"active":true,"saturated":false,"log":[-0.02219647914171219,-0.20758147537708282,0.0016796180279925466,-0.0,-0.08860719203948975,0.0927688255906105,-0.018718183040618896,-0.03480032458901405,-0.05459083616733551]},{"input":[true,5.9399999999999995,1.5144490558308648,false,false,-0.0003483533546735827,false,0.2],"torque":0.09832106977463698,"active":true,"saturated":false,"log":[-0.024186156690120697,-0.22357682883739471,0.0015949663938954473,-0.0,-0.08949363231658936,0.09832107275724411,-0.017822181805968285,-0.03536910563707352,-0.04589065536856651]},{"input":[true,5.984999999999999,1.4342098544457942,false,false,-0.0003236853616394762,false,0.2],"torque":0.10394896173523997,"active":true,"saturated":false,"log":[-0.026248900219798088,-0.23980994522571564,0.0015030952636152506,-0.0,-0.09035587310791016,0.10394895821809769,-0.016811998561024666,-0.0358792319893837,-0.036735523492097855]},{"input":[true,6.029999999999999,1.3492231538634094,false,false,-0.0002975792997345639,false,0.2],"torque":0.10962070866569441,"active":true,"saturated":false,"log":[-0.02837701514363289,-0.25618770718574524,0.0014037757646292448,-0.0,-0.09118922054767609,0.10962070524692535,-0.015689363703131676,-0.03632672131061554,-0.02711980976164341]},{"input":[true,6.075,1.2598631145742798,false,false,-0.00027015115293406986,false,0.2],"torque":0.11530443913744662,"active":true,"saturated":false,"log":[-0.030562402680516243,-0.2726166248321533,0.001296807313337922,-0.0,-0.09198913723230362,0.11530444025993347,-0.014456640928983688,-0.03670787811279297,-0.01703990250825882]},{"input":[true,6.12,1.1665235203080297,false,false,-0.00024152277897013832,false,0.2],"torque":0.12096829631546013,"active":true,"saturated":false,"log":[-0.03279656544327736,-0.2890033423900604,0.001182019361294806,-0.0,-0.09275121986865997,0.120968297123909,-0.013116830959916115,-0.03701930120587349,-0.006494251545518637]},{"input":[true,6.165,1.0696160465852356,false,false,-0.0002118213679424723,false,0.2],"torque":0.1265809548948736,"active":true,"saturated":false,"log":[-0.03507061302661896,-0.30525460839271545,0.0010592722101137042,-0.0,-0.09347254782915115,0.12658095359802246,-0.011673576198518276,-0.037257906049489975,0.004516589920967817]},{"input":[true,6.21,0.9695684490033998,false,false,-0.00018117887723833681,false,0.2],"torque":0.1321113035012701,"active":true,"saturated":false,"log":[-0.03737529739737511,-0.32127806544303894,0.0009284586412832141,-0.0,-0.09415014088153839,0.1321112960577011,-0.010131150484085083,-0.037420932203531265,0.015989912673830986]},{"input":[true,6.255,0.8668226792120537,false,false,-0.00014973144527246395,true,0.2],"torque":0.1375276976597048,"active":true,"saturated":false,"log":[-0.039701011031866074,-0.33698219060897827,0.000789505138527602,-0.0,-0.09477843344211578,0.1375277042388916,-0.008494457229971886,-0.03750595450401306,0.027920817956328392]},{"input":[true,6.3,0.7618329368565073,false,false,-0.00011761878665149471,false,0.2],"torque":0.047258885869802716,"active":true,"saturated":false,"log":[-0.009324969723820686,-0.07814324647188187,0.000789505138527602,-0.0,-0.07058502733707428,0.0472588874399662,-0.006769019644707441,-0.013588285073637962,0.03128357604146004]},{"input":[true,6.345,0.6550636660579503,false,false,-8.498357145012052e-05,false,0.2],"torque":0.05089180040726399,"active":true,"saturated":false,"log":[-0.011120136827230453,-0.09198576956987381,0.0007505846442654729,-0.0,-0.07069486379623413,0.050891801714897156,-0.0049609653651714325,-0.01309844572097063,0.03493175283074379]},{"input":[true,6.39,0.5469875052481997,false,false,-5.197079135667549e-05,false,0.2],"torque":0.05506697125473941,"active":true,"saturated":false,"log":[-0.012826026417315006,-0.10471168160438538,0.0007056935573928058,-0.0,-0.0706842765212059,0.055066972970962524,-0.0030770108569413424,-0.012468397617340088,0.038832224905490875]},{"input":[true,6.435,0.43808319938923945,false,false,-1.872711550426666e-05,false,0.2],"torque":0.058855646591808364,"active":true,"saturated":false,"log":[-0.014438018202781677,-0.116312675178051,0.0006551605183631182,-0.0,-0.07055481523275375,0.05885564535856247,-0.001124439062550664,-0.011701640672981739,0.04295129328966141]},{"input":[true,6.4799999999999995,0.32883348377987875,false,false,1.4599761150644409e-05,false,0.2],"torque":0.06375485127892416,"active":true,"saturated":false,"log":[-0.015951400622725487,-0.12678173184394836,0.000599330582190305,-0.0,-0.0703083798289299,0.0637548491358757,0.0008889224845916033,-0.010802171193063259,0.04725473001599312]},{"input":[true,6.5249999999999995,0.21972294878355703,false,false,4.786177400718783e-05,false,0.2],"torque":0.06671289991353935,"active":true,"saturated":false,"log":[-0.01736142300069332,-0.13611355423927307,0.0005385656259022653,-0.0,-0.06994717568159103,0.06671290099620819,0.002954730298370123,-0.009774498641490936,0.05170784145593643]},{"input":[true,6.569999999999999,0.11123589490195254,false,false,8.091114664128603e-05,false,0.2],"torque":0.06925641751851178,"active":true,"saturated":false,"log":[-0.018663350492715836,-0.144305020570755,0.00047324388287961483,-0.0,-0.0694737583398819,0.06925641745328903,0.005064156372100115,-0.008623647503554821,0.05627552792429924]},{"input":[true,6.614999999999999,0.003854187668162423,false,false,0.00011360104734654355,false,0.2],"torque":0.07139951168666907,"active":true,"saturated":false,"log":[-0.01985250972211361,-0.1513555347919464,0.00040376008837483823,-0.0,-0.06889104843139648,0.0713995099067688,0.007207921706140041,-0.007355168461799622,0.060922376811504364]},{"input":[true,6.66,-0.10194487815940978,false,false,0.0001457862414779463,false,0.2],"torque":0.07314394481623215,"active":true,"saturated":false,"log":[-0.020924340933561325,-0.15726734697818756,0.00033052489743568003,-0.0,-0.06820233911275864,0.07314394414424896,0.009376333095133305,-0.005975136533379555,0.06561274081468582]},{"input":[true,6.705,-0.20569069565577675,false,false,0.0001773237367001388,false,0.2],"torque":0.07449141860863988,"active":true,"saturated":false,"log":[-0.0218744408339262,-0.16204585134983063,0.00025396436103619635,-0.0,-0.06741131097078323,0.07449141889810562,0.011559324339032173,-0.0044901492074131966,0.07031083852052689]},{"input":[true,6.75,0.03724778410378332,false,false,0.0002080734182735712,false,0.2],"torque":0.04965180591886204,"active":true,"saturated":false,"log":[-0.013265987858176231,-0.0968417078256607,0.00020753340504597872,-0.0,-0.059398334473371506,0.04965180531144142,0.006825848948210478,-0.002907318528741598,0.07498087733983994]},{"input":[true,6.795,-0.01154570160828533,false,false,0.0002378986715560677,false,0.2],"torque":0.04642065307671449,"active":true,"saturated":false,"log":[-0.012462800368666649,-0.08963245898485184,0.00016391360259149224,-0.0,-0.057285938411951065,0.04642065241932869,0.007908670231699944,-0.0012342615518718958,0.07958715409040451]},{"input":[true,6.84,-0.058648801785302185,false,false,0.00026666698895416686,false,0.2],"torque":0.04319252484164225,"active":true,"saturated":false,"log":[-0.011536672711372375,-0.08172579109668732,0.0001235352538060397,-0.0,-0.05509309470653534,0.04319252446293831,0.008982846513390541,0.0005209153168834746,0.08409418165683746]},{"input":[true,6.885,-0.1038512837926519,false,false,0.0002942505586276729,false,0.2],"torque":0.039743102861423026,"active":true,"saturated":false,"log":[-0.010491504333913326,-0.07318873703479767,8.681498729856685e-05,-0.0,-0.052830588072538376,0.039743103086948395,0.010042866691946983,0.002349634189158678,0.0884668231010437]},{"input":[true,6.93,-0.14695119197420325,false,false,0.0003205268323319191,false,0.2],"torque":0.03608924805133363,"active":true,"saturated":false,"log":[-0.00933165568858385,-0.0640898123383522,5.415419218479656e-05,-0.0,-0.05050919950008392,0.036089248955249786,0.011083154007792473,0.0042428686283528805,0.09267042577266693]},{"input":[true,6.975,-0.1877557480609367,false,false,0.00034537906987493823,false,0.2],"torque":0.03225663431751968,"active":true,"saturated":false,"log":[-0.008061949163675308,-0.05449877679347992,2.593737190181855e-05,-0.0,-0.04814039170742035,0.032256633043289185,0.012098093517124653,0.006191168446093798,0.0966709554195404]},{"input":[true,7.02,-0.22608221162275305,false,false,0.0003686968577706227,false,0.2],"torque":0.028271535869233408,"active":true,"saturated":false,"log":[-0.00668767374008894,-0.044486407190561295,2.5305130293418188e-06,-0.0,-0.04573599994182587,0.02827153541147709,0.01308206096291542,0.008184690959751606,0.10043515264987946]},{"input":[true,7.0649999999999995,-0.2617586967308641,false,false,0.0003903765997836073,false,0.2],"torque":0.024160631059963043,"active":true,"saturated":false,"log":[-0.005214580800384283,-0.0341242179274559,-1.5720519513706677e-05,-0.0,-0.04330819100141525,0.024160631000995636,0.014029448851943016,0.010213231667876244,0.10393065214157104]},{"input":[true,7.109999999999999,-0.29462494118868715,false,false,0.0004103219771864765,false,0.2],"torque":0.019950893430458073,"active":true,"saturated":false,"log":[-0.0036488757468760014,-0.023484164848923683,-2.8491584089351818e-05,-0.0,-0.040869396179914474,0.01995089277625084,0.014934699051082134,0.012266261503100395,0.10712617635726929]},{"input":[true,7.154999999999999,-0.324533024895927,false,false,0.00042844437668447364,false,0.2],"torque":0.015669486490335064,"active":true,"saturated":false,"log":[-0.0019972107838839293,-0.012638350017368793,-3.5481822123983875e-05,-0.0,-0.0384322889149189,0.015669487416744232,0.015792330726981163,0.014332967810332775,0.10999161750078201]},{"input":[true,7.199999999999999,-0.3513480341325675,false,false,0.0004446632841065207,false,0.2],"torque":0.011343646245481157,"active":true,"saturated":false,"log":[-0.0002666715008672327,-0.0016586966812610626,-3.641517469077371e-05,-0.0,-0.03600971773266792,0.011343645863234997,0.016596969217061996,0.016402292996644974,0.11249824613332748]},{"input":[true,7.245,-0.3749486687859172,false,false,0.0004589066421134661,false,0.2],"torque":0.007000562183939671,"active":true,"saturated":false,"log":[0.0015352378832176328,0.009383373893797398,-3.104183997493237e-05,-0.0,-0.033614665269851685,0.007000562269240618,0.017343375831842422,0.018462978303432465,0.11461883783340454]},{"input":[true,7.29,-0.3952277897935406,false,false,0.000471111170334329,false,0.2],"torque":0.002667251472305034,"active":true,"saturated":false,"log":[0.0034006135538220406,0.020417284220457077,-1.913969208544586e-05,-0.0,-0.03126019239425659,0.0026672515086829662,0.018026480451226234,0.02050361968576908,0.11632779985666275]},{"input":[true,7.335,-0.41209290433686957,false,false,0.00048122264650824387,false,0.2],"torque":-0.0011819735204057528,"active":true,"saturated":false,"log":[0.005321172531694174,0.031373634934425354,-5.155890221431036e-07,-0.0,-0.028959373012185097,-0.0011819734936580062,0.018641412258148193,0.022512705996632576,0.11760133504867554]},{"input":[true,7.38,-0.4254665865932746,false,false,0.0004891961473830436,false,0.2],"torque":-0.005400338927883157,"active":true,"saturated":false,"log":[0.0072882771492004395,0.04218454658985138,2.499338188499678e-05,-0.0,-0.02672523632645607,-0.005400339141488075,0.01918351836502552,0.024478677660226822,0.11841755360364914]},{"input":[true,7.425,-0.4352868321372981,false,false,0.0004949962483002228,false,0.2],"torque":-0.009553343222002037,"active":true,"saturated":false,"log":[0.009292961098253727,0.0527840182185173,5.7518744142726064e-05,-0.0,-0.024570714682340622,-0.009553343057632446,0.01964840665459633,0.02638997882604599,0.11875664442777634]},{"input":[true,7.47,-0.4415073443732708,false,false,0.0004985971805795595,false,0.2],"torque":-0.013593788835232065,"active":true,"saturated":false,"log":[0.011325959116220474,0.06310824304819107,9.715960186440498e-05,-0.0,-0.02250857464969158,-0.01359378919005394,0.020031964406371117,0.02823510952293873,0.11860097199678421]},{"input":[true,7.515,-0.4440977516804527,false,false,0.0004999829460041724,false,0.2],"torque":-0.0175672168118585,"active":true,"saturated":false,"log":[0.013361530378460884,0.07332807779312134,0.00014392496086657047,-0.0,-0.020563552156090736,-0.017567217350006104,0.020330388098955154,0.030002683401107788,0.1179351955652237]},{"input":[true,7.56,-0.4430437542567967,false,false,0.0004991473878973766,false,0.2],"torque":-0.021564351775317115,"active":true,"saturated":false,"log":[0.015362853184342384,0.08375827223062515,0.00019769494247157127,-0.0,-0.018768254667520523,-0.02156435139477253,0.020540211349725723,0.03168148547410965,0.11674638837575912]},{"input":[true,7.6049999999999995,-0.43834719995709986,false,false,0.000496094218475557,false,0.2],"torque":-0.02544561435376828,"active":true,"saturated":false,"log":[0.017346467822790146,0.09394846856594086,0.00025840758462436497,-0.0,-0.01711459457874298,-0.025445614010095596,0.020658325403928757,0.033260527998209,0.115024134516716]},{"input":[true,7.6499999999999995,-0.43002608873436127,false,false,0.0004908370023555395,false,0.2],"torque":-0.029186496174832252,"active":true,"saturated":false,"log":[0.01930171623826027,0.10384323447942734,0.00032596359960734844,-0.0,-0.015614432282745838,-0.029186496511101723,0.02068200707435608,0.03472910448908806,0.11276064068078995]},{"input":[true,7.694999999999999,-0.4181145056081444,false,false,0.00048339909628973055,false,0.2],"torque":-0.03276529677040197,"active":true,"saturated":false,"log":[0.021217919886112213,0.1133885607123375,0.0004002263012807816,-0.0,-0.01427906658500433,-0.03276529535651207,0.020608937367796898,0.0360768586397171,0.10995080322027206]},{"input":[true,7.739999999999999,-0.4026624823993615,false,false,0.00047381354539676926,false,0.2],"torque":-0.03616105136174595,"active":true,"saturated":false,"log":[0.023084424436092377,0.12253212183713913,0.00048102179425768554,-0.0,-0.01311916671693325,-0.03616105020046234,0.020437223836779594,0.03729383274912834,0.10659226030111313]},{"input":[true,7.785,-0.38373578878568354,false,false,0.0004621229363487218,false,0.2],"torque":-0.03935361420167685,"active":true,"saturated":false,"log":[0.024890657514333725,0.13122354447841644,0.0005681390757672489,-0.0,-0.01214471273124218,-0.039353612810373306,0.020165419206023216,0.038370516151189804,0.10268551111221313]},{"input":[true,7.83,-0.36141565354439714,false,false,0.00044837920816707353,false,0.2],"torque":-0.04232373980181904,"active":true,"saturated":false,"log":[0.026626162230968475,0.13941457867622375,0.0006613306468352675,-0.0,-0.011364930309355259,-0.04232373833656311,0.01979253813624382,0.0392979234457016,0.09823388606309891]},{"input":[true,7.875,-0.3357984171585648,false,false,0.00043264342146811797,false,0.2],"torque":-0.04505322002877597,"active":true,"saturated":false,"log":[0.02828066609799862,0.14705947041511536,0.0007603130070492625,-0.0,-0.01078823022544384,-0.04505322128534317,0.019318070262670517,0.040067631751298904,0.09324366599321365]},{"input":[true,7.92,-0.30699511726645506,false,false,0.00041498548718293574,false,0.2],"torque":-0.047524952162062706,"active":true,"saturated":false,"log":[0.02984411083161831,0.1541149914264679,0.0008647673530504107,-0.0,-0.010422157123684883,-0.04752495139837265,0.018741993233561516,0.040671832859516144,0.08772403001785278]},{"input":[true,7.965,-0.2751310087320434,false,false,0.00039548385595720845,false,0.2],"torque":-0.0497230600621853,"active":true,"saturated":false,"log":[0.03130671754479408,0.1605408489704132,0.0009743408882059157,0.0,-0.010273332707583904,-0.0497230589389801,0.01806478574872017,0.04110339283943176,0.08168711513280869]},{"input":[true,8.0,-0.24028880630372668,false,false,0.00037422516961079937,false,0.2],"torque":-0.051356377478051725,"active":true,"saturated":false,"log":[0.03263933211565018,0.16646058857440948,0.0010885786032304168,0.0,-0.010420632548630238,-0.05135637894272804,0.01724429614841938,0.041290611028671265,0.0751500129699707]},{"input":[true,8.0,-0.20249809469733254,false,false,0.0003513038762055989,false,0.2],"torque":-0.05303221103266368,"active":true,"saturated":false,"log":[0.033747099339962006,0.1721101999282837,0.0012066933559253812,0.0,-0.01105949655175209,-0.05303221195936203,0.016188083216547966,0.04105051979422569,0.06815039366483688]},{"input":[true,8.0,-0.16213411214055995,false,false,0.00032682181043180595,false,0.2],"torque":-0.05433642530357668,"active":true,"saturated":false,"log":[0.034672461450099945,0.17682954668998718,0.0013280470157042146,0.0,-0.011935384944081306,-0.05433642491698265,0.015059949830174446,0.0406041257083416,0.060728371143341064]},{"input":[true,8.0,-0.11937618767851249,false,false,0.0003008877411769109,false,0.2],"torque":-0.055258040575412976,"active":true,"saturated":false,"log":[0.035413581877946854,0.18060927093029022,0.001451994525268674,0.0,-0.01304263062775135,-0.05525803938508034,0.013864907436072826,0.03995508700609207,0.05292530730366707]},{"input":[true,8.0,-0.07441428615803208,false,false,0.00027361688828743294,false,0.2],"torque":-0.055796821642505524,"active":true,"saturated":false,"log":[0.03596928343176842,0.1834433525800705,0.0015778871020302176,0.0,-0.014374656602740288,-0.05579682067036629,0.012608266435563564,0.03910784795880318,0.04478366672992706]},{"input":[true,8.0,-0.027448164252147633,false,false,0.0002451304106703497,false,0.2],"torque":-0.05595421501351688,"active":true,"saturated":false,"log":[0.03633907437324524,0.18532927334308624,0.0017050737515091896,0.0,-0.0159240011125803,-0.0559542141854763,0.01129561010748148,0.03806762397289276,0.03634687885642052]},{"input":[true,8.0,0.021313517018595962,false,false,0.00021555486800847306,false,0.2],"torque":-0.055732272892613115,"active":true,"saturated":false,"log":[0.036523133516311646,0.18626798689365387,0.0018329047597944736,0.0,-0.017682339996099472,-0.05573227256536484,0.009932768531143665,0.03684038668870926,0.027659164741635323]},{"input":[false,8.0,0.07165411932502488,false,false,0.00018502165848126905,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.1233499895098027,false,false,0.0001536664349892097,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.17617145323336134,false,false,0.00012162850247525857,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.22988383537073903,false,false,8.905019902106331e-05,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.2842485026258938,false,false,5.6076263467527434e-05,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.3390239237313777,false,false,2.2853192369290225e-05,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.39396674252311037,false,false,-1.0471410859959058e-05,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.4488328591228079,false,false,-4.37494917197232e-05,true,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.5033785144245768,false,false,-7.683320239902244e-05,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.5573613730675296,false,false,-0.00010957555863444268,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.6105416000829831,false,false,-0.00014183109273161314,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.6626829264329254,false,false,-0.00017345649984887746,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.7135536987057645,false,false,-0.00020431127467184204,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.7629279083057785,false,false,-0.00023425833565018857,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.8105861955637923,false,false,-0.00026316463402339594,false,0.2],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,8.0,0.8563168243080039,false,false,-0.00029090174492958534,false,0.2],"torque":-0.055533976818810095,"active":true,"saturated":false,"log":[0.04282383620738983,0.21840156614780426,0.00014988343173172325,0.0,-0.04538935795426369,-0.05553397536277771,-0.013404752127826214,0.018144769594073296,-0.013859129510819912]},{"input":[true,8.0,0.8999166225651551,false,false,-0.00031734643797131734,false,0.2],"torque":-0.04835537557213866,"active":true,"saturated":false,"log":[0.03872328996658325,0.19748878479003906,0.0002854149497579783,0.0,-0.05015288665890694,-0.04835537448525429,-0.014623323455452919,0.013905210420489311,-0.026758024469017982]},{"input":[true,8.0,0.941191885212685,false,false,-0.00034238122470342736,false,0.2],"torque":-0.040154803975801644,"active":true,"saturated":false,"log":[0.03467755764722824,0.17685554921627045,0.0004067863919772208,0.0,-0.05478513985872269,-0.04015480354428291,-0.015776926651597023,0.009771003387868404,-0.03870238736271858]},{"input":[true,8.0,0.9799592345715822,false,false,-0.0003658948806105272,false,0.2],"torque":-0.03207176320844655,"active":true,"saturated":false,"log":[0.030688712373375893,0.15651243925094604,0.0005141968722455204,0.0,-0.059277597814798355,-0.032071761786937714,-0.016860436648130417,0.005748799070715904,-0.04969773441553116]},{"input":[true,8.0,1.0160464351164764,false,false,-0.00038778293925512483,false,0.2],"torque":-0.024134439476830975,"active":true,"saturated":false,"log":[0.02675968036055565,0.1364743709564209,0.0006078557926230133,0.0,-0.06362149119377136,-0.0241344403475523,-0.01786903664469719,0.0018455703975632787,-0.059749703854322433]},{"input":[true,8.0,1.0492931586833898,false,false,-0.0004079481564009665,false,0.2],"torque":-0.016354251598356028,"active":true,"saturated":false,"log":[0.02289417013525963,0.1167602688074112,0.0006879853899590671,0.0,-0.06780681014060974,-0.01635425165295601,-0.01879825070500374,-0.0019314734963700175,-0.06886430084705353]},{"input":[true,8.0,1.0795516967754786,false,false,-0.00042630094204959286,false,0.2],"torque":-0.008738132995362136,"active":true,"saturated":false,"log":[0.019096562638878822,0.09739246964454651,0.0007548233261331916,0.0,-0.07182525098323822,-0.008738133125007153,-0.01964394748210907,-0.0055749742314219475,-0.07704813033342361]},{"input":[true,8.0,1.1066876168021378,false,false,-0.00044275975847065947,false,0.2],"torque":-0.0012941803148341657,"active":true,"saturated":false,"log":[0.0153718376532197,0.07839637249708176,0.0008086247835308313,0.0,-0.0756690725684166,-0.0012941802851855755,-0.020402370020747185,-0.009077505208551884,-0.08430864661931992]},{"input":[true,8.0,1.1305803593359243,false,false,-0.00045725148245765016,false,0.2],"torque":0.00596549169787641,"active":true,"saturated":false,"log":[0.011725487187504768,0.059799984097480774,0.0008496639784425497,0.0,-0.07932938635349274,0.005965491756796837,-0.021070148795843124,-0.012431653216481209,-0.09065432846546173]},{"input":[true,8.0,1.151123773733812,false,false,-0.00046971173019955657,false,0.2],"torque":0.012993507269980904,"active":true,"saturated":false,"log":[0.00816342607140541,0.041633471846580505,0.0008782359655015171,0.0,-0.08279753476381302,0.012993507087230682,-0.021644316613674164,-0.015630090609192848,-0.09609486162662506]},{"input":[true,8.0,1.168226589743121,false,false,-0.000480085143325183,false,0.2],"torque":0.019845929611351017,"active":true,"saturated":false,"log":[0.004691918846219778,0.02392878569662571,0.0008946576854214072,0.0,-0.0860651358962059,0.019845928996801376,-0.022122323513031006,-0.018665654584765434,-0.10064129531383514]},{"input":[true,8.0,1.1818128229968674,false,false,-0.0004883256348492412,false,0.2],"torque":0.026475094590707574,"active":true,"saturated":false,"log":[0.0013174967607483268,0.006719233468174934,0.0008992688963189721,0.0,-0.08912420272827148,0.026475094258785248,-0.022502044215798378,-0.02153140865266323,-0.1043061912059784]},{"input":[true,8.0,1.191822112597003,false,false,-0.0004943965939275431,false,0.2],"torque":0.032866278125110865,"active":true,"saturated":false,"log":[-0.0019531168509274721,-0.009960896335542202,0.0008924329886212945,0.0,-0.0919671580195427,0.03286627680063248,-0.022781794890761375,-0.02422071062028408,-0.10710371285676956]},{"input":[true,8.0,1.1982099892857276,false,false,-0.0004982710485116088,false,0.2],"torque":0.039005452121639285,"active":true,"saturated":false,"log":[-0.0051130796782672405,-0.026076706126332283,0.0008745372178964317,0.0,-0.09458697587251663,0.039005450904369354,-0.022960329428315163,-0.026727279648184776,-0.10904976725578308]},{"input":[true,8.0,1.2009480730134459,false,false,-0.0004999317851800449,false,0.2],"torque":0.04487867863531025,"active":true,"saturated":false,"log":[-0.008155502378940582,-0.04159306362271309,0.0008459929958917201,0.0,-0.09697721153497696,0.044878680258989334,-0.023036856204271317,-0.029045244678854942,-0.11016204208135605]},{"input":[true,8.0,1.2000241990256117,false,false,-0.0004993714256143098,false,0.2],"torque":0.05047223107559411,"active":true,"saturated":false,"log":[-0.011073516681790352,-0.05647493526339531,0.0008072356577031314,0.0,-0.09913205355405807,0.05047222971916199,-0.023011034354567528,-0.031169205904006958,-0.1104600802063942]},{"input":[true,8.0,1.1954424719082826,false,false,-0.0004965924593790963,false,0.2],"torque":0.05577272644250671,"active":true,"saturated":false,"log":[-0.013860340230166912,-0.07068773359060287,0.0007587244617752731,0.0,-0.10104642808437347,0.055772725492715836,-0.02288297936320305,-0.03309428319334984,-0.10996532440185547]},{"input":[true,8.0,1.1872232473522715,false,false,-0.0004916072328616975,false,0.2],"torque":0.06076725339293045,"active":true,"saturated":false,"log":[-0.016509339213371277,-0.08419763296842575,0.0007009417749941349,0.0,-0.10271599888801575,0.06076725199818611,-0.022653261199593544,-0.03481615334749222,-0.10870116949081421]},{"input":[true,8.0,1.1754030417169097,false,false,-0.0004844378944194944,false,0.2],"torque":0.06544350096024416,"active":true,"saturated":false,"log":[-0.019014092162251472,-0.09697186946868896,0.0006343924324028194,0.0,-0.10413724929094315,0.0654435008764267,-0.022322898730635643,-0.036331113427877426,-0.10669290274381638]},{"input":[true,8.0,1.1600343697952193,false,false,-0.0004751162959792648,false,0.2],"torque":0.06978986683112667,"active":true,"saturated":false,"log":[-0.021368442103266716,-0.10897905379533768,0.0005596029222942889,0.0,-0.10530749708414078,0.06978986412286758,-0.02189335785806179,-0.03763609007000923,-0.10396776348352432]}]},{"name":"city","frames":[{"input":[false,8.0,2.573317403210216,false,false,-0.0013323183357528668,true,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.505592467430547,false,false,-0.001291240963619141,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.4355023172072876,false,false,-0.0012487290121424167,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.363124823274477,false,false,-0.0012048297124508391,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.288540397623849,false,false,-0.0011595918370278437,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.211831904166451,false,false,-0.0011130656455255266,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.133084566670151,false,false,-0.0010653028289257553,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,2.0523858740753327,false,false,-0.0010163564521110636,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.9698254832939646,false,false,-0.0009662808949091303,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.8854951196000282,false,false,-0.000915131791676342,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.799488474721986,false,false,-0.0008629659694875654,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.711901102750499,false,false,-0.0008098413850007994,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.6228303139770448,false,false,-0.0007558170600668505,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.5323750667813827,false,false,-0.000700953016155571,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.4406358576879796,false,false,-0.0006453102076715109,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.3477146097135433,false,false,-0.000588950454233074,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.253714559129711,false,false,-0.0005319363719904111,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.1587401407667033,false,false,-0.0004743313040583631,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,1.0628968719853646,false,false,-0.0004161992501417374,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.9662912354465105,false,false,-0.00035760479543111017,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,8.0,0.8690305608078117,false,false,-0.000298613038848147,false,0.12],"torque":0.1402197537909187,"active":true,"saturated":false,"log":[-0.06119755282998085,-0.31210753321647644,-0.00021419143013190478,-0.0,-0.12387677282094955,0.14021974802017212,-0.013760088942945004,-0.058846041560173035,0.02496611699461937]},{"input":[true,8.0,0.7712229054796618,false,false,-0.00023928952072016532,false,0.12],"torque":0.13300066410287342,"active":true,"saturated":false,"log":[-0.05663348361849785,-0.2888307571411133,-0.00041240864084102213,-0.0,-0.11666125059127808,0.13300065696239471,-0.01102646067738533,-0.052749935537576675,0.048487477004528046]},{"input":[true,8.0,0.6729769345725043,false,false,-0.00017970014996429066,false,0.12],"torque":0.12336060408526954,"active":true,"saturated":false,"log":[-0.05222111940383911,-0.26632770895957947,-0.0005951825296506286,-0.0,-0.10954403877258301,0.12336060404777527,-0.00828058272600174,-0.04675334692001343,0.0706373006105423]},{"input":[true,8.0,0.5744018001690001,false,false,-0.00011991113086210739,false,0.12],"torque":0.1139451897709872,"active":true,"saturated":false,"log":[-0.04795031249523163,-0.24454659223556519,-0.0007630086620338261,-0.0,-0.10252124071121216,0.11394518613815308,-0.00552550470456481,-0.04085184633731842,0.09148335456848145]},{"input":[true,8.0,0.4756070200551651,false,false,-5.998888950615651e-05,false,0.12],"torque":0.10478536366739052,"active":true,"saturated":false,"log":[-0.04381181672215462,-0.2234402596950531,-0.00091634999262169,-0.0,-0.09558964520692825,0.10478536039590836,-0.0027642876375466585,-0.0350416824221611,0.11108840256929398]},{"input":[true,8.0,0.37670235604520796,false,false,0.0,false,0.12],"torque":0.09586578075004867,"active":true,"saturated":false,"log":[-0.03979722037911415,-0.20296582579612732,-0.0010556402849033475,-0.0,-0.08874667435884476,0.09586577862501144,3.5181249580240603e-10,-0.029319729655981064,0.1295105367898941]},{"input":[true,8.0,0.27779769203525095,false,false,5.998888950615651e-05,false,0.12],"torque":0.08711412643013544,"active":true,"saturated":false,"log":[-0.035898882895708084,-0.18308430910110474,-0.0011812864104285836,-0.0,-0.08181031048297882,0.0871141254901886,0.0027642883360385895,-0.023683425039052963,0.14680355787277222]},{"input":[true,8.0,0.17900291192141596,false,false,0.00011991113086210739,false,0.12],"torque":0.07777201018582891,"active":true,"saturated":false,"log":[-0.032109882682561874,-0.16376040875911713,-0.0012936709681525826,-0.0,-0.0724581703543663,0.07777201384305954,0.005525505170226097,-0.018130745738744736,0.1630173623561859]},{"input":[true,8.0,0.08042777751791175,false,false,0.00017970014996429066,false,0.12],"torque":0.0686705521240572,"active":true,"saturated":false,"log":[-0.028423966839909554,-0.14496223628520966,-0.001393154845573008,-0.0,-0.06335876137018204,0.06867054849863052,0.008280583657324314,-0.012660151347517967,0.17819814383983612]},{"input":[true,8.0,-0.017818193389245895,false,false,0.00023928952072016532,false,0.12],"torque":0.05981290437118993,"active":true,"saturated":false,"log":[-0.024835504591464996,-0.12666107714176178,-0.0014800791395828128,-0.0,-0.054504673928022385,0.05981290340423584,0.011026461608707905,-0.00727055175229907,0.19238875806331635]},{"input":[true,8.0,-0.11562584871739578,false,false,0.000298613038848147,false,0.12],"torque":0.051187541822798,"active":true,"saturated":false,"log":[-0.021339435130357742,-0.10883112251758575,-0.0015547671355307102,-0.0,-0.04588958993554115,0.05118754133582115,0.013760088942945004,-0.001961271045729518,0.20562900602817535]},{"input":[true,8.0,-0.21288652335609465,false,false,0.00035760479543111017,false,0.12],"torque":0.04279353796391231,"active":true,"saturated":false,"log":[-0.017931228503584862,-0.09144926816225052,-0.0016175264026969671,-0.0,-0.037508368492126465,0.04279353842139244,0.01647842861711979,0.0032679904252290726,0.2179558128118515]},{"input":[true,8.0,-0.30949215989494877,false,false,0.0004161992501417374,false,0.12],"torque":0.03461418258085366,"active":true,"saturated":false,"log":[-0.014606849290430546,-0.07449492812156677,-0.0016686504241079092,-0.0,-0.029355574399232864,0.03461418300867081,0.01917846128344536,0.00841718539595604,0.22940349578857422]},{"input":[true,8.0,-0.4053354286762873,false,false,0.0004743313040583631,false,0.12],"torque":0.026635889466563516,"active":true,"saturated":false,"log":[-0.011362718418240547,-0.057949863374233246,-0.0017084198771044612,-0.0,-0.021427558735013008,0.02663588896393776,0.021857187151908875,0.013485953211784363,0.2400040179491043]},{"input":[true,8.0,-0.500309847039295,false,false,0.0005319363719904111,false,0.12],"torque":0.01885233210184727,"active":true,"saturated":false,"log":[-0.008195674978196621,-0.0417979434132576,-0.0017371047288179398,-0.0,-0.013721751980483532,0.01885233260691166,0.024511627852916718,0.01847364380955696,0.2497871220111847]},{"input":[true,8.0,-0.5943098976231271,false,false,0.000588950454233074,false,0.12],"torque":0.011258158673182455,"active":true,"saturated":false,"log":[-0.005102947354316711,-0.02602503076195717,-0.0017549650510773063,-0.0,-0.0062362272292375565,0.011258158832788467,0.027138836681842804,0.023379351943731308,0.2587805390357971]},{"input":[true,8.0,-0.6872311455975635,false,false,0.0006453102076715109,false,0.12],"torque":0.0038488060551476405,"active":true,"saturated":false,"log":[-0.002082123653963208,-0.010618831031024456,-0.0017622525338083506,-0.0,0.0010303730377927423,0.0038488060235977173,0.02973589487373829,0.028201935812830925,0.2670102119445801]},{"input":[true,8.0,-0.7789703546909665,false,false,0.000700953016155571,false,0.12],"torque":-0.0033795558647147024,"active":true,"saturated":false,"log":[0.0008688793750479817,0.004431284964084625,-0.0017592114163562655,-0.0,0.008078915998339653,-0.0033795558847486973,0.032299917191267014,0.03294004499912262,0.27450037002563477]},{"input":[true,8.0,-0.8694256018866282,false,false,0.0007558170600668505,false,0.12],"torque":-0.010395625054446403,"active":true,"saturated":false,"log":[0.003751840442419052,0.019134385511279106,-0.001746080000884831,-0.0,0.01490984857082367,-0.010395624674856663,0.034828051924705505,0.03759213536977768,0.2812737226486206]},{"input":[true,8.0,-0.9584963906600825,false,false,0.0008098413850007994,false,0.12],"torque":-0.017271067129697244,"active":true,"saturated":false,"log":[0.006568259559571743,0.0334981232881546,-0.0017230911180377007,-0.0,0.021523259580135345,-0.017271067947149277,0.03731749206781387,0.042156513780355453,0.28735166788101196]},{"input":[true,8.0,-1.0460837626315698,false,false,0.0008629659694875654,false,0.12],"torque":-0.02397416326187912,"active":true,"saturated":false,"log":[0.009319380857050419,0.047528840601444244,-0.001690473291091621,-0.0,0.02791893109679222,-0.023974163457751274,0.03976547345519066,0.04663132131099701,0.29275426268577576]},{"input":[true,8.0,-1.132090407509612,false,false,0.000915131791676342,false,0.12],"torque":-0.030505681867144672,"active":true,"saturated":false,"log":[0.012006213888525963,0.061231691390275955,-0.0016484515508636832,-0.0,0.03409639745950699,-0.030505681410431862,0.042169272899627686,0.05101458728313446,0.29750049114227295]},{"input":[true,8.0,-1.2164207712035484,false,false,0.0009662808949091303,false,0.12],"torque":-0.036866701191045056,"active":true,"saturated":false,"log":[0.014629554934799671,0.07461073249578476,-0.0015972480177879333,-0.0,0.04005497321486473,-0.036866702139377594,0.044526223093271255,0.05530422925949097,0.30160826444625854]},{"input":[true,8.0,-1.2989811619849165,false,false,0.0010163564521110636,false,0.12],"torque":-0.043057901834816115,"active":true,"saturated":false,"log":[0.017190005630254745,0.08766902983188629,-0.0015370830660685897,-0.0,0.0457938089966774,-0.04305790364742279,0.04683370515704155,0.05949806421995163,0.3050946593284607]},{"input":[true,8.0,-1.3796798545797342,false,false,0.0010653028289257553,false,0.12],"torque":-0.049079591031083204,"active":true,"saturated":false,"log":[0.019687993451952934,0.10040877014398575,-0.0014681750908493996,-0.0,0.0513119101524353,-0.049079589545726776,0.049089156091213226,0.06359384953975677,0.3079758286476135]},{"input":[true,8.0,-1.4584271920760348,false,false,0.0011130656455255266,false,0.12],"torque":-0.05493172368347496,"active":true,"saturated":false,"log":[0.022123780101537704,0.11283127963542938,-0.0013907417887821794,-0.0,0.0566081777215004,-0.054931722581386566,0.05129006505012512,0.06758927553892136,0.31026726961135864]},{"input":[true,8.0,-1.5351356855334333,false,false,0.0011595918370278437,false,0.12],"torque":-0.06061395362287513,"active":true,"saturated":false,"log":[0.024497488513588905,0.12493719160556793,-0.0013050006236881018,-0.0,0.06168143078684807,-0.060613952577114105,0.053433991968631744,0.07148198038339615,0.31198376417160034]},{"input":[true,8.0,-1.609720111184061,false,false,0.0012048297124508391,false,0.12],"torque":-0.0661256553101686,"active":true,"saturated":false,"log":[0.02680911123752594,0.1367264688014984,-0.001211168710142374,-0.0,0.06653042882680893,-0.06612565368413925,0.05551855266094208,0.07526957988739014,0.31313955783843994]},{"input":[true,8.0,-1.6820976051168717,false,false,0.0012487290121424167,false,0.12],"torque":-0.07146595278186679,"active":true,"saturated":false,"log":[0.02905852347612381,0.1481984704732895,-0.0011094638612121344,-0.0,0.0711539089679718,-0.07146595418453217,0.05754143372178078,0.07894966006278992,0.31374841928482056]},{"input":[true,8.0,-1.7521877553401313,false,false,0.001291240963619141,false,0.12],"torque":-0.07663375227248942,"active":true,"saturated":false,"log":[0.03124549798667431,0.15935203433036804,-0.0010001047048717737,-0.0,0.07555058598518372,-0.0766337513923645,0.05950038507580757,0.08251982182264328,0.31382355093955994]},{"input":[true,8.0,-1.8199126911197998,false,false,0.0013323183357528668,false,0.12],"torque":-0.08162778155081046,"active":true,"saturated":false,"log":[0.033369723707437515,0.17018559575080872,-0.000883310625795275,-0.0,0.0797191932797432,-0.08162777870893478,0.06139322742819786,0.08597764372825623,0.31337791681289673]},{"input":[true,8.0,-1.8851971694945628,false,false,0.0013719154912449113,false,0.12],"torque":-0.0864465964180381,"active":true,"saturated":false,"log":[0.03543080389499664,0.18069709837436676,-0.0007593028130941093,-0.0,0.08365848660469055,-0.0864465981721878,0.06321786344051361,0.08932073414325714,0.31242403388023376]},{"input":[true,8.0,-1.9479686588713758,false,false,0.0014099884373294702,false,0.12],"torque":-0.09108862039911172,"active":true,"saturated":false,"log":[0.037428274750709534,0.19088420271873474,-0.0006283038528636098,-0.0,0.08736726641654968,-0.09108862280845642,0.06497226655483246,0.0925467237830162,0.31097421050071716]},{"input":[true,8.0,-2.0081574196086636,false,false,0.0014464948746499401,true,0.12],"torque":-0.09555217644095272,"active":true,"saturated":false,"log":[0.0393616184592247,0.20074425637722015,-0.0004905381938442588,-0.0,0.09084439277648926,-0.09555217623710632,0.06665448099374771,0.09565329551696777,0.30904048681259155]},{"input":[true,8.0,-2.065696581497637,false,false,0.0014813942442538548,false,0.12],"torque":-0.09983550644681176,"active":true,"saturated":false,"log":[0.04123026877641678,0.2102743685245514,-0.00034623226383700967,-0.0,0.09408882260322571,-0.09983550757169724,0.06826264411211014,0.0986381396651268,0.30663469433784485]},{"input":[true,8.0,-2.120522218055646,false,false,0.0015146477726542137,false,0.12],"torque":-0.10393678751487452,"active":true,"saturated":false,"log":[0.04303361475467682,0.21947143971920013,-0.00019561460067052394,-0.0,0.09709957987070084,-0.10393678396940231,0.06979496777057648,0.101499043405056,0.3037685751914978]},{"input":[true,8.0,-2.172573417549024,false,false,0.0015462185149071458,false,0.12],"torque":-0.10785418461706325,"active":true,"saturated":false,"log":[0.04477102681994438,0.22833223640918732,-3.8916008634259924e-05,-0.0,0.099875807762146,-0.10785418748855591,0.07124975323677063,0.10423382371664047,0.30045366287231445]},{"input":[true,8.0,-2.221792350666532,false,false,0.0015760713956580457,false,0.12],"torque":-0.11158583669240393,"active":true,"saturated":false,"log":[0.04644184187054634,0.23685339093208313,0.00012363043788354844,-0.0,0.1024167612195015,-0.111585833132267,0.07262536883354187,0.10684037953615189,0.29670146107673645]},{"input":[true,8.0,-2.2681243347681947,false,false,0.0016041732481105837,false,0.12],"torque":-0.115129901780723,"active":true,"saturated":false,"log":[0.048045385628938675,0.2450314611196518,0.0002917892998084426,-0.0,0.10472184419631958,-0.11512990295886993,0.07392030209302902,0.10931668430566788,0.29252341389656067]},{"input":[true,8.0,-2.3115178946381647,false,false,0.001630492850875286,false,0.12],"torque":-0.11848456630867493,"active":true,"saturated":false,"log":[0.04958097264170647,0.25286296010017395,0.0004653226933442056,-0.0,0.10679057985544205,-0.11848456412553787,0.07513310760259628,0.11166080832481384,0.2879308760166168]},{"input":[true,8.0,-2.3519248196741134,false,false,0.0016550009626567592,false,0.12],"torque":-0.12164808338904765,"active":true,"saturated":false,"log":[0.051047924906015396,0.26034441590309143,0.0006439904100261629,-0.0,0.10862264037132263,-0.12164808064699173,0.0762624442577362,0.11387088149785995,0.2829352021217346]},{"input":[true,8.0,-2.3893002174496036,false,false,0.0016776703547410072,false,0.12],"torque":-0.12461875597971281,"active":true,"saturated":false,"log":[0.052445556968450546,0.2674723267555237,0.0008275498985312879,-0.0,0.1102178692817688,-0.12461875379085541,0.07730705291032791,0.11594516038894653,0.27754783630371094]},{"input":[true,8.0,-2.4236025635899496,false,false,0.0016984758412467596,false,0.12],"torque":-0.12739498514494585,"active":true,"saturated":false,"log":[0.05377320200204849,0.2742433249950409,0.0010157560463994741,-0.0,0.11157626658678055,-0.12739498913288116,0.07826576381921768,0.11788199096918106,0.2717801630496979]},{"input":[true,8.0,-2.454793747906133,false,false,0.0017173943071071896,false,0.12],"torque":-0.1299752700547123,"active":true,"saturated":false,"log":[0.055030208081007004,0.28065407276153564,0.0012083618203178048,-0.0,0.11269798874855042,-0.12997527420520782,0.07913752645254135,0.11967983096837997,0.2656436562538147]},{"input":[true,8.0,-2.4828391167355424,false,false,0.0017344047337509473,false,0.12],"torque":-0.13235821820956042,"active":true,"saturated":false,"log":[0.05621594190597534,0.2867012917995453,0.001405117567628622,-0.0,0.11358338594436646,-0.13235822319984436,0.07992137223482132,0.12133723497390747,0.2591497600078583]},{"input":[true,8.0,-2.5077075114424763,false,false,0.0017494882224539628,false,0.12],"torque":-0.13454255507116594,"active":true,"saturated":false,"log":[0.05732979252934456,0.2923819422721863,0.0016057718312367797,-0.0,0.11423297226428986,-0.1345425546169281,0.08061641454696655,0.1228528842329979,0.2523101270198822]},{"input":[true,8.0,-2.5293713030356417,false,false,0.0017626280153360862,false,0.12],"torque":-0.13652715741515273,"active":true,"saturated":false,"log":[0.05837118998169899,0.29769307374954224,0.0018100710585713387,-0.0,0.11464743316173553,-0.1365271508693695,0.08122190088033676,0.12422559410333633,0.2451363503932953]},{"input":[true,8.0,-2.547806422864194,false,false,0.0017738095139792282,false,0.12],"torque":-0.1383110314750462,"active":true,"saturated":false,"log":[0.05933959037065506,0.3026319146156311,0.002017759718000889,-0.0,0.11482764780521393,-0.13831102848052979,0.08173714578151703,0.12545427680015564,0.23764021694660187]},{"input":[true,8.0,-2.5629923893582034,false,false,0.0017830202956463255,false,0.12],"torque":-0.13989332594863865,"active":true,"saturated":false,"log":[0.06023448333144188,0.30719587206840515,0.002228580415248871,-0.0,0.11477465182542801,-0.1398933231830597,0.08216157555580139,0.1265380084514618,0.22983354330062866]},{"input":[true,8.0,-2.57491233078385,false,false,0.0017902501270831014,false,0.12],"torque":-0.1412733637570088,"active":true,"saturated":false,"log":[0.06105541065335274,0.31138259172439575,0.0024422742426395416,-0.0,0.11448968201875687,-0.1412733644247055,0.08249472826719284,0.12747596204280853,0.22172825038433075]},{"input":[true,8.0,-2.5835530039880577,false,false,0.001795490975887298,false,0.12],"torque":-0.14245061879254556,"active":true,"saturated":false,"log":[0.06180195137858391,0.3151899576187134,0.0026585811283439398,-0.0,0.11397413909435272,-0.14245061576366425,0.08273622393608093,0.12826745212078094,0.21333640813827515]},{"input":[true,8.0,-2.588904809111748,false,false,0.0017987370194327387,false,0.12],"torque":-0.1434247462882244,"active":true,"saturated":false,"log":[0.06247374042868614,0.3186160624027252,0.0028772391378879547,-0.0,0.11322961002588272,-0.14342474937438965,0.08288580179214478,0.12891195714473724,0.20467014610767365]},{"input":[true,8.0,-2.590961800255359,false,false,0.0017999846513383159,false,0.12],"torque":-0.1441955591839027,"active":true,"saturated":false,"log":[0.06307045370340347,0.3216593265533447,0.0030979858711361885,-0.0,0.11225784569978714,-0.144195556640625,0.08294329047203064,0.129409059882164,0.1957417130470276]},{"input":[true,8.0,-2.5897216920847925,false,false,0.001799232485474709,false,0.12],"torque":-0.14476306446451118,"active":true,"saturated":false,"log":[0.06359183043241501,0.3243183493614197,0.003320557065308094,-0.0,0.11106079071760178,-0.1447630673646927,0.08290863037109375,0.12975852191448212,0.18656346201896667]},{"input":[true,8.0,-2.585185862370438,false,false,0.0017964813575043867,false,0.12],"torque":-0.145127427395322,"active":true,"saturated":false,"log":[0.06403765082359314,0.32659202814102173,0.0035446889232844114,-0.0,0.10964054614305496,-0.14512743055820465,0.08278185874223709,0.12996019423007965,0.17714786529541016]},{"input":[true,8.0,-2.5773593504564576,false,false,0.001791734323953177,false,0.12],"torque":-0.14528903218270267,"active":true,"saturated":false,"log":[0.06440777331590652,0.3284796476364136,0.003770116250962019,-0.0,0.10799942165613174,-0.14528903365135193,0.082563117146492,0.13001413643360138,0.16750748455524445]},{"input":[true,8.0,-2.566250851662032,false,false,0.0017849966588144434,false,0.12],"torque":-0.14524824246885304,"active":true,"saturated":false,"log":[0.064702108502388,0.3299807608127594,0.003996573388576508,-0.0,0.10613986849784851,-0.14524824917316437,0.08225264400243759,0.12992049753665924,0.1576550006866455]},{"input":[true,8.0,-2.551872707620794,false,false,0.0017762758476896352,false,0.12],"torque":-0.14500519374336424,"active":true,"saturated":false,"log":[0.06492061167955399,0.3310951292514801,0.004223795607686043,-0.0,0.10406455397605896,-0.14500519633293152,0.08185078948736191,0.1296796202659607,0.14760316908359528]},{"input":[true,8.0,-2.534240892569165,false,false,0.0017655815794717213,false,0.12],"torque":-0.14500993064443607,"active":true,"saturated":false,"log":[0.06506332755088806,0.3318229615688324,0.004451517481356859,-0.0,0.1031603291630745,-0.14500993490219116,0.08135800063610077,0.12929198145866394,0.13736487925052643]},{"input":[true,8.0,-2.513374995598854,false,false,0.0017529257355807513,false,0.12],"torque":-0.14493398802823254,"active":true,"saturated":false,"log":[0.06513035297393799,0.33216479420661926,0.004679473582655191,-0.0,0.1023871973156929,-0.14493398368358612,0.08077482134103775,0.12875817716121674,0.12695305049419403]},{"input":[true,8.0,-2.489298198893209,false,false,0.0017383223767634996,false,0.12],"torque":-0.1446703920324368,"active":true,"saturated":false,"log":[0.0651218444108963,0.3321214020252228,0.004907399881631136,-0.0,0.10143302381038666,-0.14467039704322815,0.08010189235210419,0.12807898223400116,0.11638074368238449]},{"input":[true,8.0,-2.462037251971625,false,false,0.00172178772747186,false,0.12],"torque":-0.14421802428558306,"active":true,"saturated":false,"log":[0.0650380477309227,0.3316940367221832,0.005135033279657364,0.0,0.10029907524585724,-0.14421802759170532,0.07933998107910156,0.12725533545017242,0.1056610494852066]},{"input":[true,8.0,-2.431622441970606,false,false,0.001703340157837346,false,0.12],"torque":-0.14357784849150143,"active":true,"saturated":false,"log":[0.06487925350666046,0.33088418841362,0.005362110678106546,0.0,0.09898677468299866,-0.14357784390449524,0.07848991453647614,0.1262882798910141,0.09480715543031693]},{"input":[true,8.0,-2.3980875599945066,false,false,0.001683000163261724,false,0.12],"torque":-0.14275107564502867,"active":true,"saturated":false,"log":[0.06464582681655884,0.3296937048435211,0.005588370840996504,0.0,0.09749773144721985,-0.142751082777977,0.07755264639854431,0.12517903745174408,0.08383229374885559]},{"input":[true,8.0,-2.3614698635733364,false,false,0.0016607903416464523,false,0.12],"torque":-0.1417391424235849,"active":true,"saturated":false,"log":[0.06433821469545364,0.32812488079071045,0.005813554860651493,0.0,0.09583372622728348,-0.1417391449213028,0.07652921974658966,0.1239289864897728,0.07274975627660751]},{"input":[true,8.0,-2.3218100352693405,false,false,0.001636735368286227,false,0.12],"torque":-0.1405436733810408,"active":true,"saturated":false,"log":[0.06395692378282547,0.3261803090572357,0.006037403829395771,0.0,0.09399673342704773,-0.14054366946220398,0.07542076706886292,0.12253962457180023,0.06157287582755089]},{"input":[true,8.0,-2.2791521374783335,false,false,0.0016108619684545263,false,0.12],"torque":-0.1391664794140402,"active":true,"saturated":false,"log":[0.06350252032279968,0.32386285066604614,0.006259662564843893,0.0,0.09198890626430511,-0.1391664743423462,0.07422851771116257,0.1210126057267189,0.0503150150179863]},{"input":[true,8.0,-2.233543563476017,false,false,0.001583198887711611,false,0.12],"torque":-0.13760961798878982,"active":true,"saturated":false,"log":[0.06297566741704941,0.3211759030818939,0.006480077747255564,0.0,0.08981255441904068,-0.13760961592197418,0.07295380532741547,0.11934974789619446,0.038989562541246414]},{"input":[true,8.0,-2.185034984763661,false,false,0.0015537768599679727,false,0.12],"torque":-0.13587530641831008,"active":true,"saturated":false,"log":[0.062377072870731354,0.31812307238578796,0.00669839745387435,0.0,0.08747020363807678,-0.13587529957294464,0.07159803807735443,0.1175529807806015,0.027609923854470253]},{"input":[true,8.0,-2.133680294771648,false,false,0.0015226285733387025,false,0.12],"torque":-0.13396598566995976,"active":true,"saturated":false,"log":[0.06170753017067909,0.314708411693573,0.006914373487234116,0.0,0.08496451377868652,-0.13396598398685455,0.07016272842884064,0.11562439799308777,0.01618950255215168]},{"input":[true,8.0,-2.0795365489834268,true,false,0.0014897886338267289,false,0.12],"torque":-0.13177504161454417,"active":true,"saturated":false,"log":[0.06096789985895157,0.31093630194664,0.006914373487234116,0.0,0.08229836076498032,-0.13177503645420074,0.06864946335554123,0.11356622725725174,0.004741694778203964]},{"input":[true,8.0,-2.0226639015464,true,false,0.001455293526875262,false,0.12],"torque":-0.1294528859071597,"active":true,"saturated":false,"log":[0.0601591132581234,0.30681148171424866,0.006914373487234116,0.0,0.07947953045368195,-0.1294528841972351,0.06705992668867111,0.11138083040714264,-0.006720121018588543]},{"input":[true,8.0,-1.963125538440177,true,false,0.0014191815768321676,false,0.12],"torque":-0.1269663576410134,"active":true,"saturated":false,"log":[0.05928216129541397,0.30233901739120483,0.006914373487234116,0.0,0.07650972157716751,-0.12696635723114014,0.06539588421583176,0.10907071828842163,-0.018182598054409027]},{"input":[true,8.0,-1.900987607276423,true,false,0.0013814929043712983,false,0.12],"torque":-0.12431764335092213,"active":true,"saturated":false,"log":[0.05833812057971954,0.29752442240715027,0.006914373487234116,0.0,0.0733889639377594,-0.12431764602661133,0.0636591911315918,0.10663852095603943,-0.029632432386279106]},{"input":[true,8.0,-1.8363191438083206,true,false,0.0013422693819180966,false,0.12],"torque":-0.12151018640212334,"active":true,"saturated":false,"log":[0.05732811614871025,0.292373389005661,0.006914373487234116,0.0,0.07012083381414413,-0.12151018530130386,0.06185177341103554,0.10408700257539749,-0.041056372225284576]},{"input":[true,8.0,-1.7691919952312787,true,false,0.0013015545871289842,false,0.12],"torque":-0.11854762528350424,"active":true,"saturated":false,"log":[0.05625335872173309,0.2868921160697937,0.006914373487234116,0.0,0.06670906394720078,-0.11854762583971024,0.05997563526034355,0.10141906142234802,-0.05244123190641403]},{"input":[true,8.0,-1.6996807403601109,true,false,0.0012593937544762292,false,0.12],"torque":-0.11543373087192096,"active":true,"saturated":false,"log":[0.05511511117219925,0.28108707070350647,0.006914373487234116,0.0,0.06315752118825912,-0.11543373018503189,0.05803286284208298,0.09863771498203278,-0.06377390772104263]},{"input":[true,8.0,-1.6278626067713529,true,false,0.001215833724992072,false,0.12],"torque":-0.11217243720601441,"active":true,"saturated":false,"log":[0.05391471087932587,0.2749650180339813,0.006914373487234116,0.0,0.059470243752002716,-0.1121724396944046,0.05602561682462692,0.09574609994888306,-0.07504138350486755]},{"input":[true,8.0,-1.5538173850027899,true,false,0.0011709228942279536,false,0.12],"torque":-0.10876780966039624,"active":true,"saturated":false,"log":[0.05265355482697487,0.2685331404209137,0.006914373487234116,0.0,0.05565137788653374,-0.1087678074836731,0.053956128656864166,0.09274747967720032,-0.08623073995113373]},{"input":[true,8.0,-1.4776273399055144,true,false,0.0011247111584866602,false,0.12],"torque":-0.10522404440259567,"active":true,"saturated":false,"log":[0.05133310332894325,0.26179882884025574,0.006914373487234116,0.0,0.051705218851566315,-0.10522404313087463,0.05182668939232826,0.08964522927999496,-0.09732917696237564]},{"input":[true,8.0,-1.3993771192470006,true,false,0.0010772498593871215,false,0.12],"torque":-0.10154546180212681,"active":true,"saturated":false,"log":[0.04995488002896309,0.254769891500473,0.006914373487234116,0.0,0.047636184841394424,-0.10154546052217484,0.04963967204093933,0.08644283562898636,-0.10832402110099792]},{"input":[true,8.0,-1.3191536596667321,true,false,0.0010285917268234453,false,0.12],"torque":-0.09773649397423326,"active":true,"saturated":false,"log":[0.048520468175411224,0.24745438992977142,0.006914373487234116,0.0,0.043448809534311295,-0.09773649275302887,0.04739750549197197,0.08314389735460281,-0.11920274794101715]},{"input":[true,8.0,-1.2370460900888813,true,false,0.0009787908203815724,false,0.12],"torque":-0.09380168533569722,"active":true,"saturated":false,"log":[0.04703151434659958,0.23986072838306427,0.006914373487234116,0.0,0.039147742092609406,-0.09380168467760086,0.04510268196463585,0.079752117395401,-0.12995296716690063]},{"input":[true,8.0,-1.153145632699348,true,false,0.0009279024692786355,false,0.12],"torque":-0.08974565644721792,"active":true,"saturated":false,"log":[0.045489709824323654,0.2319975197315216,0.006914373487234116,0.0,0.03473773971199989,-0.0897456556558609,0.04275774583220482,0.07627128809690475,-0.14056245982646942]},{"input":[true,8.0,-1.0675455015971484,true,false,0.0008759832108917369,false,0.12],"torque":-0.08528530443557114,"active":true,"saturated":false,"log":[0.043896809220314026,0.22387371957302094,0.006914373487234116,0.0,0.029345745220780373,-0.08528530597686768,0.0403653085231781,0.07270532101392746,-0.15101918578147888]},{"input":[true,8.0,-0.9803407992327922,true,false,0.0008230907279444615,false,0.12],"torque":-0.08044276778709497,"active":true,"saturated":false,"log":[0.04225463047623634,0.21549861133098602,0.006914373487234116,0.0,0.02302870713174343,-0.08044277131557465,0.0379280224442482,0.06905819475650787,-0.1613112837076187]},{"input":[true,8.0,-0.89162841074867,false,false,0.0007692837844208935,true,0.12],"torque":-0.07519473564323115,"active":true,"saturated":false,"log":[0.04056502506136894,0.20688162744045258,0.005673476494848728,0.0,0.01664591208100319,-0.07519473880529404,0.0354485958814621,0.06533398479223251,-0.17142710089683533]},{"input":[true,8.0,-0.8015068963388725,false,false,0.0007146221602783498,false,0.12],"torque":-0.07020320696375695,"active":true,"saturated":false,"log":[0.038829904049634933,0.19803251326084137,0.005809381138533354,0.0,0.010204305872321129,-0.07020320743322372,0.032929789274930954,0.061536870896816254,-0.18135519325733185]},{"input":[true,8.0,-0.7100763817479909,false,false,0.0006591665850313498,false,0.12],"torque":-0.06512988970817402,"active":true,"saturated":false,"log":[0.037051230669021606,0.18896128237247467,0.005939060356467962,0.0,0.0037108890246599913,-0.06512989103794098,0.030374396592378616,0.05767107754945755,-0.1910843402147293]},{"input":[true,8.0,-0.6174384470305996,false,false,0.0006029786702806292,false,0.12],"torque":-0.05997406383004729,"active":true,"saturated":false,"log":[0.035231009125709534,0.17967814207077026,0.0060623688623309135,0.0,-0.002827279968187213,-0.0599740631878376,0.02778525836765766,0.053740933537483215,-0.20060355961322784]},{"input":[true,8.0,-1.4365999163482108,false,false,0.000546120841262141,false,0.12],"torque":0.010422273157429721,"active":true,"saturated":false,"log":[-0.0012612525606527925,-0.006432387977838516,0.0060579548589885235,-0.0,-0.035499293357133865,0.01042227353900671,0.0506800152361393,0.04975081607699394,-0.2099020928144455]},{"input":[true,8.0,-1.2457984776803503,false,false,0.0004886562674921086,false,0.12],"torque":0.010093411317133672,"active":true,"saturated":false,"log":[0.00048576414701528847,0.002477397210896015,0.006059654988348484,0.0,-0.03936793655157089,0.010093411430716515,0.04534730315208435,0.04570517688989639,-0.218969464302063]},{"input":[true,8.0,-1.0531944272261218,false,false,0.00043064879258516834,false,0.12],"torque":0.008463718163541988,"active":true,"saturated":false,"log":[0.0022319317795336246,0.011382851749658585,0.006067466456443071,0.0,-0.04323224723339081,0.008463717997074127,0.039964206516742706,0.0416085347533226,-0.22779546678066254]},{"input":[true,8.0,-0.859001749671533,false,false,0.0003721628633235905,false,0.12],"torque":0.0068107863451862295,"active":true,"saturated":false,"log":[0.003975332248955965,0.020274193957448006,0.006081380415707827,0.0,-0.047087833285331726,0.006810786202549934,0.03453671559691429,0.037465453147888184,-0.23637014627456665]},{"input":[true,8.0,-0.6634361946803775,false,false,0.0003132634580563725,false,0.12],"torque":0.005159774566063578,"active":true,"saturated":false,"log":[0.005714048631489277,0.02914164774119854,0.006101379636675119,0.0,-0.050930313766002655,0.005159774795174599,0.02907084859907627,0.033280547708272934,-0.2446838617324829]},{"input":[true,8.0,-0.466715037194594,false,false,0.00025401601450776096,false,0.12],"torque":0.0035128207869728877,"active":true,"saturated":false,"log":[0.007446167524904013,0.03797545284032822,0.006127440836280584,0.0,-0.054755304008722305,0.0035128206945955753,0.023572687059640884,0.029058486223220825,-0.2527272701263428]},{"input":[true,8.0,-0.2690568360400009,false,false,0.00019448635707539666,false,0.12],"torque":0.0018716344326893352,"active":true,"saturated":false,"log":[0.009169782511889935,0.04676588997244835,0.006159535143524408,0.0,-0.05855845287442207,0.0018716343911364675,0.01804833486676216,0.024803970009088516,-0.260491281747818]},{"input":[true,8.0,-0.07068119110564713,false,false,0.00013474062369886897,false,0.12],"torque":0.00023791319340784914,"active":true,"saturated":false,"log":[0.010882995091378689,0.055503275245428085,0.006197625771164894,0.0,-0.06233542412519455,0.00023791319108568132,0.012503930367529392,0.02052173763513565,-0.26796722412109375]},{"input":[true,8.0,0.12819150063346238,false,false,7.484519237992288e-05,false,0.12],"torque":-0.001386648590907314,"active":true,"saturated":false,"log":[0.012583916075527668,0.0641779750585556,0.006241669412702322,0.0,-0.06608191132545471,-0.0013866486260667443,0.006945634260773659,0.01621655933558941,-0.2751466929912567]},{"input":[true,8.0,0.3273402899792381,false,false,1.4866607435947705e-05,false,0.12],"torque":-0.002992461450411582,"active":true,"saturated":false,"log":[0.014270670711994171,0.07278042286634445,0.006291616708040237,0.0,-0.06979364901781082,-0.002992461435496807,0.0013796215644106269,0.011893224902451038,-0.2820216119289398]},{"input":[true,8.0,0.5265439209868732,false,false,-4.5128494431304356e-05,false,0.12],"torque":-0.004593526456535874,"active":true,"saturated":false,"log":[0.015941396355628967,0.08130112290382385,0.0063474117778241634,0.0,-0.07346642017364502,-0.004593526478856802,-0.004187923856079578,0.007556550670415163,-0.28858426213264465]},{"input":[true,8.0,0.725581076782026,false,false,-0.00010507345816964415,false,0.12],"torque":-0.006180583279529862,"active":true,"saturated":false,"log":[0.01759425178170204,0.08973068743944168,0.0064089917577803135,0.0,-0.07709604501724243,-0.0061805834993720055,-0.009750816971063614,0.0032113627530634403,-0.29482728242874146]},{"input":[true,8.0,0.9242306254456785,false,false,-0.0001649016844307542,false,0.12],"torque":-0.007752243008796462,"active":true,"saturated":false,"log":[0.019227409735322,0.09805978834629059,0.006476287730038166,0.0,-0.08067868649959564,-0.007752242963761091,-0.015302876010537148,-0.0011375034227967262,-0.30074363946914673]},{"input":[true,8.0,1.122271865693535,false,false,-0.0002245467035626178,false,0.12],"torque":-0.009308235403740062,"active":true,"saturated":false,"log":[0.020839067175984383,0.10627923905849457,0.006549224257469177,0.0,-0.08421075344085693,-0.009308235719799995,-0.0208379328250885,-0.005485208239406347,-0.3063267469406128]},{"input":[true,8.0,1.3194847720769574,false,false,-0.00028394224945784677,false,0.12],"torque":-0.010845981515085588,"active":true,"saturated":false,"log":[0.02242744155228138,0.11437994986772537,0.0066277203150093555,0.0,-0.08768739551305771,-0.010845981538295746,-0.026349840685725212,-0.00982691440731287,-0.31157031655311584]},{"input":[true,8.0,1.5156502394330522,false,true,-0.0003430223331758737,false,0.12],"torque":-0.012337043087846826,"active":true,"saturated":false,"log":[0.023990780115127563,0.1223529800772667,0.0066277203150093555,0.0,-0.09110471606254578,-0.012337042950093746,-0.03183247148990631,-0.014157790690660477,-0.3164684474468231]},{"input":[true,8.0,1.7105503263123043,false,true,-0.00040172131625720533,false,0.12],"torque":-0.01380425128529488,"active":true,"saturated":false,"log":[0.0255273524671793,0.13018949329853058,0.0066277203150093555,0.0,-0.09445884823799133,-0.013804251328110695,-0.03727973625063896,-0.01847301982343197,-0.32101568579673767]},{"input":[true,8.0,1.9039684971133346,false,true,-0.0004599739836482962,false,0.12],"torque":-0.015246611757204341,"active":true,"saturated":false,"log":[0.027035459876060486,0.1378808468580246,0.0066277203150093555,0.0,-0.09774604439735413,-0.015246612019836903,-0.04268558695912361,-0.022767802700400352,-0.3252069056034088]},{"input":[true,8.0,2.095689862655727,false,true,-0.000517715616156007,false,0.12],"torque":-0.016662597066944344,"active":true,"saturated":false,"log":[0.028513433411717415,0.14541850984096527,0.0066277203150093555,0.0,-0.10096261650323868,-0.01666259765625,-0.048044007271528244,-0.02703736163675785,-0.3290373980998993]},{"input":[true,8.0,2.285501418923683,false,true,-0.0005748820623511629,false,0.12],"torque":-0.018050693815457427,"active":true,"saturated":false,"log":[0.02995963953435421,0.15279416739940643,0.0066277203150093555,0.0,-0.10410497337579727,-0.018050692975521088,-0.05334905534982681,-0.031276948750019073,-0.33250290155410767]},{"input":[true,8.0,2.4731922837152243,false,true,-0.0006314098098413157,false,0.12],"torque":-0.019409407204776727,"active":true,"saturated":false,"log":[0.03137247636914253,0.15999962389469147,0.0066277203150093555,0.0,-0.10716962814331055,-0.019409406930208206,-0.058594830334186554,-0.03548184782266617,-0.33559948205947876]},{"input":[true,8.0,2.658553930934075,false,true,-0.0006872360558335404,false,0.12],"torque":-0.020737268188727058,"active":true,"saturated":false,"log":[0.03275037929415703,0.16702693700790405,0.0066277203150093555,0.0,-0.11015316843986511,-0.020737268030643463,-0.06377550214529037,-0.039647381752729416,-0.338323712348938]},{"input":[true,8.0,2.841380422263861,false,false,-0.0007422987769088523,false,0.12],"torque":-0.022070964700614173,"active":true,"saturated":false,"log":[0.03409182280302048,0.1738682985305786,0.006747041828930378,0.0,-0.11305228620767593,-0.02207096479833126,-0.06888532638549805,-0.04376892372965813,-0.34067249298095703]},{"input":[true,8.0,3.0214686359672682,false,false,-0.0007965367979307344,false,0.12],"torque":-0.02337332354806632,"active":true,"saturated":false,"log":[0.03539532795548439,0.18051616847515106,0.006870925426483154,0.0,-0.11586379259824753,-0.023373324424028397,-0.07391861081123352,-0.047841884195804596,-0.34264320135116577]},{"input":[true,8.0,3.1986184925559797,false,false,-0.000849889860011222,false,0.12],"torque":-0.024642071011965205,"active":true,"saturated":false,"log":[0.03665944188833237,0.18696315586566925,0.0069992332719266415,0.0,-0.11858458817005157,-0.02464207075536251,-0.0788697749376297,-0.05186174064874649,-0.34423360228538513]},{"input":[true,8.0,3.3726331770806297,false,false,-0.0009022986874590205,false,0.12],"torque":-0.025875788689917113,"active":true,"saturated":false,"log":[0.037882767617702484,0.19320210814476013,0.007131822872906923,0.0,-0.12121168524026871,-0.025875788182020187,-0.08373332023620605,-0.05582401901483536,-0.34544190764427185]},{"input":[true,8.0,3.5433193577938296,false,false,-0.0009537050536352881,false,0.12],"torque":-0.027073104464077208,"active":true,"saturated":false,"log":[0.0390639528632164,0.19922615587711334,0.0072685470804572105,0.0,-0.12374220043420792,-0.027073103934526443,-0.08850383013486862,-0.059724316000938416,-0.3462667763233185]},{"input":[true,8.0,3.710487400943326,false,false,-0.001004051845643908,false,0.12],"torque":-0.028232672253063248,"active":true,"saturated":false,"log":[0.040201690047979355,0.20502862334251404,0.007409252692013979,0.0,-0.12617337703704834,-0.028232673183083534,-0.09317600727081299,-0.06355829536914825,-0.3467072546482086]},{"input":[true,8.0,3.873951581456678,false,false,-0.0010532831277843902,false,0.12],"torque":-0.029353164166691887,"active":true,"saturated":false,"log":[0.04129471257328987,0.2106030285358429,0.007553784176707268,0.0,-0.12850254774093628,-0.02935316413640976,-0.09774467349052429,-0.06732170283794403,-0.3467628061771393]},{"input":[true,8.0,4.033530289283329,false,false,-0.001101344203696894,false,0.12],"torque":-0.03043329969666385,"active":true,"saturated":false,"log":[0.04234181344509125,0.21594324707984924,0.0077019804157316685,0.0,-0.13072721660137177,-0.030433299019932747,-0.10220474004745483,-0.07101033627986908,-0.3464333713054657]},{"input":[true,8.0,2.0368681429823012,false,false,-0.0005,false,0.12],"torque":0.10046148897974629,"active":true,"saturated":false,"log":[-0.034356798976659775,-0.17521966993808746,0.007581731770187616,-0.0,-0.14628872275352478,0.10046149045228958,-0.04639999940991402,-0.07171161472797394,-0.3214818239212036]},{"input":[true,8.0,2.0331802518671784,false,false,-0.0004988893003505612,false,0.12],"torque":0.1033144527302749,"active":true,"saturated":false,"log":[-0.03532862290740013,-0.18017597496509552,0.007458081468939781,-0.0,-0.14262518286705017,0.10331445187330246,-0.04629692807793617,-0.07232451438903809,-0.29610052704811096]},{"input":[true,8.0,2.022132963079285,false,false,-0.0004955621360170897,false,0.12],"torque":0.10405507991687778,"active":true,"saturated":false,"log":[-0.03644945099949837,-0.18589219450950623,0.007330508437007666,-0.0,-0.13887301087379456,0.10405507683753967,-0.045988164842128754,-0.07284149527549744,-0.27026060223579407]},{"input":[true,8.0,2.003775357497757,false,false,-0.0004900332889206208,false,0.12],"torque":0.10494054271514937,"active":true,"saturated":false,"log":[-0.037708066403865814,-0.1923111379146576,0.007198530249297619,-0.0,-0.13502143323421478,0.10494054108858109,-0.04547508805990219,-0.07325568050146103,-0.24393808841705322]},{"input":[true,8.0,1.9781889942669297,false,false,-0.00048232732261528193,false,0.12],"torque":0.10600090652846864,"active":true,"saturated":false,"log":[-0.039092887192964554,-0.19937372207641602,0.007061705458909273,-0.0,-0.1310598999261856,0.10600090771913528,-0.044759973883628845,-0.07356080412864685,-0.21711401641368866]},{"input":[true,8.0,1.9454875484454872,false,false,-0.00047247847315736883,false,0.12],"torque":0.10721415254938103,"active":true,"saturated":false,"log":[-0.04059199243783951,-0.2070191651582718,0.0069196331314742565,-0.0,-0.12697811424732208,0.10721415281295776,-0.0438460037112236,-0.07375126332044601,-0.18977421522140503]},{"input":[true,8.0,1.9058163059710693,false,false,-0.00046053049700144256,false,0.12],"torque":0.108556659959141,"active":true,"saturated":false,"log":[-0.04219312220811844,-0.21518492698669434,0.006771957501769066,-0.0,-0.12276637554168701,0.10855665802955627,-0.04273723065853119,-0.07382208853960037,-0.16190946102142334]},{"input":[true,8.0,1.859351518184112,false,false,-0.00044653647659921467,false,0.12],"torque":0.110650851180394,"active":true,"saturated":false,"log":[-0.043883729726076126,-0.22380702197551727,0.0066183642484247684,-0.0,-0.12043365836143494,0.11065085232257843,-0.04143858328461647,-0.07376895844936371,-0.1335153579711914]},{"input":[true,8.0,1.8062996187786418,false,false,-0.00043055858456490516,false,0.12],"torque":0.1137259012643198,"active":true,"saturated":false,"log":[-0.045650988817214966,-0.2328200489282608,0.006458585616201162,-0.0,-0.12071597576141357,0.11372590065002441,-0.03995583578944206,-0.07358819991350174,-0.10459230840206146]},{"input":[true,8.0,1.7468963066589442,false,false,-0.0004126678074548392,false,0.12],"torque":0.11690522812739303,"active":true,"saturated":false,"log":[-0.04748184606432915,-0.24215741455554962,0.006292399484664202,-0.0,-0.12092664837837219,0.11690522730350494,-0.03829557076096535,-0.07327678054571152,-0.07514551281929016]},{"input":[true,8.0,0.3,false,false,-0.00039294363038847404,false,0.12],"torque":0.21839735955293815,"active":true,"saturated":false,"log":[-0.10176897048950195,-0.51902174949646,0.005936208181083202,-0.0,-0.16039659082889557,0.2183973640203476,0.0021437557879835367,-0.07283230125904083,-0.04518482834100723]},{"input":[true,8.0,11.520726004670493,false,false,-0.00037147368391202205,false,0.12],"torque":-0.5791044770897088,"active":true,"saturated":false,"log":[0.32469442486763,1.6559416055679321,0.007072638254612684,0.0,0.14895287156105042,-0.579104483127594,-0.31146448850631714,-0.07225300371646881,-0.014724771492183208]},{"input":[true,8.0,22.566356332907066,false,false,-0.0003483533546735827,false,0.12],"torque":-0.8352573251178039,"active":true,"saturated":false,"log":[0.7970598936080933,4.065005302429199,0.007072638254612684,0.0,0.15043257176876068,-0.835257351398468,-0.6201789975166321,-0.0329626239836216,-0.008021863177418709]},{"input":[true,8.0,33.26452761774428,false,false,-0.0003236853616394762,false,0.12],"torque":-0.8408960726703208,"active":true,"saturated":false,"log":[1.2041682004928589,6.1412577629089355,0.007072638254612684,0.0,0.15201131999492645,-0.8408960700035095,-0.9191823601722717,-0.0320378802716732,-0.0009080310119315982]},{"input":[true,8.0,43.44829847437827,false,false,-0.0002975792997345639,false,0.12],"torque":-0.8406724548049617,"active":true,"saturated":false,"log":[1.59201180934906,8.11925983428955,0.007072638254612684,0.0,0.15368211269378662,-0.8406724333763123,-1.2038087844848633,-0.030928919091820717,0.006558808498084545]},{"input":[true,8.0,52.958754564641595,false,false,-0.00027015115293406986,false,0.12],"torque":-0.8402646942561977,"active":true,"saturated":false,"log":[1.9545515775680542,9.968213081359863,0.007072638254612684,0.0,0.15543751418590546,-0.8402646780014038,-1.4696166515350342,-0.029643608257174492,0.01432101521641016]},{"input":[true,8.0,61.647488402100066,false,false,-0.00024152277897013832,false,0.12],"torque":-0.8398107286822426,"active":true,"saturated":false,"log":[2.2861459255218506,11.659344673156738,0.007072638254612684,0.0,0.15726973116397858,-0.8398107290267944,-1.7124581336975098,-0.028190387412905693,0.022321350872516632]},{"input":[true,8.0,69.37891520124244,false,false,-0.0002118213679424723,false,0.12],"torque":-0.8393191908852679,"active":true,"saturated":false,"log":[2.581638813018799,13.16635799407959,0.007072638254612684,0.0,0.15917061269283295,-0.8393191695213318,-1.9285439252853394,-0.026578249409794807,0.030503112822771072]},{"input":[true,8.0,76.03238863271068,false,false,-0.00018117887723833681,false,0.12],"torque":-0.8387964607415196,"active":true,"saturated":false,"log":[2.836440324783325,14.465846061706543,0.007072638254612684,0.0,0.16113173961639404,-0.8387964367866516,-2.114501953125,-0.024816717952489853,0.038810282945632935]},{"input":[true,8.0,81.50408346891857,false,false,-0.00014973144527246395,true,0.12],"torque":-0.8382488814108872,"active":true,"saturated":false,"log":[3.0465986728668213,15.537652969360352,0.007072638254612684,0.0,0.16314436495304108,-0.8382489085197449,-2.267430543899536,-0.02291581593453884,0.04718765616416931]},{"input":[true,8.0,85.70861574200275,false,false,-0.00011761878665149471,false,0.12],"torque":-0.837682688426097,"active":true,"saturated":false,"log":[3.20885968208313,16.365184783935547,0.007072638254612684,0.0,0.16519957780838013,-0.8376826643943787,-2.3849430084228516,-0.020886028185486794,0.05558100342750549]},{"input":[true,8.0,88.580375132084,false,false,-8.498357145012052e-05,false,0.12],"torque":-0.8371039067893044,"active":true,"saturated":false,"log":[3.3207199573516846,16.935670852661133,0.007072638254612684,0.0,0.16728822886943817,-0.8371039032936096,-2.4652059078216553,-0.0187382735311985,0.0639372169971466]},{"input":[true,8.0,90.0745487943649,false,false,-5.197079135667549e-05,false,0.12],"torque":-0.8365182599310387,"active":true,"saturated":false,"log":[3.3804640769958496,17.240365982055664,0.007072638254612684,0.0,0.16940104961395264,-0.8365182876586914,-2.5069665908813477,-0.016483858227729797,0.07220445573329926]},{"input":[true,8.0,90.16782064858484,false,false,-1.872711550426666e-05,false,0.12],"torque":-0.8359310931768685,"active":true,"saturated":false,"log":[3.3871912956237793,17.274675369262695,0.007072638254612684,0.0,0.1715286523103714,-0.835931122303009,-2.509573459625244,-0.014134436845779419,0.08033232390880585]},{"input":[true,8.0,88.85873521865432,false,false,1.4599761150644409e-05,false,0.12],"torque":-0.8353473129697666,"active":true,"saturated":false,"log":[3.3408308029174805,17.038236618041992,0.007072638254612684,0.0,0.17366155982017517,-0.8353472948074341,-2.4729857444763184,-0.011701976880431175,0.08827198296785355]},{"input":[true,8.0,86.16772034487244,false,false,4.786177400718783e-05,false,0.12],"torque":-0.8347713424006045,"active":true,"saturated":false,"log":[3.242140531539917,16.534915924072266,0.007072638254612684,0.0,0.17579033970832825,-0.8347713351249695,-2.3977744579315186,-0.009198702871799469,0.0959763377904892]},{"input":[true,8.0,82.13676841431135,false,false,8.091114664128603e-05,false,0.12],"torque":-0.8342070929293586,"active":true,"saturated":false,"log":[3.0926966667175293,15.77275276184082,0.007072638254612684,0.0,0.17790549993515015,-0.8342071175575256,-2.2851133346557617,-0.006637056823819876,0.10340015590190887]},{"input":[true,8.0,76.82878108366067,false,false,0.00011360104734654355,false,0.12],"torque":-0.8336579515634763,"active":true,"saturated":false,"log":[2.8948683738708496,14.76382827758789,0.007072638254612684,0.0,0.17999765276908875,-0.83365797996521,-2.1367604732513428,-0.00402964698150754,0.11050022393465042]},{"input":[true,8.0,70.32658771991291,false,false,0.0001457862414779463,false,0.12],"torque":-0.8331267822320928,"active":true,"saturated":false,"log":[2.651780605316162,13.524081230163574,0.007072638254612684,0.0,0.18205749988555908,-0.8331267833709717,-1.9550304412841797,-0.0013891984708607197,0.11723549664020538]},{"input":[true,8.0,62.73165287579446,false,false,0.0001773237367001388,false,0.12],"torque":-0.8330943516921837,"active":true,"saturated":false,"log":[2.3672657012939453,12.073055267333984,0.007072638254612684,-0.0,0.18407590687274933,-0.8330943584442139,-1.7427594661712646,0.0012714965268969536,0.12356720119714737]},{"input":[true,8.0,54.16249296935608,false,false,0.0002080734182735712,false,0.12],"torque":-0.8336167638679584,"active":true,"saturated":false,"log":[2.0458016395568848,10.433588027954102,0.007072638254612684,-0.0,0.1860438883304596,-0.8336167931556702,-1.5032598972320557,0.003939630463719368,0.12945899367332458]},{"input":[true,8.0,44.752826874908024,false,false,0.0002378986715560677,false,0.12],"torque":-0.8341758048836772,"active":true,"saturated":false,"log":[1.6924445629119873,8.63146686553955,0.007072638254612684,-0.0,0.1879526972770691,-0.8341758251190186,-1.2402690649032593,0.006602431181818247,0.1348770260810852]},{"input":[true,8.0,34.64948928470985,false,false,0.00026666698895416686,false,0.12],"torque":-0.8347576717951097,"active":true,"saturated":false,"log":[1.3127474784851074,6.695012092590332,0.007072638254612684,-0.0,0.18979386985301971,-0.8347576856613159,-0.9578906893730164,0.00924721546471119,0.1397901475429535]},{"input":[true,8.0,24.010139402707875,false,false,0.0002942505586276729,false,0.12],"torque":-0.8353596220390581,"active":true,"saturated":false,"log":[0.9126742482185364,4.654638767242432,0.007072638254612684,-0.0,0.19155922532081604,-0.8353596329689026,-0.6605312824249268,0.011861439794301987,0.1441698968410492]},{"input":[true,8.0,13.00080072538805,false,false,0.0003205268323319191,false,0.12],"torque":-0.8359786737449084,"active":true,"saturated":false,"log":[0.49850669503211975,2.542384147644043,0.008817411959171295,-0.0,0.19324089586734772,-0.8359786868095398,-0.3528311252593994,0.014432751573622227,0.14799068868160248]},{"input":[true,8.0,-0.1927297695244199,false,false,0.00034537906987493823,false,0.12],"torque":-0.00933191036761358,"active":true,"saturated":false,"log":[0.0014034670311957598,0.007157681975513697,0.008822323754429817,-0.0,-0.034789733588695526,-0.009331909939646721,0.015915067866444588,0.016949040815234184,0.15122981369495392]},{"input":[true,8.0,-0.2311741880869846,false,false,0.0003686968577706227,false,0.12],"torque":0.0017365203505137468,"active":true,"saturated":false,"log":[0.003269781591370702,0.016675885766744614,0.008833767846226692,-0.0,-0.03145880997180939,0.0017365203239023685,0.01698955148458481,0.019398491829633713,0.15386755764484406]},{"input":[true,8.0,-0.26691793359167054,false,false,0.0003903765997836073,false,0.12],"torque":-0.002113539806849784,"active":true,"saturated":false,"log":[0.005132249090820551,0.026174470782279968,0.008851731196045876,-0.0,-0.02833741530776024,-0.0021135397255420685,0.017988553270697594,0.021769626066088676,0.15588727593421936]},{"input":[true,8.0,-0.29980220377607053,false,false,0.0004103219771864765,false,0.12],"torque":-0.006127745770358804,"active":true,"saturated":false,"log":[0.006981846410781145,0.035607416182756424,0.008876167237758636,-0.0,-0.025440890341997147,-0.006127745844423771,0.018907636404037476,0.024051358923316002,0.15727537870407104]},{"input":[true,8.0,-0.3296809004507219,false,false,0.00042844437668447364,false,0.12],"torque":-0.010055649212181125,"active":true,"saturated":false,"log":[0.008809659630060196,0.04492926597595215,0.008907001465559006,-0.0,-0.02278347685933113,-0.01005564909428358,0.01974271796643734,0.026233039796352386,0.1580214500427246]},{"input":[true,8.0,-0.35642127858393485,false,false,0.0004446632841065207,false,0.12],"torque":-0.013854047357105402,"active":true,"saturated":false,"log":[0.010606920346617699,0.05409529432654381,0.008944125846028328,-0.0,-0.02037825435400009,-0.013854047283530235,0.02049008384346962,0.02830450050532818,0.15811817348003387]},{"input":[true,8.0,-0.3799045360612365,false,false,0.0004589066421134661,false,0.12],"torque":-0.017504048835961582,"active":true,"saturated":false,"log":[0.01236504316329956,0.06306172162294388,0.008987403474748135,-0.0,-0.018237091600894928,-0.017504049465060234,0.021146418526768684,0.03025609254837036,0.15756143629550934]},{"input":[true,8.0,-0.40002634149923516,false,false,0.000471111170334329,false,0.12],"torque":-0.020988048555240318,"active":true,"saturated":false,"log":[0.014075654558837414,0.0717858374118805,0.00903666764497757,-0.0,-0.016370581462979317,-0.02098804898560047,0.02170880325138569,0.032078731805086136,0.1563502848148346]},{"input":[true,8.0,-0.41669729776894743,false,false,0.00048122264650824387,false,0.12],"torque":-0.02428928160215839,"active":true,"saturated":false,"log":[0.015730636194348335,0.08022624254226685,0.009091725572943687,-0.0,-0.014788009226322174,-0.02428928203880787,0.022174740210175514,0.03376394137740135,0.15448687970638275]},{"input":[true,8.0,-0.4298433391692344,false,false,0.0004891961473830436,false,0.12],"torque":-0.02739187110907088,"active":true,"saturated":false,"log":[0.017322150990366936,0.08834297209978104,0.009152352809906006,-0.0,-0.013497299514710903,-0.027391871437430382,0.022542158141732216,0.03530387207865715,0.1519765555858612]},{"input":[true,8.0,-0.43940606048579706,false,false,0.0004949962483002228,false,0.12],"torque":-0.030280898076785405,"active":true,"saturated":false,"log":[0.018842676654458046,0.09609764814376831,0.009218302555382252,-0.0,-0.012504998594522476,-0.03028089739382267,0.022809427231550217,0.03669135645031929,0.1488276720046997]},{"input":[true,8.0,-0.44534297647377935,false,false,0.0004985971805795595,false,0.12],"torque":-0.032977467206411176,"active":true,"saturated":false,"log":[0.020285042002797127,0.10345371067523956,0.009289300069212914,-0.0,-0.011706698685884476,-0.03297746554017067,0.022975359112024307,0.037919916212558746,0.14505167305469513]},{"input":[true,8.0,-0.44762771061115486,false,false,0.0004999829460041724,false,0.12],"torque":-0.03563134474650866,"active":true,"saturated":false,"log":[0.021642444655299187,0.11037646979093552,0.009365048259496689,-0.0,-0.010599744506180286,-0.035631343722343445,0.023039214313030243,0.03898381069302559,0.14066292345523834]},{"input":[true,8.0,-0.44625011228430206,false,false,0.0004991473878973766,false,0.12],"torque":-0.038069284669590274,"active":true,"saturated":false,"log":[0.022908497601747513,0.11683333665132523,0.009445227682590485,-0.0,-0.009704076685011387,-0.038069285452365875,0.02300071157515049,0.03987804427742958,0.1356787234544754]},{"input":[true,8.0,-0.44121630188513544,false,false,0.000496094218475557,false,0.12],"torque":-0.04027597368242132,"active":true,"saturated":false,"log":[0.024077236652374268,0.12279390543699265,0.009529498405754566,-0.0,-0.009023914113640785,-0.040275972336530685,0.022860022261738777,0.040598396211862564,0.13011913001537323]},{"input":[true,8.0,-0.43254864361943784,false,false,0.0004908370023555395,false,0.12],"torque":-0.04224082624397413,"active":true,"saturated":false,"log":[0.025143155828118324,0.12823009490966797,0.009617499075829983,-0.0,-0.008562487550079823,-0.042240824550390244,0.022617768496274948,0.041141439229249954,0.12400692701339722]},{"input":[true,8.0,-0.42028564614719827,false,false,0.00048339909628973055,false,0.12],"torque":-0.043954474594806635,"active":true,"saturated":false,"log":[0.02610122784972191,0.13311626017093658,0.00970885343849659,-0.0,-0.00832202285528183,-0.04395447298884392,0.022275030612945557,0.04150453954935074,0.11736748367547989]},{"input":[true,8.0,-0.40448179149639035,false,false,0.00047381354539676926,false,0.12],"torque":-0.045408732920559434,"active":true,"saturated":false,"log":[0.02694692835211754,0.137429341673851,0.0098031684756279,-0.0,-0.008303741924464703,-0.04540873318910599,0.021833328530192375,0.041685886681079865,0.11022864282131195]},{"input":[true,8.0,-0.38520729301029594,false,false,0.0004621229363487218,false,0.12],"torque":-0.04659661871172042,"active":true,"saturated":false,"log":[0.02767624706029892,0.1411488652229309,0.009900035336613655,-0.0,-0.00850785244256258,-0.04659662023186684,0.021294625476002693,0.04168449342250824,0.10262057185173035]},{"input":[true,8.0,-0.3625477834037612,false,false,0.00044837920816707353,false,0.12],"torque":-0.04751239152263384,"active":true,"saturated":false,"log":[0.028285710141062737,0.14425712823867798,0.00999903492629528,-0.0,-0.008933559991419315,-0.04751238971948624,0.020661314949393272,0.041500188410282135,0.09457562863826752]},{"input":[true,8.0,-0.3366039343142923,false,false,0.00043264342146811797,false,0.12],"torque":-0.04815156870885167,"active":true,"saturated":false,"log":[0.028772389516234398,0.1467391848564148,0.010099737904965878,-0.0,-0.009579069912433624,-0.048151567578315735,0.01993620954453945,0.041133634746074677,0.08612822741270065]},{"input":[true,8.0,-0.30749100903824395,false,false,0.00041498548718293574,false,0.12],"torque":-0.04851095971914904,"active":true,"saturated":false,"log":[0.029133925214409828,0.14858302474021912,0.010201706551015377,-0.0,-0.01044160034507513,-0.04851096123456955,0.0191225316375494,0.040586311370134354,0.07731465250253677]},{"input":[true,8.0,-0.2753383504392097,false,false,0.00039548385595720845,false,0.12],"torque":-0.0485886531384901,"active":true,"saturated":false,"log":[0.02936851978302002,0.14977945387363434,0.010304496623575687,-0.0,-0.011517397128045559,-0.04858865216374397,0.018223896622657776,0.03986050933599472,0.06817290186882019]},{"input":[true,8.0,-0.24028880630372668,false,false,0.00037422516961079937,false,0.12],"torque":-0.04838303110796813,"active":true,"saturated":false,"log":[0.029474955052137375,0.15032227337360382,0.010407659225165844,-0.0,-0.012801752425730228,-0.04838303104043007,0.01724429614841938,0.0389593243598938,0.05874252691864967]},{"input":[true,8.0,-0.20249809469733254,false,false,0.0003513038762055989,false,0.12],"torque":-0.047895390716643796,"active":true,"saturated":false,"log":[0.029452597722411156,0.1502082496881485,0.010510743595659733,-0.0,-0.014289031736552715,-0.047895390540361404,0.016188083216547966,0.037886638194322586,0.04906441643834114]},{"input":[true,8.0,-0.16213411214055995,false,false,0.00032682181043180595,false,0.12],"torque":-0.047128103456317336,"active":true,"saturated":false,"log":[0.029301395639777184,0.14943711459636688,0.010613298043608665,-0.0,-0.015972694382071495,-0.0471281036734581,0.015059949830174446,0.03664711117744446,0.03918064385652542]},{"input":[true,8.0,-0.11937618767851249,false,false,0.0003008877411769109,false,0.12],"torque":-0.046084429117513065,"active":true,"saturated":false,"log":[0.02902187965810299,0.14801158010959625,0.010714874602854252,-0.0,-0.01784532703459263,-0.04608443006873131,0.013864907436072826,0.035246141254901886,0.029134254902601242]},{"input":[true,8.0,-0.07441428615803208,false,false,0.00027361688828743294,false,0.12],"torque":-0.04476887429119716,"active":true,"saturated":false,"log":[0.028615159913897514,0.14593730866909027,0.010815028101205826,-0.0,-0.0198986753821373,-0.04476887360215187,0.012608266435563564,0.03368985652923584,0.018969079479575157]},{"input":[true,8.0,-0.027448164252147633,false,false,0.0002451304106703497,false,0.12],"torque":-0.04318717756909753,"active":true,"saturated":false,"log":[0.028082922101020813,0.1432228982448578,0.010913318023085594,-0.0,-0.022123683243989944,-0.043187178671360016,0.01129561010748148,0.0319850854575634,0.008729536086320877]},{"input":[true,8.0,0.021313517018595962,false,false,0.00021555486800847306,false,0.12],"torque":-0.04134664203351044,"active":true,"saturated":false,"log":[0.027427418157458305,0.13987983763217926,0.011009314097464085,-0.0,-0.024509988725185394,-0.04134664312005043,0.009932768531143665,0.030139317736029625,-0.001539579825475812]},{"input":[false,8.0,0.07165411932502488,false,false,0.00018502165848126905,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.1233499895098027,false,false,0.0001536664349892097,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.17617145323336134,false,false,0.00012162850247525857,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.22988383537073903,false,false,8.905019902106331e-05,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.2842485026258938,false,false,5.6076263467527434e-05,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.3390239237313777,false,false,2.2853192369290225e-05,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.39396674252311037,false,false,-1.0471410859959058e-05,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.4488328591228079,false,false,-4.37494917197232e-05,true,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.5033785144245768,false,false,-7.683320239902244e-05,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.5573613730675296,false,false,-0.00010957555863444268,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.6105416000829831,false,false,-0.00014183109273161314,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.6626829264329254,false,false,-0.00017345649984887746,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.7135536987057645,false,false,-0.00020431127467184204,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.7629279083057785,false,false,-0.00023425833565018857,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,8.0,0.8105861955637923,false,false,-0.00026316463402339594,false,0.12],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,8.0,0.8563168243080039,false,false,-0.00029090174492958534,false,0.12],"torque":-0.01944323856234006,"active":true,"saturated":false,"log":[0.023616664111614227,0.12044498324394226,0.0,0.0,-0.059786129742860794,-0.019443238154053688,-0.013404752127826214,0.0039943065494298935,-0.014207550324499607]},{"input":[true,8.0,0.8999166225651551,false,false,-0.00031734643797131734,false,0.12],"torque":-0.01305039370780318,"active":true,"saturated":false,"log":[0.02029408887028694,0.10349985212087631,7.102930976543576e-05,0.0,-0.06396645307540894,-0.013050394132733345,-0.014623323455452919,0.00032790080877020955,-0.02717483416199684]},{"input":[true,8.0,0.941191885212685,false,false,-0.00034238122470342736,false,0.12],"torque":-0.0063764733443485225,"active":true,"saturated":false,"log":[0.01705964282155037,0.0870041772723198,0.00013073805894237012,0.0,-0.06798941642045975,-0.006376473233103752,-0.015776926651597023,-0.003208608366549015,-0.03892677277326584]},{"input":[true,8.0,0.9799592345715822,false,false,-0.0003658948806105272,false,0.12],"torque":0.0001076625284318719,"active":true,"saturated":false,"log":[0.013915198855102062,0.07096751779317856,0.00017944125283975154,0.0,-0.07184838503599167,0.00010766252671601251,-0.016860436648130417,-0.006608718074858189,-0.049487896263599396]},{"input":[true,8.0,1.0160464351164764,false,false,-0.00038778293925512483,false,0.12],"torque":0.006356199172488515,"active":true,"saturated":false,"log":[0.010863101109862328,0.055401816964149475,0.00021746210404671729,0.0,-0.07553507387638092,0.006356199271976948,-0.01786903664469719,-0.00986588653177023,-0.05888265743851662]},{"input":[true,8.0,1.0492931586833898,false,false,-0.0004079481564009665,false,0.12],"torque":0.01242340502928238,"active":true,"saturated":false,"log":[0.007906099781394005,0.04032110795378685,0.000245133473072201,0.0,-0.07904113084077835,0.012423405423760414,-0.01879825070500374,-0.01297360472381115,-0.06713566929101944]},{"input":[true,8.0,1.0795516967754786,false,false,-0.00042630094204959286,false,0.12],"torque":0.018270964714592493,"active":true,"saturated":false,"log":[0.005047281738370657,0.02574113756418228,0.0002627989451866597,0.0,-0.08235843479633331,0.018270963802933693,-0.01964394748210907,-0.015925472602248192,-0.07427197694778442]},{"input":[true,8.0,1.1066876168021378,false,false,-0.00044275975847065947,false,0.12],"torque":0.023889810043834767,"active":true,"saturated":false,"log":[0.002290001604706049,0.011679008603096008,0.0002708139654714614,0.0,-0.08547914028167725,0.023889809846878052,-0.020402370020747185,-0.018715260550379753,-0.08031725883483887]},{"input":[true,8.0,1.1305803593359243,false,false,-0.00045725148245765016,false,0.12],"torque":0.029271307497854296,"active":true,"saturated":false,"log":[-0.0003621818032115698,-0.0018471272196620703,0.0002695463190320879,0.0,-0.08839576691389084,0.029271308332681656,-0.021070148795843124,-0.021336978301405907,-0.08529800921678543]},{"input":[true,8.0,1.151123773733812,false,false,-0.00046971173019955657,false,0.12],"torque":0.034406715578297914,"active":true,"saturated":false,"log":[-0.0029055674094706774,-0.014818393625319004,0.0002593768294900656,0.0,-0.09110128879547119,0.034406714141368866,-0.021644316613674164,-0.023784929886460304,-0.08924172818660736]},{"input":[true,8.0,1.168226589743121,false,false,-0.000480085143325183,false,0.12],"torque":0.03928729173642924,"active":true,"saturated":false,"log":[-0.0053363703191280365,-0.027215488255023956,0.0002406995336059481,0.0,-0.0935891792178154,0.03928729146718979,-0.022122323513031006,-0.02605377696454525,-0.09217702597379684]},{"input":[true,8.0,1.1818128229968674,false,false,-0.0004883256348492412,false,0.12],"torque":0.04390441275960317,"active":true,"saturated":false,"log":[-0.007650779560208321,-0.03901897743344307,0.00021392179769463837,0.0,-0.09585347771644592,0.04390441253781319,-0.022502044215798378,-0.02813858911395073,-0.09413377195596695]},{"input":[true,8.0,1.191822112597003,false,false,-0.0004943965939275431,false,0.12],"torque":0.048249686772957656,"active":true,"saturated":false,"log":[-0.009845010936260223,-0.05020955577492714,0.0001794642594177276,0.0,-0.09788887202739716,0.04824968799948692,-0.022781794890761375,-0.03003489039838314,-0.095143161714077]},{"input":[true,8.0,1.1982099892857276,false,false,-0.0004982710485116088,false,0.12],"torque":0.05231506213200572,"active":true,"saturated":false,"log":[-0.01191535871475935,-0.06076832860708237,0.00013776050764136016,0.0,-0.09969072043895721,0.052315063774585724,-0.022960329428315163,-0.03173870965838432,-0.09523780643939972]},{"input":[true,8.0,1.2009480730134459,false,false,-0.0004999317851800449,false,0.12],"torque":0.0560929292493052,"active":true,"saturated":false,"log":[-0.013858243823051453,-0.07067704200744629,8.925666043069214e-05,0.0,-0.1012551411986351,0.05609292909502983,-0.023036856204271317,-0.033246614038944244,-0.09445179253816605]},{"input":[true,8.0,1.2000241990256117,false,false,-0.0004993714256143098,false,0.12],"torque":0.059576218018195064,"active":true,"saturated":false,"log":[-0.015670260414481163,-0.07991832494735718,3.441074659349397e-05,0.0,-0.10257900506258011,0.05957621708512306,-0.023011034354567528,-0.03455575555562973,-0.09282068908214569]},{"input":[true,8.0,1.1954424719082826,false,false,-0.0004965924593790963,false,0.12],"torque":0.0627584811475644,"active":true,"saturated":false,"log":[-0.017348214983940125,-0.08847589790821075,-2.6308005544706248e-05,0.0,-0.10366000980138779,0.06275848299264908,-0.02288297936320305,-0.035663895308971405,-0.09038157016038895]},{"input":[true,8.0,1.1872232473522715,false,false,-0.0004916072328616975,false,0.12],"torque":0.06563398267724357,"active":true,"saturated":false,"log":[-0.018889170140028,-0.09633477032184601,-9.24201012821868e-05,0.0,-0.10449671000242233,0.06563398241996765,-0.022653261199593544,-0.036569442600011826,-0.08717300742864609]},{"input":[true,8.0,1.1754030417169097,false,false,-0.0004844378944194944,false,0.12],"torque":0.06819777826982457,"active":true,"saturated":false,"log":[-0.02029048465192318,-0.1034814715385437,-0.00016343679453711957,0.0,-0.10508850961923599,0.06819777935743332,-0.022322898730635643,-0.037271466106176376,-0.08323506265878677]},{"input":[true,8.0,1.1600343697952193,false,false,-0.0004751162959792648,false,0.12],"torque":0.07044576706849172,"active":true,"saturated":false,"log":[-0.021549837663769722,-0.10990417003631592,-0.0002388612338108942,0.0,-0.10543569922447205,0.07044576853513718,-0.02189335785806179,-0.037769727408885956,-0.07860919833183289]}]},{"name":"highway","frames":[{"input":[false,23.0,3.1545927455934697,false,false,-0.0013323183357528668,true,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,3.0689462193721715,false,false,-0.001291240963619141,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.980308588050073,false,false,-0.0012487290121424167,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.888778328765439,false,false,-0.0012048297124508391,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.7944571323900176,false,false,-0.0011595918370278437,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.6974497905496495,false,false,-0.0011130656455255266,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.5978640791999017,false,false,-0.0010653028289257553,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.495810638886099,false,false,-0.0010163564521110636,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.391402851820766,false,false,-0.0009662808949091303,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.284756715915053,false,false,-0.000915131791676342,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.175990715904102,false,false,-0.0008629659694875654,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,2.0652256917095344,false,false,-0.0008098413850007994,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.9525847041852988,false,false,-0.0007558170600668505,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.8381928983960498,false,false,-0.000700953016155571,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.722177364579951,false,false,-0.0006453102076715109,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.604666996950376,false,false,-0.000588950454233074,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.4857923504933706,false,false,-0.0005319363719904111,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.3656854959199956,false,false,-0.0004743313040583631,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.2444798729346718,false,false,-0.0004161992501417374,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,1.1223101419825754,false,false,-0.00035760479543111017,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,23.0,0.9993120346407722,false,false,-0.000298613038848147,false,0.35],"torque":0.003458141909455566,"active":true,"saturated":false,"log":[0.11046585440635681,0.1384505331516266,0.0,-0.0,-0.17728012800216675,0.003458141814917326,-0.11373573541641235,-0.011075318790972233,-0.03164376690983772]},{"input":[true,23.0,0.8756222028193177,false,false,-0.00023928952072016532,false,0.35],"torque":0.005581825596640087,"active":true,"saturated":false,"log":[0.07743847370147705,0.09705621749162674,0.0002710346598178148,-0.0,-0.15769034624099731,0.005581825505942106,-0.09114059060811996,-0.019173864275217056,-0.05478246882557869]},{"input":[true,23.0,0.7513780669398596,false,false,-0.00017970014996429066,false,0.35],"torque":0.0080999750697488,"active":true,"saturated":false,"log":[0.04729127883911133,0.059271737933158875,0.0004365541390143335,-0.0,-0.13695798814296722,0.008099975064396858,-0.06844419240951538,-0.024494482204318047,-0.06998423486948013]},{"input":[true,23.0,0.6267176632604193,false,false,-0.00011991113086210739,false,0.35],"torque":0.010745190217460947,"active":true,"saturated":false,"log":[0.019849784672260284,0.024878397583961487,0.0005060283583588898,-0.0,-0.11516954004764557,0.01074519008398056,-0.045671749860048294,-0.02722453512251377,-0.07778438925743103]},{"input":[true,23.0,0.5017794905159711,false,false,-5.998888950615651e-05,false,0.35],"torque":0.013308493030981331,"active":true,"saturated":false,"log":[-0.0050488971173763275,-0.006327950861304998,0.000488357269205153,-0.0,-0.09240610897541046,0.013308493420481682,-0.022848565131425858,-0.02754071168601513,-0.07868774980306625]},{"input":[true,23.0,0.37670235604520796,false,false,0.0,false,0.35],"torque":0.015629157170874007,"active":true,"saturated":false,"log":[-0.02755693532526493,-0.03453802689909935,0.0003919079608749598,-0.0,-0.06874653697013855,0.015629157423973083,2.2994597337344658e-09,-0.025609780102968216,-0.07317079603672028]},{"input":[true,23.0,0.2516252215744448,false,false,5.998888950615651e-05,false,0.35],"torque":0.01758633276784615,"active":true,"saturated":false,"log":[-0.04781654477119446,-0.059930071234703064,0.00022455006546806544,-0.0,-0.044269610196352005,0.01758633367717266,0.022848570719361305,-0.021589288488030434,-0.06168368458747864]},{"input":[true,23.0,0.1266870488299966,false,false,0.00011991113086210739,false,0.35],"torque":0.019092038900045075,"active":true,"saturated":false,"log":[-0.06596070528030396,-0.08267074823379517,-6.312408459052676e-06,-0.0,-0.01905502937734127,0.01909203827381134,0.04567175358533859,-0.015628214925527573,-0.04465204104781151]},{"input":[true,23.0,0.0020266451505563388,false,false,0.00017970014996429066,false,0.35],"torque":0.02008521296368073,"active":true,"saturated":false,"log":[-0.08211386203765869,-0.10291603952646255,-0.00029371093842200935,-0.0,0.0068168239668011665,0.02008521370589733,0.06844419240951538,-0.007867556065320969,-0.022478731349110603]},{"input":[true,23.0,-0.12221749072890159,false,false,0.00023928952072016532,false,0.35],"torque":0.020529037684579577,"active":true,"saturated":false,"log":[-0.09639251232147217,-0.12081194669008255,-0.0006310847238637507,-0.0,0.033254723995923996,0.020529037341475487,0.09114059805870056,0.0015591104747727513,0.004454601090401411]},{"input":[true,23.0,-0.24590732255035613,false,false,0.000298613038848147,false,0.35],"torque":0.02030244477282309,"active":true,"saturated":false,"log":[-0.10890582948923111,-0.13649530708789825,-0.0010122550884261727,-0.0,0.06011124700307846,0.02030244469642639,0.11373573541641235,0.01252511702477932,0.03578604757785797]},{"input":[true,23.0,-0.3689054298921594,false,false,0.00035760479543111017,false,0.35],"torque":0.018988995962110324,"active":true,"saturated":false,"log":[-0.11975616961717606,-0.1500944048166275,-0.0014314017025753856,-0.0,0.08735337853431702,0.018988996744155884,0.13620451092720032,0.024910233914852142,0.07117209583520889]},{"input":[true,23.0,-0.4910751608442558,false,false,0.0004161992501417374,false,0.35],"torque":0.016762974788142593,"active":true,"saturated":false,"log":[-0.12903960049152374,-0.161729633808136,-0.0018830402987077832,-0.0,0.11489768326282501,0.016762975603342056,0.15852198004722595,0.03860022500157356,0.11028636246919632]},{"input":[true,23.0,-0.6122807838295796,false,false,0.0004743313040583631,false,0.35],"torque":0.013752188638353174,"active":true,"saturated":false,"log":[-0.13684631884098053,-0.17151404917240143,-0.0023620023857802153,-0.0,0.14265719056129456,0.01375218853354454,0.1806633025407791,0.05348645895719528,0.1528184562921524]},{"input":[true,23.0,-0.7323876384029546,false,false,0.0005319363719904111,false,0.35],"torque":0.010052735994049676,"active":true,"saturated":false,"log":[-0.1432611495256424,-0.17955397069454193,-0.0028634164482355118,-0.0,0.17054203152656555,0.010052735917270184,0.20260392129421234,0.0694655179977417,0.19847290217876434]},{"input":[true,23.0,-0.8512622848599598,false,false,0.000588950454233074,false,0.35],"torque":0.07782561395627413,"active":true,"saturated":false,"log":[-0.8642393350601196,-1.0831799507141113,-0.005888253916054964,-0.0,0.02110760658979416,0.07782561331987381,0.2243194580078125,-0.5788533091545105,0.3598373532295227]},{"input":[true,23.0,-0.9687726524895353,false,false,0.0006453102076715109,false,0.35],"torque":0.1239296042585836,"active":true,"saturated":false,"log":[-0.8074448108673096,-1.0119974613189697,-0.008714310824871063,-0.0,0.05092191696166992,0.12392960488796234,0.24578575789928436,-0.5046055912971497,0.509888231754303]},{"input":[true,23.0,-1.084788186305634,false,false,0.000700953016155571,false,0.35],"torque":0.1532288610297261,"active":true,"saturated":false,"log":[-0.7535020112991333,-0.9443891644477844,-0.011351567693054676,-0.0,0.08035696297883987,0.15322886407375336,0.2669789791107178,-0.4332810640335083,0.6494187712669373]},{"input":[true,23.0,-1.1991799920948827,false,false,0.0007558170600668505,false,0.35],"torque":0.16857318538754634,"active":true,"saturated":false,"log":[-0.7021345496177673,-0.8800086379051208,-0.013809039257466793,-0.0,0.12640951573848724,0.16857318580150604,0.28787559270858765,-0.3646466135978699,0.779166579246521]},{"input":[true,23.0,-1.3118209796191185,false,false,0.0008098413850007994,false,0.35],"torque":0.1729605025528329,"active":true,"saturated":false,"log":[-0.6530870199203491,-0.8185357451438904,-0.013809039257466793,-0.0,0.17788416147232056,0.17296050488948822,0.30845239758491516,-0.29848796129226685,0.8998174667358398]},{"input":[true,23.0,-1.422586003813686,false,false,0.0008629659694875654,false,0.35],"torque":0.1662434807003763,"active":true,"saturated":false,"log":[-0.6061232089996338,-0.7596744298934937,-0.013809039257466793,-0.0,0.22762329876422882,0.16624347865581512,0.3286864757537842,-0.234608456492424,1.0120093822479248]},{"input":[true,23.0,-1.5313520038246364,false,false,0.000915131791676342,false,0.35],"torque":0.14990742135373375,"active":true,"saturated":false,"log":[-0.5610249042510986,-0.7031512260437012,-0.013809039257466793,-0.0,0.27713167667388916,0.1499074250459671,0.34855538606643677,-0.17282786965370178,1.1163352727890015]},{"input":[true,23.0,-1.63799813973035,false,false,0.0009662808949091303,false,0.35],"torque":0.12862585509973215,"active":true,"saturated":false,"log":[-0.5175909399986267,-0.648714005947113,-0.013809039257466793,-0.0,0.3266052305698395,0.12862585484981537,0.36803707480430603,-0.11298125237226486,1.2133466005325317]},{"input":[true,23.0,-1.7424059267956837,false,false,0.0010163564521110636,false,0.35],"torque":0.10506542232114843,"active":true,"saturated":false,"log":[-0.475635826587677,-0.5961302518844604,-0.013809039257466793,-0.0,0.37557002902030945,0.10506542026996613,0.3871098458766937,-0.05491787940263748,1.3035563230514526]},{"input":[true,23.0,-1.8444593671094862,false,false,0.0010653028289257553,false,0.35],"torque":0.08065999187015593,"active":true,"saturated":false,"log":[-0.4349888265132904,-0.5451859831809998,-0.013809039257466793,-0.0,0.42360013723373413,0.08065999299287796,0.4057525396347046,0.0014997281832620502,1.3874412775039673]},{"input":[true,23.0,-1.9440450784592331,false,false,0.0011130656455255266,false,0.35],"torque":0.05622564456971676,"active":true,"saturated":false,"log":[-0.3954930007457733,-0.49568456411361694,-0.015193264000117779,-0.0,0.47143417596817017,0.056225646287202835,0.4239444434642792,0.056396715342998505,1.4654449224472046]},{"input":[true,23.0,-2.041052420299602,false,false,0.0011595918370278437,false,0.35],"torque":0.03175804675294511,"active":true,"saturated":false,"log":[-0.3570042848587036,-0.4474453628063202,-0.016442779451608658,-0.0,0.5183174014091492,0.03175804764032364,0.4416653513908386,0.10988674312829971,1.5379794836044312]},{"input":[true,23.0,-2.135373616675023,false,false,0.0012048297124508391,false,0.35],"torque":0.007156138153225827,"active":true,"saturated":false,"log":[-0.3193906843662262,-0.4003029763698578,-0.017560645937919617,-0.0,0.5646003484725952,0.0071561383083462715,0.4588955342769623,0.1620727926492691,1.6054285764694214]},{"input":[true,23.0,-2.2269038759596573,false,false,0.0012487290121424167,false,0.35],"torque":-0.017866096529742373,"active":true,"saturated":false,"log":[-0.2825314700603485,-0.3541060984134674,-0.01854950748383999,-0.0,0.6105777025222778,-0.017866097390651703,0.475615918636322,0.21304790675640106,1.6681487560272217]},{"input":[true,23.0,-2.3155415072817562,false,false,0.001291240963619141,false,0.35],"torque":-0.0312776307526293,"active":true,"saturated":false,"log":[-0.24631652235984802,-0.30871671438217163,-0.019411614164710045,-0.0,0.6562475562095642,-0.03127763047814369,0.49180784821510315,0.2628958821296692,1.7264714241027832]},{"input":[true,23.0,-2.4011880335030544,false,false,0.0013323183357528668,false,0.35],"torque":-0.047608032024468935,"active":true,"saturated":false,"log":[-0.21064552664756775,-0.2640090584754944,-0.020148873329162598,-0.0,0.7011253237724304,-0.04760803282260895,0.5074533820152283,0.31169193983078003,1.7807049751281738]},{"input":[true,23.0,-2.483748300627684,false,false,0.0013719154912449113,false,0.35],"torque":-0.06654070693930893,"active":true,"saturated":false,"log":[-0.17542745172977448,-0.21986907720565796,-0.020762870088219643,-0.0,0.7432522177696228,-0.06654071062803268,0.5225351452827454,0.35950329899787903,1.8311361074447632]},{"input":[true,23.0,-2.563130583519048,false,false,0.0014099884373294702,false,0.35],"torque":-0.08763516646496676,"active":true,"saturated":false,"log":[-0.1405799388885498,-0.17619352042675018,-0.021254900842905045,-0.0,0.7782087922096252,-0.08763516694307327,0.5370364189147949,0.4063897430896759,1.8780314922332764]},{"input":[true,23.0,-2.639246687807198,false,false,0.0014464948746499401,true,0.35],"torque":-0.11050366509507532,"active":true,"saturated":false,"log":[-0.10602869093418121,-0.1328892856836319,-0.021626001223921776,-0.0,0.8105132579803467,-0.1105036661028862,0.550940990447998,0.4524042010307312,1.9216388463974]},{"input":[true,23.0,-2.7120120478735097,false,false,0.0014813942442538548,false,0.35],"torque":-0.13466127647811182,"active":true,"saturated":false,"log":[-0.07170704752206802,-0.0898728296160698,-0.021876975893974304,-0.0,0.8404983878135681,-0.13466127216815948,0.5642334222793579,0.4975931644439697,1.9621888399124146]},{"input":[true,23.0,-2.781345820803782,false,false,0.0015146477726542137,false,0.35],"torque":-0.1596559391502566,"active":true,"saturated":false,"log":[-0.037555448710918427,-0.0470694974064827,-0.022008419036865234,-0.0,0.8685391545295715,-0.15965594351291656,0.5768990516662598,0.5419972538948059,1.9998958110809326]},{"input":[true,23.0,-2.8471709762053754,false,false,0.0015462185149071458,false,0.35],"torque":-0.18509787095992697,"active":true,"saturated":false,"log":[-0.003520977683365345,-0.004412958864122629,-0.022020742297172546,-0.0,0.8949332237243652,-0.18509787321090698,0.5889236927032471,0.5856515169143677,2.034959077835083]},{"input":[true,23.0,-2.909414381788617,false,false,0.0015760713956580457,false,0.35],"torque":-0.21054452443852284,"active":true,"saturated":false,"log":[0.03044302389025688,0.03815525770187378,-0.02191419154405594,-0.0,0.9198853373527527,-0.21054452657699585,0.6002940535545349,0.6285859942436218,2.067564010620117]},{"input":[true,23.0,-2.9680068846173775,false,false,0.0016041732481105837,false,0.35],"torque":-0.23578823876380162,"active":true,"saturated":false,"log":[0.06437738239765167,0.08068631589412689,-0.02168887108564377,-0.0,0.9435290694236755,-0.2357882410287857,0.6109974980354309,0.6708260178565979,2.0978829860687256]},{"input":[true,23.0,-3.0228833879385446,false,false,0.001630492850875286,false,0.35],"torque":-0.2608309392720057,"active":true,"saturated":false,"log":[0.09831743687391281,0.12322451919317245,-0.021344760432839394,-0.0,0.9678671360015869,-0.26083093881607056,0.62102210521698,0.7123925089836121,2.1260759830474854]},{"input":[true,23.0,-3.0739829235050635,false,false,0.0016550009626567592,false,0.35],"torque":-0.28587760419010677,"active":true,"saturated":false,"log":[0.13229340314865112,0.16580773890018463,-0.020881732925772667,-0.0,0.9955551028251648,-0.28587761521339417,0.6303567886352539,0.7533023953437805,2.1522927284240723]},{"input":[true,23.0,-3.1212487193121508,false,false,0.0016776703547410072,false,0.35],"torque":-0.31062210096713394,"active":true,"saturated":false,"log":[0.164878711104393,0.20664797723293304,-0.020304657518863678,-0.0,1.0213737487792969,-0.31062209606170654,0.6389910578727722,0.7922195792198181,2.1728155612945557]},{"input":[true,23.0,-3.164628262671469,false,false,0.0016984758412467596,false,0.35],"torque":-0.33483575772987456,"active":true,"saturated":false,"log":[0.19608694314956665,0.24576230347156525,-0.019618352875113487,-0.0,1.0453310012817383,-0.3348357677459717,0.6469154953956604,0.8291470408439636,2.1877543926239014]},{"input":[true,23.0,-3.204073358553144,false,false,0.0017173943071071896,false,0.35],"torque":-0.3583542391781177,"active":true,"saturated":false,"log":[0.2259671539068222,0.28321215510368347,-0.018827468156814575,-0.0,1.067450761795044,-0.35835424065589905,0.6541211605072021,0.8641216158866882,2.197314977645874]},{"input":[true,23.0,-3.239540183130852,false,false,0.0017344047337509473,false,0.35],"torque":-0.3810615080694353,"active":true,"saturated":false,"log":[0.25456398725509644,0.3190535306930542,-0.017936494201421738,-0.0,1.0877538919448853,-0.38106149435043335,0.6606000661849976,0.8971767425537109,2.201693058013916]},{"input":[true,23.0,-3.2709893324704424,false,false,0.0017494882224539628,false,0.35],"torque":-0.4023154736698855,"active":true,"saturated":false,"log":[0.2819180190563202,0.35333725810050964,-0.016949782148003578,-0.0,1.0982064008712769,-0.4023154675960541,0.6663450598716736,0.9283429980278015,2.2010762691497803]},{"input":[true,23.0,-3.2983858663080396,false,false,0.0017626280153360862,false,0.35],"torque":-0.42205658529962786,"active":true,"saturated":false,"log":[0.308066189289093,0.3861096203327179,-0.015871549025177956,-0.0,1.1051573753356934,-0.4220565855503082,0.6713497638702393,0.9576482176780701,2.195643663406372]},{"input":[true,23.0,-3.3216993468689586,false,false,0.0017738095139792282,false,0.35],"torque":-0.44041723235848096,"active":true,"saturated":false,"log":[0.33304205536842346,0.4174126982688904,-0.014705902896821499,-0.0,1.1110724210739136,-0.44041723012924194,0.6756085753440857,0.9851180911064148,2.185567617416382]},{"input":[true,23.0,-3.3409038726843296,false,false,0.0017830202956463255,false,0.35],"torque":-0.45750794657377625,"active":true,"saturated":false,"log":[0.356876015663147,0.44728460907936096,-0.013456836342811584,-0.0,1.1159448623657227,-0.4575079381465912,0.6791167855262756,1.010776162147522,2.1710140705108643]},{"input":[true,23.0,-3.3559781073678336,false,false,0.0017902501270831014,false,0.35],"torque":-0.4734206194549991,"active":true,"saturated":false,"log":[0.37959572672843933,0.47575998306274414,-0.012128251604735851,-0.0,1.1197694540023804,-0.4734206199645996,0.6818704605102539,1.0346442461013794,2.1521425247192383]},{"input":[true,23.0,-3.366905303320613,false,false,0.001795490975887298,false,0.35],"torque":-0.4882314466144239,"active":true,"saturated":false,"log":[0.4012262523174286,0.5028702616691589,-0.010723959654569626,-0.0,1.122541904449463,-0.48823145031929016,0.6838666200637817,1.056742548942566,2.129107713699341]},{"input":[true,23.0,-3.373673320337987,false,false,0.0017987370194327387,false,0.35],"torque":-0.5020035463402898,"active":true,"saturated":false,"log":[0.4217904210090637,0.5286439657211304,-0.009247693233191967,-0.0,1.124259114265442,-0.50200355052948,0.6851029396057129,1.0770899057388306,2.1020596027374268]},{"input":[true,23.0,-3.3762746390973195,false,false,0.0017999846513383159,false,0.35],"torque":-0.5147892038034518,"active":true,"saturated":false,"log":[0.4413088262081146,0.5531070828437805,-0.007703111972659826,-0.0,1.124919056892395,-0.5147892236709595,0.6855781674385071,1.095704436302185,2.071143627166748]},{"input":[true,23.0,-3.3747063695120487,false,false,0.001799232485474709,false,0.35],"torque":-0.52663180449149,"active":true,"saturated":false,"log":[0.4598003029823303,0.5762830376625061,-0.006093811243772507,-0.0,1.1245211362838745,-0.5266318321228027,0.6852916479110718,1.1126028299331665,2.03650164604187]},{"input":[true,23.0,-3.3689702539425994,false,false,0.0017964813575043867,false,0.35],"torque":-0.5375674370700096,"active":true,"saturated":false,"log":[0.4772818684577942,0.5981932878494263,-0.0044233244843780994,-0.0,1.1230658292770386,-0.5375674366950989,0.6842437982559204,1.1278012990951538,1.9982719421386719]},{"input":[true,23.0,-3.3590726652605984,false,false,0.001791734323953177,false,0.35],"torque":-0.5476262542574671,"active":true,"saturated":false,"log":[0.49376916885375977,0.6188573837280273,-0.00269513251259923,-0.0,1.1205546855926514,-0.547626256942749,0.6824357509613037,1.1413155794143677,1.956590175628662]},{"input":[true,23.0,-3.3450245997685735,false,false,0.0017849966588144434,false,0.35],"torque":-0.5568335730015357,"active":true,"saturated":false,"log":[0.5092763304710388,0.6382930278778076,-0.0009126653312705457,-0.0,1.1169904470443726,-0.5568335652351379,0.679869532585144,1.153160810470581,1.9115886688232422]},{"input":[true,23.0,-3.326841664982961,false,false,0.0017762758476896352,false,0.35],"torque":-0.5652108147739034,"active":true,"saturated":false,"log":[0.5238164663314819,0.6565166115760803,0.0009206922841258347,-0.0,1.1123770475387573,-0.5652108192443848,0.6765479445457458,1.1633518934249878,1.8633980751037598]},{"input":[true,23.0,-3.3045440622940303,false,false,0.0017655815794717213,false,0.35],"torque":-0.5727762512978043,"active":true,"saturated":false,"log":[0.5374014973640442,0.6735432147979736,0.002801597584038973,-0.0,1.1067198514938354,-0.5727762579917908,0.67247474193573,1.1719038486480713,1.8121464252471924]},{"input":[true,23.0,-3.278156564521974,false,false,0.0017529257355807513,false,0.35],"torque":-0.579545638231852,"active":true,"saturated":false,"log":[0.5500425100326538,0.6893866062164307,0.004726746119558811,-0.0,1.100024938583374,-0.5795456171035767,0.6676543354988098,1.1788312196731567,1.7579600811004639]},{"input":[true,23.0,-3.2477084883940983,false,false,0.0017383223767634996,false,0.35],"torque":-0.5855327302948247,"active":true,"saturated":false,"log":[0.5617498159408569,0.7040597796440125,0.006692870520055294,-0.0,1.0922996997833252,-0.5855327248573303,0.6620922088623047,1.184149146080017,1.700964093208313]},{"input":[true,23.0,-3.2132336619736974,false,false,0.00172178772747186,false,0.35],"torque":-0.590749694708998,"active":true,"saturated":false,"log":[0.5725329518318176,0.7175746560096741,0.008696735836565495,0.0,1.0835528373718262,-0.5907496809959412,0.6557945013046265,1.1878726482391357,1.6412817239761353]},{"input":[true,23.0,-3.174770387076801,false,false,0.001703340157837346,false,0.35],"torque":-0.59520747387549,"active":true,"saturated":false,"log":[0.5824010372161865,0.7299426198005676,0.01073513925075531,0.0,1.0737941265106201,-0.5952074527740479,0.6487681865692139,1.1900172233581543,1.5790350437164307]},{"input":[true,23.0,-3.1323613967185358,false,false,0.001683000163261724,false,0.35],"torque":-0.5989160645651623,"active":true,"saturated":false,"log":[0.5913626551628113,0.7411745190620422,0.0128049086779356,0.0,1.0630342960357666,-0.5989160537719727,0.6410211324691772,1.190598487854004,1.5143451690673828]},{"input":[true,23.0,-3.0860538076363953,false,false,0.0016607903416464523,false,0.35],"torque":-0.6018847563299327,"active":true,"saturated":false,"log":[0.5994260311126709,0.7512806057929993,0.014902899973094463,0.0,1.0512852668762207,-0.6018847823143005,0.6325618028640747,1.189632773399353,1.4473323822021484]},{"input":[true,23.0,-3.0358990679431597,false,false,0.001636735368286227,false,0.35],"torque":-0.6041223264607568,"active":true,"saturated":false,"log":[0.6065990924835205,0.7602708339691162,0.01702599599957466,0.0,1.038560152053833,-0.6041223406791687,0.6233997941017151,1.1871370077133179,1.3781160116195679]},{"input":[true,23.0,-2.981952899967616,false,false,0.0016108619684545263,false,0.35],"torque":-0.6056372105640742,"active":true,"saturated":false,"log":[0.6128896474838257,0.768155038356781,0.019171111285686493,0.0,1.02487313747406,-0.6056371927261353,0.6135451197624207,1.1831283569335938,1.3068145513534546]},{"input":[true,23.0,-2.9242752383465906,false,false,0.001583198887711611,false,0.35],"torque":-0.6064376324617504,"active":true,"saturated":false,"log":[0.6183053255081177,0.7749426960945129,0.02133517898619175,0.0,1.0102393627166748,-0.606437623500824,0.6030088067054749,1.177625060081482,1.233546257019043]},{"input":[true,23.0,-2.8629301634370785,false,false,0.0015537768599679727,false,0.35],"torque":-0.6065317121099159,"active":true,"saturated":false,"log":[0.6228536367416382,0.7806432247161865,0.023515166714787483,0.0,0.9946751594543457,-0.6065317392349243,0.5918025374412537,1.170645833015442,1.1584285497665405]},{"input":[true,23.0,-2.7979858301224363,false,false,0.0015226285733387025,false,0.35],"torque":-0.6055244831068309,"active":true,"saturated":false,"log":[0.626542329788208,0.7852663993835449,0.025708064436912537,0.0,0.9781976938247681,-0.605524480342865,0.5799387693405151,1.1622099876403809,1.0815784931182861]},{"input":[true,23.0,-2.729514392091744,true,false,0.0014897886338267289,false,0.35],"torque":-0.5989636509093171,"active":true,"saturated":false,"log":[0.6293788552284241,0.7888215184211731,0.025708064436912537,0.0,0.9608253836631775,-0.5989636778831482,0.5674306750297546,1.1523381471633911,1.0031126737594604]},{"input":[true,23.0,-2.657591921676465,true,false,0.001455293526875262,false,0.35],"torque":-0.5936461785314185,"active":true,"saturated":false,"log":[0.6313712000846863,0.7913185954093933,0.025708064436912537,0.0,0.9425774812698364,-0.5936461687088013,0.5542922019958496,1.141051173210144,0.9231472611427307]},{"input":[true,23.0,-2.5822983253334604,true,false,0.0014191815768321676,false,0.35],"torque":-0.5876856215652068,"active":true,"saturated":false,"log":[0.6325271725654602,0.7927674055099487,0.025708064436912537,0.0,0.9234742522239685,-0.5876856446266174,0.5405378937721252,1.1283711194992065,0.8417981863021851]},{"input":[true,23.0,-2.503717254868256,true,false,0.0013814929043712983,false,0.35],"torque":-0.5810924134774934,"active":true,"saturated":false,"log":[0.6328549981117249,0.7931782603263855,0.025708064436912537,0.0,0.9035369157791138,-0.5810924172401428,0.526183009147644,1.114320993423462,0.7591808438301086]},{"input":[true,23.0,-2.421936014497193,true,false,0.0013422693819180966,false,0.35],"torque":-0.5738773368238682,"active":true,"saturated":false,"log":[0.6323631405830383,0.7925618290901184,0.025708064436912537,0.0,0.8827877044677734,-0.5738773345947266,0.5112435817718506,1.0989243984222412,0.6754104495048523]},{"input":[true,23.0,-2.3370454638517324,true,false,0.0013015545871289842,false,0.35],"torque":-0.5660514939917114,"active":true,"saturated":false,"log":[0.6310603022575378,0.7909289002418518,0.025708064436912537,0.0,0.861249566078186,-0.5660514831542969,0.49573612213134766,1.082206130027771,0.5906016826629639]},{"input":[true,23.0,-2.2491399170326534,true,false,0.0012593937544762292,false,0.35],"torque":-0.5576262952399741,"active":true,"saturated":false,"log":[0.6289554834365845,0.7882908582687378,0.025708064436912537,0.0,0.8389464616775513,-0.5576263070106506,0.47967788577079773,1.0641918182373047,0.5048691034317017]},{"input":[true,23.0,-2.15831703782631,true,false,0.001215833724992072,false,0.35],"torque":-0.5486134612193073,"active":true,"saturated":false,"log":[0.6260581016540527,0.7846595048904419,0.025708064436912537,0.0,0.8159032464027405,-0.5486134886741638,0.4630867540836334,1.044908046722412,0.41832658648490906]},{"input":[true,23.0,-2.064677731199358,true,false,0.0011709228942279536,false,0.35],"torque":-0.5390249604556555,"active":true,"saturated":false,"log":[0.6223779916763306,0.7800470590591431,0.025708064436912537,0.0,0.7921453714370728,-0.5390249490737915,0.44598111510276794,1.0243823528289795,0.33108779788017273]},{"input":[true,23.0,-1.96832603119251,true,false,0.0011247111584866602,false,0.35],"torque":-0.5288729015769023,"active":true,"saturated":false,"log":[0.6179253458976746,0.7744664549827576,0.025708064436912537,0.0,0.7676993608474731,-0.5288729071617126,0.42837998270988464,1.00264310836792,0.24326588213443756]},{"input":[true,23.0,-1.8693689853378623,true,false,0.0010772498593871215,false,0.35],"torque":-0.5181694483622045,"active":true,"saturated":false,"log":[0.6127107739448547,0.7679308652877808,0.025708064436912537,0.0,0.7425923347473145,-0.5181694626808167,0.41030293703079224,0.9797199964523315,0.15497352182865143]},{"input":[true,23.0,-1.7679165357281967,true,false,0.0010285917268234453,false,0.35],"torque":-0.5069267220031564,"active":true,"saturated":false,"log":[0.6067454218864441,0.7604542374610901,0.025708064436912537,0.0,0.7168521881103516,-0.5069267153739929,0.39177000522613525,0.9556432366371155,0.06632284075021744]},{"input":[true,23.0,-1.6640813968704125,true,false,0.0009787908203815724,false,0.35],"torque":-0.4945096862140992,"active":true,"saturated":false,"log":[0.6000407338142395,0.752051055431366,0.025708064436912537,0.0,0.6885895729064941,-0.4945096969604492,0.37280184030532837,0.9304441213607788,-0.02257462963461876]},{"input":[true,23.0,-1.5579789304587908,true,false,0.0009279024692786355,false,0.35],"torque":-0.4787066458595091,"active":true,"saturated":false,"log":[0.5926087498664856,0.742736279964447,0.025708064436912537,0.0,0.6512402296066284,-0.4787066578865051,0.3534194827079773,0.9041549563407898,-0.11160798370838165]},{"input":[true,23.0,-1.4497270172071886,true,false,0.0008759832108917369,false,0.35],"torque":-0.46230655255895575,"active":true,"saturated":false,"log":[0.58446204662323,0.7325257658958435,0.025708064436912537,0.0,0.6130709648132324,-0.46230655908584595,0.33364447951316833,0.8768088817596436,-0.20066702365875244]},{"input":[true,23.0,-1.339445925882603,true,false,0.0008230907279444615,false,0.35],"torque":-0.4453252790806373,"active":true,"saturated":false,"log":[0.5756135582923889,0.7214356660842896,0.025708064436912537,0.0,0.5741302967071533,-0.4453252851963043,0.3134987950325012,0.8484399318695068,-0.2896423041820526]},{"input":[true,23.0,-1.2272581796855777,false,false,0.0007692837844208935,true,0.35],"torque":-0.43886873686472044,"active":true,"saturated":false,"log":[0.5660768747329712,0.7094830274581909,0.02254772186279297,0.0,0.5344672203063965,-0.4388687312602997,0.2930048108100891,0.819083034992218,-0.3784252107143402]},{"input":[true,23.0,-1.1132884201259248,false,false,0.0007146221602783498,false,0.35],"torque":-0.4285991684391149,"active":true,"saturated":false,"log":[0.5558658838272095,0.696685254573822,0.024493250995874405,0.0,0.4941280782222748,-0.42859917879104614,0.2721852958202362,0.7887740731239319,-0.4669080078601837]},{"input":[true,23.0,-0.9976632685449691,false,false,0.0006591665850313498,false,0.35],"torque":-0.41564423760379265,"active":true,"saturated":false,"log":[0.5449951887130737,0.6830606460571289,0.0264007356017828,0.0,0.45315930247306824,-0.41564422845840454,0.25106337666511536,0.7575495839118958,-0.5549840331077576]},{"input":[true,23.0,-0.8805111854382062,false,false,0.0006029786702806292,false,0.35],"torque":-0.4007417865293545,"active":true,"saturated":false,"log":[0.5334798097610474,0.6686280369758606,0.028267914429306984,0.0,0.4116125702857971,-0.4007417857646942,0.22966252267360687,0.7254469990730286,-0.6425474882125854]},{"input":[true,23.0,-1.916441798789181,false,false,0.000546120841262141,false,0.35],"torque":-0.340955684397237,"active":true,"saturated":false,"log":[0.294405072927475,0.36898767948150635,0.0292983315885067,-0.0,0.2819584310054779,-0.3409556746482849,0.4189019799232483,0.6925045847892761,-0.7294938564300537]},{"input":[true,23.0,-1.6751498694311018,false,false,0.0004886562674921086,false,0.35],"torque":-0.29962934396002305,"active":true,"saturated":false,"log":[0.3055257797241211,0.3829256296157837,0.030367672443389893,0.0,0.24865180253982544,-0.2996293306350708,0.37482377886772156,0.6587613224983215,-0.815719723701477]},{"input":[true,23.0,-1.4315783153550277,false,false,0.00043064879258516834,false,0.35],"torque":-0.27022472941348186,"active":true,"saturated":false,"log":[0.31627553701400757,0.396398663520813,0.03147463500499725,0.0,0.2150510996580124,-0.2702247202396393,0.33032914996147156,0.6242568492889404,-0.9011229872703552]},{"input":[true,23.0,-1.1859977465631695,false,false,0.0003721628633235905,false,0.35],"torque":-0.24846193195184235,"active":true,"saturated":false,"log":[0.32664456963539124,0.4093945324420929,0.03261789306998253,0.0,0.1812327653169632,-0.24846193194389343,0.2854675352573395,0.5890316367149353,-0.9856029152870178]},{"input":[true,23.0,-0.9386810050896264,false,false,0.0003132634580563725,false,0.35],"torque":-0.2315342053179063,"active":true,"saturated":false,"log":[0.33662348985671997,0.4219014346599579,0.033796075731515884,0.0,0.1472931206226349,-0.2315341979265213,0.24028873443603516,0.5531266331672668,-1.069060206413269]},{"input":[true,23.0,-0.6899028618706533,false,false,0.00025401601450776096,false,0.35],"torque":-0.21759707861576544,"active":true,"saturated":false,"log":[0.3462030589580536,0.4339078366756439,0.0350077860057354,0.0,0.11335571855306625,-0.2175970822572708,0.19484297931194305,0.5165835618972778,-1.1513969898223877]},{"input":[true,23.0,-0.439939711471868,false,false,0.00019448635707539666,false,0.35],"torque":-0.20543845318584839,"active":true,"saturated":false,"log":[0.35537442564964294,0.4454026222229004,0.03625159710645676,0.0,0.07957670092582703,-0.20543844997882843,0.14918076992034912,0.479444682598114,-1.2325172424316406]},{"input":[true,23.0,-0.1890692650116222,false,false,0.00013474062369886897,false,0.35],"torque":-0.19426904316025623,"active":true,"saturated":false,"log":[0.36412906646728516,0.4563750922679901,0.037526048719882965,0.0,0.04614019766449928,-0.19426904618740082,0.10335280001163483,0.44175276160240173,-1.312326192855835]},{"input":[true,23.0,0.062429758378326816,false,false,7.484519237992288e-05,false,0.35],"torque":-0.18359272369718022,"active":true,"saturated":false,"log":[0.3724587857723236,0.4668150246143341,0.038829654455184937,0.0,0.013228937983512878,-0.18359272181987762,0.05741000548005104,0.4035511314868927,-1.3907313346862793]},{"input":[true,23.0,0.3142779412120869,false,false,1.4866607435947705e-05,false,0.35],"torque":-0.1731253724193179,"active":true,"saturated":false,"log":[0.3803557753562927,0.47671258449554443,0.0401608981192112,0.0,-0.01904413104057312,-0.17312537133693695,0.011403433978557587,0.36488351225852966,-1.4676415920257568]},{"input":[true,23.0,0.5661954780847456,false,false,-4.5128494431304356e-05,false,0.35],"torque":-0.162982882782955,"active":true,"saturated":false,"log":[0.3889220058917999,0.48744890093803406,0.04152212664484978,0.0,-0.050152093172073364,-0.16298288106918335,-0.034615810960531235,0.32682523131370544,-1.5400222539901733]},{"input":[true,23.0,0.8179024865384856,false,false,-0.00010507345816964415,false,0.35],"torque":-0.15318577946561587,"active":true,"saturated":false,"log":[0.3983229696750641,0.4992314577102661,0.04152212664484978,0.0,-0.08028087764978409,-0.15318578481674194,-0.08059659600257874,0.28958114981651306,-1.607328176498413]},{"input":[true,23.0,1.0691193180142862,false,false,-0.0001649016844307542,false,0.35],"torque":-0.14389549657725528,"active":true,"saturated":false,"log":[0.4083860516548157,0.5118438601493835,0.04152212664484978,0.0,-0.1098632961511612,-0.1438954919576645,-0.1264878362417221,0.25304195284843445,-1.6699150800704956]},{"input":[true,23.0,1.319566868543796,false,false,-0.0002245467035626178,false,0.35],"torque":-0.13521106188221693,"active":true,"saturated":false,"log":[0.41894930601119995,0.5250831246376038,0.04152212664484978,0.0,-0.13925053179264069,-0.1352110654115677,-0.1722385436296463,0.2171081006526947,-1.7281138896942139]},{"input":[true,23.0,1.5689668888361255,false,false,-0.00028394224945784677,false,0.35],"torque":-0.12718033254086183,"active":true,"saturated":false,"log":[0.42986077070236206,0.5387588143348694,0.04152212664484978,0.0,-0.16863244771957397,-0.1271803379058838,-0.2177979052066803,0.18168920278549194,-1.7822322845458984]},{"input":[true,23.0,1.8170422934151076,false,true,-0.0003430223331758737,false,0.35],"torque":-0.11980718022629452,"active":true,"saturated":false,"log":[0.44097793102264404,0.5526923537254333,0.04152212664484978,0.0,-0.1980486661195755,-0.1198071837425232,-0.2631152868270874,0.14670346677303314,-1.8325563669204712]},{"input":[true,23.0,2.0635174684635307,false,true,-0.00040172131625720533,false,0.35],"torque":-0.11305850433411224,"active":true,"saturated":false,"log":[0.45216718316078186,0.566716194152832,0.04152212664484978,0.0,-0.22744914889335632,-0.11305850744247437,-0.3081403374671936,0.11207703500986099,-1.87935209274292]},{"input":[true,23.0,2.308118578032378,false,true,-0.0004599739836482962,false,0.35],"torque":-0.10687305248858521,"active":true,"saturated":false,"log":[0.4633033573627472,0.5806735157966614,0.04152212664484978,0.0,-0.2567526698112488,-0.1068730503320694,-0.35282304883003235,0.07774363458156586,-1.922866940498352]},{"input":[true,23.0,2.5505738682748076,false,true,-0.000517715616156007,false,0.35],"torque":-0.10117145804835197,"active":true,"saturated":false,"log":[0.4742692708969116,0.5944175124168396,0.04152212664484978,0.0,-0.2858818769454956,-0.10117145627737045,-0.39711377024650574,0.04364395886659622,-1.963330864906311]},{"input":[true,23.0,2.7906139693669245,false,true,-0.0005748820623511629,false,0.35],"torque":-0.09586573807679634,"active":true,"saturated":false,"log":[0.48495519161224365,0.6078104972839355,0.04152212664484978,0.0,-0.3147767186164856,-0.09586574137210846,-0.4409632980823517,0.00972529873251915,-2.000957727432251]},{"input":[true,23.0,3.0279721947798546,false,true,-0.0006314098098413157,false,0.35],"torque":-0.09103427597386987,"active":true,"saturated":false,"log":[0.4952585995197296,0.620724081993103,0.04152212664484978,0.0,-0.34383025765419006,-0.09103427827358246,-0.4843229055404663,-0.024058910086750984,-2.0359466075897217]},{"input":[true,23.0,3.262384837570697,false,true,-0.0006872360558335404,false,0.35],"torque":-0.08658546025611076,"active":true,"saturated":false,"log":[0.5050837993621826,0.633038341999054,0.04152212664484978,0.0,-0.3728210926055908,-0.08658546209335327,-0.5271444320678711,-0.05774947628378868,-2.0684821605682373]},{"input":[true,23.0,3.4935914633631,false,false,-0.0007422987769088523,false,0.35],"torque":-0.08253716122023587,"active":true,"saturated":false,"log":[0.5143414735794067,0.6446413397789001,0.04332232102751732,0.0,-0.40165847539901733,-0.08253715932369232,-0.5693802833557129,-0.09138181805610657,-2.0987367630004883]},{"input":[true,23.0,3.721335199691984,false,false,-0.0007965367979307344,false,0.35],"torque":-0.07860737064680859,"active":true,"saturated":false,"log":[0.5229483842849731,0.6554286479949951,0.04515264183282852,0.0,-0.43044015765190125,-0.07860737293958664,-0.6109835505485535,-0.12498631328344345,-2.1268701553344727]},{"input":[true,23.0,3.9453630213909903,false,false,-0.000849889860011222,false,0.35],"torque":-0.07467797534635566,"active":true,"saturated":false,"log":[0.5308272838592529,0.6653035283088684,0.04701053723692894,0.0,-0.45927202701568604,-0.074677973985672,-0.6519080400466919,-0.15858861804008484,-2.1530308723449707]},{"input":[true,23.0,4.1654260317055085,false,false,-0.0009022986874590205,false,0.35],"torque":-0.07068490365893039,"active":true,"saturated":false,"log":[0.537906289100647,0.6741758584976196,0.04889320954680443,0.0,-0.48824918270111084,-0.07068490236997604,-0.6921082139015198,-0.19220998883247375,-2.1773576736450195]},{"input":[true,23.0,4.381279738819022,false,false,-0.0009537050536352881,false,0.35],"torque":-0.06655128996622943,"active":true,"saturated":false,"log":[0.5441190004348755,0.6819624900817871,0.05079762265086174,0.0,-0.51742023229599,-0.06655129045248032,-0.7315394878387451,-0.22586750984191895,-2.1999785900115967]},{"input":[true,23.0,4.592684327485529,false,false,-0.001004051845643908,false,0.35],"torque":-0.06219145932572522,"active":true,"saturated":false,"log":[0.5494040250778198,0.6885863542556763,0.05272053927183151,0.0,-0.5467424988746643,-0.06219146028161049,-0.7701579928398132,-0.2595744729042053,-2.2210137844085693]},{"input":[true,23.0,4.799404925466259,false,false,-0.0010532831277843902,false,0.35],"torque":-0.057521424869968904,"active":true,"saturated":false,"log":[0.5537047386169434,0.6939765810966492,0.05465850606560707,0.0,-0.5760539174079895,-0.05752142518758774,-0.8079208135604858,-0.2933405041694641,-2.2405738830566406]},{"input":[true,23.0,5.0012118644746675,false,false,-0.001101344203696894,false,0.35],"torque":-0.0524723898996505,"active":true,"saturated":false,"log":[0.5569692254066467,0.6980680823326111,0.05660789832472801,0.0,-0.6050930619239807,-0.05247239023447037,-0.8447860479354858,-0.3271718919277191,-2.258762836456299]},{"input":[true,23.0,2.4761865558559117,false,false,-0.0005,false,0.35],"torque":0.006265545450819962,"active":true,"saturated":false,"log":[0.0373692661523819,0.04683614522218704,0.05673868954181671,-0.0,-0.4880206882953644,0.006265545263886452,-0.38352498412132263,-0.34879621863365173,-2.240603446960449]},{"input":[true,23.0,2.4715227631264476,false,false,-0.0004988893003505612,false,0.35],"torque":0.028945505367891797,"active":true,"saturated":false,"log":[0.014375745318830013,0.01801760122179985,0.05678900331258774,-0.0,-0.4931979477405548,0.028945505619049072,-0.3826730251312256,-0.3693130612373352,-2.217897653579712]},{"input":[true,23.0,2.457552105229853,false,false,-0.0004955621360170897,false,0.35],"torque":0.04976596549441602,"active":true,"saturated":false,"log":[-0.009218942373991013,-0.011554407887160778,0.05675673857331276,-0.0,-0.4968046247959137,0.04976596683263779,-0.3801209330558777,-0.38868847489356995,-2.190638780593872]},{"input":[true,23.0,2.434336650985442,false,false,-0.0004900332889206208,false,0.35],"torque":0.06899137112432731,"active":true,"saturated":false,"log":[-0.03336869552731514,-0.041822098195552826,0.05663994699716568,-0.0,-0.4990598261356354,0.06899137049913406,-0.37588003277778625,-0.4068909287452698,-2.1588287353515625]},{"input":[true,23.0,2.4019795419807766,false,false,-0.00048232732261528193,false,0.35],"torque":0.08686068928104237,"active":true,"saturated":false,"log":[-0.058022238314151764,-0.0727212056517601,0.056436870247125626,-0.0,-0.5000101327896118,0.08686068654060364,-0.3699691593647003,-0.4238916039466858,-2.1224780082702637]},{"input":[true,23.0,2.360624534334371,false,false,-0.00047247847315736883,false,0.35],"torque":0.1035651781560625,"active":true,"saturated":false,"log":[-0.08312340825796127,-0.10418134182691574,0.056145939975976944,-0.0,-0.499705970287323,0.10356517881155014,-0.3624145984649658,-0.4396645724773407,-2.0816071033477783]},{"input":[true,23.0,2.3104553600162063,false,false,-0.00046053049700144256,false,0.35],"torque":0.1192581011266841,"active":true,"saturated":false,"log":[-0.10861150175333023,-0.13612641394138336,0.055765796452760696,-0.0,-0.49819642305374146,0.1192580983042717,-0.3532499074935913,-0.4541870057582855,-2.0362462997436523]},{"input":[true,23.0,2.2516949105635944,false,false,-0.00044653647659921467,false,0.35],"torque":0.1340621479140734,"active":true,"saturated":false,"log":[-0.13442160189151764,-0.16847507655620575,0.055295322090387344,-0.0,-0.495526522397995,0.13406214118003845,-0.3425157964229584,-0.4674392640590668,-1.9864356517791748]},{"input":[true,23.0,2.184604246818967,false,false,-0.00043055858456490516,false,0.35],"torque":0.14807511224640282,"active":true,"saturated":false,"log":[-0.1604849398136139,-0.2011411190032959,0.054733626544475555,-0.0,-0.49173635244369507,0.14807511866092682,-0.33025994896888733,-0.47940513491630554,-1.932226538658142]},{"input":[true,23.0,2.109481439089129,false,false,-0.0004126678074548392,false,0.35],"torque":0.16137426855695935,"active":true,"saturated":false,"log":[-0.18672922253608704,-0.2340339571237564,0.05408007279038429,-0.0,-0.48686131834983826,0.1613742709159851,-0.3165368437767029,-0.4900718927383423,-1.873680830001831]},{"input":[true,23.0,2.0266602428789247,false,false,-0.00039294363038847404,false,0.35],"torque":0.17401979851362348,"active":true,"saturated":false,"log":[-0.2130790650844574,-0.2670590877532959,0.05333429574966431,-0.0,-0.48093271255493164,0.17401979863643646,-0.3014073967933655,-0.49943044781684875,-1.8108712434768677]},{"input":[true,23.0,1.93650861608274,false,false,-0.00037147368391202205,false,0.35],"torque":0.1860575095256399,"active":true,"saturated":false,"log":[-0.23945629596710205,-0.3001185655593872,0.05249619856476784,-0.0,-0.47397857904434204,0.18605750799179077,-0.28493890166282654,-0.5074753761291504,-1.7438818216323853]},{"input":[true,23.0,1.8394270842216902,false,false,-0.0003483533546735827,false,0.35],"torque":0.19752104358237604,"active":true,"saturated":false,"log":[-0.2657804489135742,-0.3331114947795868,0.05156596750020981,-0.0,-0.4660244882106781,0.19752104580402374,-0.2672044336795807,-0.5142050385475159,-1.672808051109314]},{"input":[true,23.0,1.735846960989396,false,false,-0.0003236853616394762,false,0.35],"torque":0.2084336868701645,"active":true,"saturated":false,"log":[-0.29196903109550476,-0.3659345209598541,0.050544075667858124,-0.0,-0.4570941925048828,0.20843368768692017,-0.24828284978866577,-0.5196215510368347,-1.597756266593933]},{"input":[true,23.0,1.626228432012109,false,false,-0.0002975792997345639,false,0.35],"torque":0.21880989764773667,"active":true,"saturated":false,"log":[-0.31793808937072754,-0.39848241209983826,0.049431294202804565,-0.0,-0.4472104012966156,0.21880990266799927,-0.2282581925392151,-0.523730993270874,-1.5188441276550293]},{"input":[true,23.0,1.5110585103366574,false,false,-0.00027015115293406986,false,0.35],"torque":0.2284630022755521,"active":true,"saturated":false,"log":[-0.34360241889953613,-0.4306483566761017,0.048228684812784195,-0.0,-0.4333571493625641,0.22846300899982452,-0.2072194367647171,-0.526543140411377,-1.436200499534607]},{"input":[true,23.0,1.390848872729565,false,false,-0.00024152277897013832,false,0.35],"torque":0.23740630711650726,"active":true,"saturated":false,"log":[-0.3688761591911316,-0.4623247981071472,0.046937618404626846,-0.0,-0.41821274161338806,0.23740631341934204,-0.18526004254817963,-0.5280716419219971,-1.3499650955200195]},{"input":[true,23.0,1.2661335864002283,false,false,-0.0002118213679424723,false,0.35],"torque":0.24569028200370724,"active":true,"saturated":false,"log":[-0.3936730623245239,-0.4934035837650299,0.04555976018309593,-0.0,-0.4025006890296936,0.24569028615951538,-0.16247758269309998,-0.5283339619636536,-1.2602885961532593]},{"input":[true,23.0,1.1374667362478705,false,false,-0.00018117887723833681,false,0.35],"torque":0.2533533574560129,"active":true,"saturated":false,"log":[-0.41790705919265747,-0.5237768292427063,0.044097088277339935,-0.0,-0.3862908184528351,0.2533533573150635,-0.13897325098514557,-0.5273512601852417,-1.1673316955566406]},{"input":[true,23.0,1.0054199631739265,false,false,-0.00014973144527246395,true,0.35],"torque":0.2604240271740118,"active":true,"saturated":false,"log":[-0.4414924681186676,-0.5533372163772583,0.04255186393857002,-0.0,-0.36965513229370117,0.26042401790618896,-0.11485150456428528,-0.5251483917236328,-1.0712655782699585]},{"input":[true,23.0,0.8705799243966466,false,false,-0.00011761878665149471,false,0.35],"torque":0.26692259855312844,"active":true,"saturated":false,"log":[-0.4643445611000061,-0.5819784998893738,0.04092665761709213,-0.0,-0.35266751050949097,0.26692259311676025,-0.09021949023008347,-0.5217537879943848,-0.9722713232040405]},{"input":[true,23.0,0.7335456870512322,false,false,-8.498357145012052e-05,false,0.35],"torque":0.2728626275394161,"active":true,"saturated":false,"log":[-0.48637986183166504,-0.6095960736274719,0.039224326610565186,-0.0,-0.3354035019874573,0.27286261320114136,-0.06518664956092834,-0.5171992778778076,-0.8705390691757202]},{"input":[true,23.0,0.5949260666552059,false,false,-5.197079135667549e-05,false,0.35],"torque":0.27825211158496627,"active":true,"saturated":false,"log":[-0.5075166821479797,-0.6360875964164734,0.03744801878929138,-0.0,-0.31793972849845886,0.27825212478637695,-0.0398641936480999,-0.5115200877189636,-0.7662681937217712]},{"input":[true,23.0,0.4553369222636838,false,false,-1.872711550426666e-05,false,0.35],"torque":0.28309445502526576,"active":true,"saturated":false,"log":[-0.5276752710342407,-0.6613529920578003,0.03560115769505501,-0.0,-0.3003538250923157,0.28309446573257446,-0.014364631846547127,-0.5047547817230225,-0.6596662402153015]},{"input":[true,23.0,0.31539842033163185,false,false,1.4599761150644409e-05,false,0.35],"torque":0.28738929762458754,"active":true,"saturated":false,"log":[-0.5467785000801086,-0.6852957010269165,0.03368743136525154,-0.0,-0.28272390365600586,0.2873893082141876,0.011198748834431171,-0.49694472551345825,-0.5509489178657532]},{"input":[true,23.0,0.17573227943920505,false,false,4.786177400718783e-05,false,0.35],"torque":0.29113318499890894,"active":true,"saturated":false,"log":[-0.5647518038749695,-0.7078222632408142,0.031710799783468246,-0.0,-0.26512831449508667,0.29113319516181946,0.03671237453818321,-0.4881344437599182,-0.44033899903297424]},{"input":[true,23.0,0.036959008121314196,false,false,8.091114664128603e-05,false,0.35],"torque":0.29432020191116537,"active":true,"saturated":false,"log":[-0.5815240144729614,-0.7288434505462646,0.029675466939806938,-0.0,-0.24764518439769745,0.2943201959133148,0.06206289678812027,-0.4783709943294525,-0.3280661106109619]},{"input":[true,23.0,-0.1003048519268237,false,false,0.00011360104734654355,false,0.35],"torque":0.29694280690615477,"active":true,"saturated":false,"log":[-0.5970273017883301,-0.7482742071151733,0.027585869655013084,-0.0,-0.2303522229194641,0.296942800283432,0.08713768422603607,-0.46770408749580383,-0.21436578035354614]},{"input":[true,23.0,-0.23544946502026426,false,false,0.0001457862414779463,false,0.35],"torque":0.29899155857231274,"active":true,"saturated":false,"log":[-0.6111977696418762,-0.7660345435142517,0.025446677580475807,-0.0,-0.21332626044750214,0.2989915609359741,0.11182533949613571,-0.45618563890457153,-0.09947902709245682]},{"input":[true,23.0,-0.3678744108614617,false,false,0.0001773237367001388,false,0.35],"torque":0.300456702517262,"active":true,"saturated":false,"log":[-0.6239755749702454,-0.7820493578910828,0.023262763395905495,-0.0,-0.1966429203748703,0.300456702709198,0.13601617515087128,-0.4438697397708893,0.0163485836237669]},{"input":[true,23.0,-0.057131761096165634,false,false,0.0002080734182735712,false,0.35],"torque":0.2868511655499554,"active":true,"saturated":false,"log":[-0.5488443374633789,-0.6878849267959595,0.021341808140277863,-0.0,-0.1516585499048233,0.286851167678833,0.0792510062456131,-0.43081235885620117,0.13286752998828888]},{"input":[true,23.0,-0.11931756326969212,false,false,0.0002378986715560677,false,0.35],"torque":0.27443479435460677,"active":true,"saturated":false,"log":[-0.5462819337844849,-0.6846733689308167,0.019429821521043777,-0.0,-0.1261308193206787,0.27443480491638184,0.09061084687709808,-0.4170711636543274,0.24982516467571259]},{"input":[true,23.0,-0.17929964884122723,false,false,0.00026666698895416686,false,0.35],"torque":0.2626885425025071,"active":true,"saturated":false,"log":[-0.5426140427589417,-0.6800762414932251,0.017530672252178192,-0.0,-0.10130014270544052,0.262688547372818,0.10156812518835068,-0.4027051627635956,0.3669664263725281]},{"input":[true,23.0,-0.23681152948510392,false,false,0.0002942505586276729,false,0.35],"torque":0.25124811792022433,"active":true,"saturated":false,"log":[-0.5378531217575073,-0.6741092205047607,0.015648186206817627,-0.0,-0.07684924453496933,0.2512481212615967,0.11207415163516998,-0.38777461647987366,0.4840347170829773]},{"input":[true,23.0,-0.29159769149864306,false,false,0.0003205268323319191,false,0.35],"torque":0.23985648058037495,"active":true,"saturated":false,"log":[-0.5320147275924683,-0.6667917966842651,0.013786135241389275,-0.0,-0.05247778445482254,0.23985648155212402,0.12208226323127747,-0.37234067916870117,0.6007725596427917]},{"input":[true,23.0,-0.34341473099807457,false,false,0.00034537906987493823,false,0.35],"torque":0.16883931823337978,"active":true,"saturated":false,"log":[-0.18202754855155945,-0.228141188621521,0.013786135241389275,-0.0,0.07821235805749893,0.16883932054042816,0.1315479874610901,-0.037617627531290054,0.6482353806495667]},{"input":[true,23.0,-0.3920324353129702,false,false,0.0003686968577706227,false,0.35],"torque":0.11973519239739308,"active":true,"saturated":false,"log":[-0.17344382405281067,-0.21738292276859283,0.013786135241389275,-0.0,0.09751655906438828,0.11973518878221512,0.14042925834655762,-0.02075914666056633,0.6947236657142639]},{"input":[true,23.0,-0.4372348057747744,false,false,0.0003903765997836073,false,0.35],"torque":0.08483428530180535,"active":true,"saturated":false,"log":[-0.16342219710350037,-0.20482248067855835,0.013786135241389275,-0.0,0.11646263301372528,0.08483428508043289,0.14868664741516113,-0.003188258968293667,0.7398974299430847]},{"input":[true,23.0,-0.47882101735538335,false,false,0.0004103219771864765,false,0.35],"torque":0.0591614271482731,"active":true,"saturated":false,"log":[-0.15205398201942444,-0.19057431817054749,0.013253945857286453,-0.0,0.13518892228603363,0.05916142836213112,0.156283438205719,0.014973489567637444,0.7834317088127136]},{"input":[true,23.0,-0.5166063108923018,false,false,0.00042844437668447364,false,0.35],"torque":0.039408843466296466,"active":true,"saturated":false,"log":[-0.13943317532539368,-0.1747562438249588,0.012765930034220219,-0.0,0.15338566899299622,0.0394088439643383,0.16318589448928833,0.033604975789785385,0.825017511844635]},{"input":[true,23.0,-0.5504228139363869,false,false,0.0004446632841065207,false,0.35],"torque":0.023427686584555622,"active":true,"saturated":false,"log":[-0.12565630674362183,-0.15748924016952515,0.0123261334374547,-0.0,0.1708976775407791,0.023427685722708702,0.1693633496761322,0.05258582904934883,0.8643627166748047]},{"input":[true,23.0,-0.5801202865753345,false,false,0.0004589066421134661,false,0.35],"torque":0.009825647858507872,"active":true,"saturated":false,"log":[-0.11082223057746887,-0.13889719545841217,0.011938255280256271,-0.0,0.18763546645641327,0.009825647808611393,0.17478837072849274,0.07179675251245499,0.9011925458908081]},{"input":[true,23.0,-0.6055667889193468,false,false,0.000471111170334329,false,0.35],"torque":-0.00229694765854108,"active":true,"saturated":false,"log":[-0.09503181278705597,-0.11910653859376907,0.011605643667280674,-0.0,0.20351754128932953,-0.002296947641298175,0.1794368177652359,0.09111989289522171,0.9352505207061768]},{"input":[true,23.0,-0.6266492672834922,false,false,0.00048122264650824387,false,0.35],"torque":-0.008020878494809418,"active":true,"saturated":false,"log":[-0.07838774472475052,-0.09824597090482712,0.01133128721266985,-0.0,0.2184692621231079,-0.00802087876945734,0.1832880824804306,0.11043916642665863,0.9662989974021912]},{"input":[true,23.0,-0.6432740564624573,false,false,0.0004891961473830436,false,0.35],"torque":-0.014453578584421593,"active":true,"saturated":false,"log":[-0.060994215309619904,-0.07644608616828918,0.011117806658148766,-0.0,0.23242130875587463,-0.014453578740358353,0.18632502853870392,0.1296406239271164,0.9941197037696838]},{"input":[true,23.0,-0.6553672958661898,false,false,0.0004949962483002228,false,0.35],"torque":-0.021499381531219795,"active":true,"saturated":false,"log":[-0.0429566353559494,-0.05383898317813873,0.010967458598315716,-0.0,0.2453078329563141,-0.021499382331967354,0.18853417038917542,0.1486128270626068,1.0185142755508423]},{"input":[true,23.0,-0.6628752576676245,false,false,0.0004985971805795595,false,0.35],"torque":-0.029075099168972406,"active":true,"saturated":false,"log":[-0.024381298571825027,-0.030557895079255104,0.010882124304771423,-0.0,0.2570643126964569,-0.029075099155306816,0.18990570306777954,0.16724716126918793,1.0393049716949463]},{"input":[true,23.0,-0.6657645855045977,false,false,0.0004999829460041724,false,0.35],"torque":-0.03710576935560456,"active":true,"saturated":false,"log":[-0.005375050939619541,-0.006736730691045523,0.01086331158876419,-0.0,0.26762545108795166,-0.03710576891899109,0.19043350219726562,0.18543826043605804,1.0563347339630127]},{"input":[true,23.0,-0.6640224426754473,false,false,0.0004991473878973766,false,0.35],"torque":-0.04551430400716395,"active":true,"saturated":false,"log":[0.013955047354102135,0.0174903254956007,0.010912153869867325,-0.0,0.27692368626594543,-0.0455143041908741,0.19011525809764862,0.20308424532413483,1.0694680213928223]},{"input":[true,23.0,-0.6576565691698917,false,false,0.000496094218475557,false,0.35],"torque":-0.05422612263894313,"active":true,"saturated":false,"log":[0.033502086997032166,0.0419892817735672,0.011029412038624287,-0.0,0.28488868474960327,-0.05422612279653549,0.18895237147808075,0.2200872153043747,1.0785905122756958]},{"input":[true,23.0,-0.6466952472818144,false,false,0.0004908370023555395,false,0.35],"torque":-0.06316731335188885,"active":true,"saturated":false,"log":[0.05315966159105301,0.06662677228450775,0.011215470731258392,-0.0,0.2914487421512604,-0.06316731125116348,0.18694999814033508,0.23635344207286835,1.083609700202942]},{"input":[true,23.0,-0.6311871759567298,false,false,0.00048339909628973055,false,0.35],"torque":-0.0722602889796107,"active":true,"saturated":false,"log":[0.07282225787639618,0.0912705659866333,0.011470348574221134,-0.0,0.29653334617614746,-0.0722602903842926,0.18411704897880554,0.2517937421798706,1.084455132484436]},{"input":[true,23.0,-0.6112012544321745,false,false,0.00047381354539676926,false,0.35],"torque":-0.08142279953439643,"active":true,"saturated":false,"log":[0.09238560497760773,0.11578995734453201,0.01179369818419218,-0.0,0.30007705092430115,-0.08142279833555222,0.18046610057353973,0.2663238048553467,1.081078052520752]},{"input":[true,23.0,-0.5868262761322731,false,false,0.0004621229363487218,false,0.35],"torque":-0.09056799723993669,"active":true,"saturated":false,"log":[0.11174706369638443,0.1400563269853592,0.012184812687337399,-0.0,0.3020232915878296,-0.09056799858808517,0.17601338028907776,0.27986449003219604,1.0734517574310303]},{"input":[true,23.0,-0.558170534176436,false,false,0.00044837920816707353,false,0.35],"torque":-0.09960542400523772,"active":true,"saturated":false,"log":[0.13080598413944244,0.16394349932670593,0.01264263316988945,-0.0,0.3023279011249542,-0.09960542619228363,0.17077867686748505,0.29234200716018677,1.0615712404251099]},{"input":[true,23.0,-0.5253613402548429,false,false,0.00043264342146811797,false,0.35],"torque":-0.10844266828752985,"active":true,"saturated":false,"log":[0.14946411550045013,0.18732835352420807,0.013165757991373539,-0.0,0.3009609580039978,-0.10844267159700394,0.1647852212190628,0.30368831753730774,1.0454533100128174]},{"input":[true,23.0,-0.48854445900824167,false,false,0.00041498548718293574,false,0.35],"torque":-0.11698736746986121,"active":true,"saturated":false,"log":[0.16762588918209076,0.21009111404418945,0.013752448372542858,0.0,0.29790762066841125,-0.11698736995458603,0.15805967152118683,0.31384122371673584,1.02513587474823]},{"input":[true,23.0,-0.4478834604250091,false,false,0.00039548385595720845,false,0.35],"torque":-0.12514926905107682,"active":true,"saturated":false,"log":[0.18519888818264008,0.2321159392595291,0.014400645159184933,0.0,0.2931673526763916,-0.12514926493167877,0.1506318897008896,0.322744756937027,1.0006780624389648]},{"input":[true,23.0,-0.4035589931326334,false,false,0.00037422516961079937,false,0.35],"torque":-0.13284209920482118,"active":true,"saturated":false,"log":[0.20209407806396484,0.2532912492752075,0.015107974410057068,0.0,0.2867526412010193,-0.13284209370613098,0.14253488183021545,0.3303491473197937,0.9721593856811523]},{"input":[true,23.0,-0.35576798181224784,false,false,0.0003513038762055989,false,0.35],"torque":-0.13998510931183505,"active":true,"saturated":false,"log":[0.21822625398635864,0.27351024746894836,0.015871765092015266,0.0,0.27868717908859253,-0.1399851143360138,0.13380461931228638,0.33661115169525146,0.9396796822547913]},{"input":[true,23.0,-0.3047227523019314,false,false,0.00032682181043180595,false,0.35],"torque":-0.14650422338551092,"active":true,"saturated":false,"log":[0.23351427912712097,0.29267123341560364,0.016689065843820572,0.0,0.26900389790534973,-0.1465042233467102,0.12447988986968994,0.34149420261383057,0.9033581018447876]},{"input":[true,23.0,-0.25065008827577556,false,false,0.0003008877411769109,false,0.35],"torque":-0.15233279848949974,"active":true,"saturated":false,"log":[0.2478814274072647,0.31067806482315063,0.017556650564074516,0.0,0.2577431797981262,-0.15233279764652252,0.11460212618112564,0.34496843814849854,0.8633326888084412]},{"input":[true,23.0,-0.19379022368969118,false,false,0.00027361688828743294,false,0.35],"torque":-0.15741204051337573,"active":true,"saturated":false,"log":[0.2612557113170624,0.3274405002593994,0.018471045419573784,0.0,0.2449510544538498,-0.15741203725337982,0.10421520471572876,0.3470107913017273,0.8197594881057739]},{"input":[true,23.0,-0.13439577547033138,false,false,0.0002451304106703497,false,0.35],"torque":-0.16169112799428245,"active":true,"saturated":false,"log":[0.27357012033462524,0.34287455677986145,0.019428541883826256,0.0,0.2306772917509079,-0.16169112920761108,0.09336527436971664,0.3476051390171051,0.772812008857727]},{"input":[true,23.0,-0.07273062118895907,false,false,0.00021555486800847306,false,0.35],"torque":-0.16512709823924632,"active":true,"saturated":false,"log":[0.2847628593444824,0.3569027781486511,0.020425211638212204,0.0,0.21497304737567902,-0.16512709856033325,0.08210054039955139,0.34674227237701416,0.7226800322532654]},{"input":[false,23.0,-0.009068726706545793,false,false,0.00018502165848126905,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.05630707100137239,false,false,0.0001536664349892097,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.12310632043240999,false,false,0.00012162850247525857,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.19103224597486435,false,false,8.905019902106331e-05,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.25978306642198434,false,false,5.6076263467527434e-05,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.3290533357250939,false,false,2.2853192369290225e-05,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.39853530002886506,false,false,-1.0471410859959058e-05,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.46792026495972083,false,false,-4.37494917197232e-05,true,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.536899967092761,false,false,-7.683320239902244e-05,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.6051679435040762,false,false,-0.00010957555863444268,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.672420893323793,false,false,-0.00014183109273161314,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.7383600252407583,false,false,-0.00017345649984887746,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.8026923849721548,false,false,-0.00020431127467184204,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.8651321568003763,false,false,-0.00023425833565018857,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,23.0,0.9254019333947003,false,false,-0.00026316463402339594,false,0.35],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,23.0,0.9832339482761874,false,false,-0.00029090174492958534,false,0.35],"torque":-0.03779011176544307,"active":true,"saturated":false,"log":[0.3696204721927643,0.46325767040252686,0.0012936716666445136,0.0,-0.07688579708337784,-0.03779011219739914,-0.1107986569404602,0.23270468413829803,-0.08328093588352203]},{"input":[true,23.0,1.038371265450217,false,false,-0.00031734643797131734,false,0.35],"torque":-0.048082179852388766,"active":true,"saturated":false,"log":[0.35214367508888245,0.4413534104824066,0.0012936716666445136,0.0,-0.09739600867033005,-0.04808218032121658,-0.1208709105849266,0.20639052987098694,-0.16390679776668549]},{"input":[true,23.0,1.0905689209213631,false,false,-0.00034238122470342736,false,0.35],"torque":-0.05418854835075351,"active":true,"saturated":false,"log":[0.33390259742736816,0.4184912443161011,0.0012936716666445136,0.0,-0.1169440969824791,-0.054188549518585205,-0.13040615618228912,0.17990311980247498,-0.2416795939207077]},{"input":[true,23.0,1.1395950110190929,false,false,-0.0003658948806105272,false,0.35],"torque":-0.05697462663454102,"active":true,"saturated":false,"log":[0.3149222433567047,0.3947025537490845,0.0012936716666445136,0.0,-0.1354907751083374,-0.0569746270775795,-0.13936203718185425,0.15330800414085388,-0.31640276312828064]},{"input":[true,23.0,1.185231722699067,false,false,-0.00038778293925512483,false,0.35],"torque":-0.056861427628792455,"active":true,"saturated":false,"log":[0.29523468017578125,0.37002745270729065,0.0012936716666445136,0.0,-0.157588928937912,-0.056861426681280136,-0.1476987600326538,0.1266748458147049,-0.38788285851478577]},{"input":[true,23.0,1.2272763012426275,false,false,-0.0004079481564009665,false,0.35],"torque":-0.05225124174429919,"active":true,"saturated":false,"log":[0.27487891912460327,0.3445149064064026,0.0012936716666445136,0.0,-0.17976875603199005,-0.052251242101192474,-0.1553792953491211,0.10007686913013458,-0.455931156873703]},{"input":[true,23.0,1.2655419510551775,false,false,-0.00042630094204959286,false,0.35],"torque":-0.04477515663597809,"active":true,"saturated":false,"log":[0.2539002597332001,0.31822165846824646,0.0012936716666445136,0.0,-0.20156587660312653,-0.044775158166885376,-0.162369504570961,0.07359034568071365,-0.5203650593757629]},{"input":[true,23.0,1.2998586655613877,false,false,-0.00044275975847065947,false,0.35],"torque":-0.035496691711023196,"active":true,"saturated":false,"log":[0.2323499619960785,0.29121196269989014,0.0012936716666445136,0.0,-0.2227838784456253,-0.03549669310450554,-0.16863833367824554,0.04729394242167473,-0.5810098052024841]},{"input":[true,23.0,1.3300739825101702,false,false,-0.00045725148245765016,false,0.35],"torque":-0.025117128250907846,"active":true,"saturated":false,"log":[0.2102847695350647,0.26355689764022827,0.0012936716666445136,0.0,-0.2431773543357849,-0.02511712908744812,-0.17415794730186462,0.021268244832754135,-0.6376994252204895]},{"input":[true,23.0,1.3560536613337548,false,false,-0.00046971173019955657,false,0.35],"torque":-0.014092081419865812,"active":true,"saturated":false,"log":[0.1877664178609848,0.2353339046239853,0.0012936716666445136,0.0,-0.2626195549964905,-0.014092081226408482,-0.17890380322933197,-0.004404833074659109,-0.6902784109115601]},{"input":[true,23.0,1.3776822795514934,false,false,-0.000480085143325183,false,0.35],"torque":-0.002701803801494533,"active":true,"saturated":false,"log":[0.16486123204231262,0.2066260725259781,0.0018706859555095434,0.0,-0.2812608778476715,-0.002701803809031844,-0.18285483121871948,-0.029642580077052116,-0.7386026978492737]},{"input":[true,23.0,1.3948637455686967,false,false,-0.0004883256348492412,false,0.35],"torque":0.00890393164013462,"active":true,"saturated":false,"log":[0.1416396051645279,0.17752163112163544,0.002366424538195133,0.0,-0.2986440658569336,0.008903931826353073,-0.18599346280097961,-0.054362017661333084,-0.7825409770011902]},{"input":[true,23.0,1.4075217255922368,false,false,-0.0004943965939275431,false,0.35],"torque":0.01487210496968884,"active":true,"saturated":false,"log":[0.11817554384469986,0.14811335504055023,0.00278003909625113,0.0,-0.31471168994903564,0.014872104860842228,-0.18830576539039612,-0.07848042994737625,-0.8219754099845886]},{"input":[true,23.0,1.4155999827662142,false,false,-0.0004982710485116088,false,0.35],"torque":0.02189274469111794,"active":true,"saturated":false,"log":[0.09454616904258728,0.11849786341190338,0.003110950579866767,0.0,-0.3294202983379364,0.021892745047807693,-0.18978147208690643,-0.1019158661365509,-0.8568028211593628]},{"input":[true,23.0,1.419062627020984,false,false,-0.0004999317851800449,false,0.35],"torque":0.02976394799310369,"active":true,"saturated":false,"log":[0.07083124667406082,0.0887751653790474,0.003358860034495592,0.0,-0.3427334725856781,0.029763948172330856,-0.19041401147842407,-0.12458765506744385,-0.8869354724884033]},{"input":[true,23.0,1.4178942745255068,false,false,-0.0004993714256143098,false,0.35],"torque":0.03832923018432064,"active":true,"saturated":false,"log":[0.04711268097162247,0.059047892689704895,0.003523754421621561,0.0,-0.3546166718006134,0.03832922875881195,-0.1902005821466446,-0.14641685783863068,-0.9123017191886902]},{"input":[true,23.0,1.412100116034611,false,false,-0.0004965924593790963,false,0.35],"torque":0.047455942149833445,"active":true,"saturated":false,"log":[0.023474035784602165,0.029420791193842888,0.003605913370847702,0.0,-0.36503341794013977,0.04745594039559364,-0.1891421377658844,-0.16732676327228546,-0.9328467845916748]},{"input":[true,23.0,1.401705893827515,false,false,-0.0004916072328616975,false,0.35],"torque":0.05702682455577872,"active":true,"saturated":false,"log":[6.035823929551043e-08,7.564899107137535e-08,0.0036059136036783457,0.0,-0.3739423155784607,0.05702682584524155,-0.18724335730075836,-0.1872432976961136,-0.9485332369804382]},{"input":[true,23.0,1.386757787340066,false,false,-0.0004844378944194944,false,0.35],"torque":0.06691921800619882,"active":true,"saturated":false,"log":[-0.023223815485835075,-0.02910718135535717,0.003524630330502987,0.0,-0.381296306848526,0.06691921502351761,-0.18451270461082458,-0.20609553158283234,-0.9593415260314941]},{"input":[true,23.0,1.3673222079988048,false,false,-0.0004751162959792648,false,0.35],"torque":0.07702566509449246,"active":true,"saturated":false,"log":[-0.04611194506287575,-0.05779363960027695,0.003363238414749503,0.0,-0.38704413175582886,0.07702566683292389,-0.18096229434013367,-0.22381599247455597,-0.9652700424194336]}]},{"name":"delay_zero","frames":[{"input":[false,15.0,2.774576177669794,false,false,-0.0013323183357528668,true,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.700646132081303,false,false,-0.001291240963619141,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.6241341658216712,false,false,-0.0012487290121424167,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.5451252843154335,false,false,-0.0012048297124508391,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.4637072670805225,false,false,-0.0011595918370278437,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.379970570204504,false,false,-0.0011130656455255266,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.294008225847118,false,false,-0.0010653028289257553,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.205915738880781,false,false,-0.0010163564521110636,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.1157909807838893,false,false,-0.0009662808949091303,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,2.0237340809048,false,false,-0.000915131791676342,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.9298473152172992,false,false,-0.0008629659694875654,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.834234992691154,false,false,-0.0008098413850007994,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.737003339403988,false,false,-0.0007558170600668505,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.6382603805232352,false,false,-0.000700953016155571,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.538115820289286,false,false,-0.0006453102076715109,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.4366809201331778,false,false,-0.000588950454233074,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.3340683750642255,false,false,-0.0005319363719904111,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.2303921884649398,false,false,-0.0004743313040583631,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.125767545432329,false,false,-0.0004161992501417374,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,1.0203106848063095,false,false,-0.00035760479543111017,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,15.0,0.914138770027396,false,false,-0.000298613038848147,false,0.0],"torque":0.05593774188759978,"active":true,"saturated":false,"log":[-0.03818652033805847,-0.07637304067611694,-0.00013365282211452723,-0.0,-0.14447635412216187,0.055937740951776505,-0.048375312238931656,-0.08046107739210129,0.09306055307388306]},{"input":[true,15.0,0.8073697589671556,false,false,-0.00023928952072016532,false,0.0],"torque":0.06019128987465609,"active":true,"saturated":false,"log":[-0.03382735699415207,-0.06765471398830414,-0.00013365282211452723,-0.0,-0.1291026473045349,0.06019128859043121,-0.03876490145921707,-0.06718793511390686,0.1801198124885559]},{"input":[true,15.0,0.7001222728760401,false,false,-0.00017970014996429066,false,0.0],"torque":0.05512193390565403,"active":true,"saturated":false,"log":[-0.029430611059069633,-0.05886122211813927,-0.00013365282211452723,-0.0,-0.11368907988071442,0.05512193217873573,-0.02911142259836197,-0.053840141743421555,0.26149457693099976]},{"input":[true,15.0,0.5925154645942011,false,false,-0.00011991113086210739,false,0.0],"torque":0.045943383971472476,"active":true,"saturated":false,"log":[-0.02500116638839245,-0.0500023327767849,-0.00013365282211452723,-0.0,-0.0901632010936737,0.04594338312745094,-0.01942560262978077,-0.040432535111904144,0.3374789357185364]},{"input":[true,15.0,0.48466888617170223,false,false,-5.998888950615651e-05,false,0.0],"torque":0.036037055536090115,"active":true,"saturated":false,"log":[-0.02054394595324993,-0.04108789190649986,-0.00013365282211452723,-0.0,-0.06685668230056763,0.036037053912878036,-0.009718199260532856,-0.026980005204677582,0.4083460569381714]},{"input":[true,15.0,0.37670235604520796,false,false,0.0,false,0.0],"torque":0.026065769540526914,"active":true,"saturated":false,"log":[-0.016063900664448738,-0.032127801328897476,-0.00013365282211452723,-0.0,-0.043836966156959534,0.026065768674016,1.1330300031531237e-09,-0.013497499749064445,0.474349707365036]},{"input":[true,15.0,0.26873582591871376,false,false,5.998888950615651e-05,false,0.0],"torque":0.016180032972972536,"active":true,"saturated":false,"log":[-0.011566009372472763,-0.023132018744945526,-0.00013365282211452723,-0.0,-0.02119751088321209,0.01618003286421299,0.009718201123178005,0.0,0.5357257127761841]},{"input":[true,15.0,0.16088924749621483,false,false,0.00011991113086210739,false,0.0],"torque":0.0063766868134467174,"active":true,"saturated":false,"log":[-0.007055267225950956,-0.014110534451901913,-0.0001583462581038475,-0.0,0.0012294702464714646,0.006376686971634626,0.01942560449242592,0.013497499749064445,0.5926933288574219]},{"input":[true,15.0,0.053282439214375726,false,false,0.00017970014996429066,false,0.0],"torque":-0.003307927920632605,"active":true,"saturated":false,"log":[-0.0025366870686411858,-0.0050733741372823715,-0.00016722465807106346,-0.0,0.023179542273283005,-0.003307927865535021,0.029111426323652267,0.026980005204677582,0.6454567313194275]},{"input":[true,15.0,-0.053965046876739586,false,false,0.00023928952072016532,false,0.0],"torque":-0.01227541879373443,"active":true,"saturated":false,"log":[0.001984711503610015,0.00396942300722003,-0.0001602781703695655,-0.0,0.04464808478951454,-0.01227541919797659,0.03876490518450737,0.040432535111904144,0.6942058205604553]},{"input":[true,15.0,-0.16073405793697992,false,false,0.000298613038848147,false,0.0],"torque":-0.021569425372305877,"active":true,"saturated":false,"log":[0.0065039051696658134,0.013007810339331627,-0.00013751450751442462,-0.0,0.06563970446586609,-0.021569425240159035,0.048375312238931656,0.053840141743421555,0.7391176819801331]},{"input":[true,15.0,-0.2669059727158934,false,false,0.00035760479543111017,false,0.0],"torque":-0.03084467265725959,"active":true,"saturated":false,"log":[0.011015872471034527,0.022031744942069054,-9.895894618239254e-05,-0.0,0.08616113662719727,-0.03084467351436615,0.05793197825551033,0.06718793511390686,0.7803572416305542]},{"input":[true,15.0,-0.37236283334191306,false,false,0.0004161992501417374,false,0.0],"torque":-0.04001892360787725,"active":true,"saturated":false,"log":[0.015515602193772793,0.031031204387545586,-4.4654341763816774e-05,-0.0,0.10621857643127441,-0.040018923580646515,0.06742428243160248,0.08046107739210129,0.8180787563323975]},{"input":[true,15.0,-0.47698747637452404,false,false,0.0004743313040583631,false,0.0],"torque":-0.04907306364711106,"active":true,"saturated":false,"log":[0.01999809220433235,0.0399961844086647,2.5338980776723474e-05,-0.0,0.1258169561624527,-0.04907306283712387,0.07684167474508286,0.09364482760429382,0.8524260520935059]},{"input":[true,15.0,-0.5806636629738096,false,false,0.0005319363719904111,false,0.0],"torque":-0.05800287381882327,"active":true,"saturated":false,"log":[0.02445836551487446,0.04891673102974892,0.00011094326328020543,-0.0,0.14495976269245148,-0.05800287425518036,0.08617369085550308,0.10672454535961151,0.8835338354110718]},{"input":[true,15.0,-0.6832762080427615,false,false,0.000588950454233074,false,0.0],"torque":-0.06680735720912837,"active":true,"saturated":false,"log":[0.028891464695334435,0.05778292939066887,0.00021206338715273887,-0.0,0.16364911198616028,-0.06680735945701599,0.09540997445583344,0.11968568712472916,0.9115283489227295]},{"input":[true,15.0,-0.7847111081988698,false,false,0.0006453102076715109,false,0.0],"torque":-0.07548586628866281,"active":true,"saturated":false,"log":[0.033292464911937714,0.06658492982387543,0.0003285870188847184,-0.0,0.1818857192993164,-0.07548586279153824,0.10454025119543076,0.1325138509273529,0.9365278482437134]},{"input":[true,15.0,-0.8848556684328188,false,false,0.000700953016155571,false,0.0],"torque":-0.08403732687963714,"active":true,"saturated":false,"log":[0.037656478583812714,0.07531295716762543,0.00046038467553444207,-0.0,0.19966883957386017,-0.08403732627630234,0.11355438828468323,0.14519479870796204,0.9586436152458191]},{"input":[true,15.0,-0.9835986273135724,false,false,0.0007558170600668505,false,0.0],"torque":-0.0924599491789143,"active":true,"saturated":false,"log":[0.04197865352034569,0.08395730704069138,0.0006073099793866277,-0.0,0.2169962078332901,-0.09245994687080383,0.12244236469268799,0.15771442651748657,0.9779803156852722]},{"input":[true,15.0,-1.0808302806007382,false,false,0.0008098413850007994,false,0.0],"torque":-0.10075105989293678,"active":true,"saturated":false,"log":[0.04625419154763222,0.09250838309526443,0.0007691996288485825,-0.0,0.23386389017105103,-0.10075105726718903,0.1311943084001541,0.17005883157253265,0.994636595249176]},{"input":[true,15.0,-1.1764426031268833,false,false,0.0008629659694875654,false,0.0],"torque":-0.10890695504982763,"active":true,"saturated":false,"log":[0.050478339195251465,0.10095667839050293,0.0009458738495595753,-0.0,0.2502662241458893,-0.10890695452690125,0.13980048894882202,0.18221430480480194,1.008705735206604]},{"input":[true,15.0,-1.2703293688143842,false,false,0.000915131791676342,false,0.0],"torque":-0.11692278811618817,"active":true,"saturated":false,"log":[0.05464640632271767,0.10929281264543533,0.001137136248871684,-0.0,0.2661956548690796,-0.1169227883219719,0.14825135469436646,0.1941673457622528,1.0202759504318237]},{"input":[true,15.0,-1.3623862686934733,false,false,0.0009662808949091303,false,0.0],"torque":-0.12479248495517835,"active":true,"saturated":false,"log":[0.058753762394189835,0.11750752478837967,0.0013427743688225746,-0.0,0.2816427946090698,-0.12479248642921448,0.1565375030040741,0.20590464770793915,1.0294311046600342]},{"input":[true,15.0,-1.452511026790365,false,false,0.0010163564521110636,false,0.0],"torque":-0.13250870872235754,"active":true,"saturated":false,"log":[0.06279584020376205,0.1255916804075241,0.0015625598607584834,-0.0,0.2965964376926422,-0.13250871002674103,0.16464973986148834,0.21741320192813873,1.0362508296966553]},{"input":[true,15.0,-1.5406035137567025,false,false,0.0010653028289257553,false,0.0],"torque":-0.14006289174615744,"active":true,"saturated":false,"log":[0.06676815450191498,0.13353630900382996,0.0017962483689188957,-0.0,0.3110436499118805,-0.14006289839744568,0.1725790649652481,0.22868020832538605,1.0408110618591309]},{"input":[true,15.0,-1.626565858114089,false,false,0.0011130656455255266,false,0.0],"torque":-0.14744531640954767,"active":true,"saturated":false,"log":[0.07066628336906433,0.14133256673812866,0.0020435804035514593,-0.0,0.32497015595436096,-0.14744532108306885,0.18031664192676544,0.23969313502311707,1.043184518814087]},{"input":[true,15.0,-1.7103025549901065,false,false,0.0011595918370278437,false,0.0],"torque":-0.15464528348134618,"active":true,"saturated":false,"log":[0.0744859054684639,0.1489718109369278,0.002304281108081341,-0.0,0.33836060762405396,-0.15464527904987335,0.1878538727760315,0.25043976306915283,1.0434406995773315]},{"input":[true,15.0,-1.791720572225018,false,false,0.0012048297124508391,false,0.0],"torque":-0.16165130763630214,"active":true,"saturated":false,"log":[0.07822276651859283,0.15644553303718567,0.0025780608411878347,-0.0,0.35119906067848206,-0.16165131330490112,0.1951824128627777,0.2609081566333771,1.0416467189788818]},{"input":[true,15.0,-1.8707294537312558,false,false,0.0012487290121424167,false,0.0],"torque":-0.16845138921725367,"active":true,"saturated":false,"log":[0.08187273144721985,0.1637454628944397,0.0028646152932196856,-0.0,0.36346933245658875,-0.16845138370990753,0.20229409635066986,0.2710866928100586,1.0378668308258057]},{"input":[true,15.0,-1.9472414199908872,false,false,0.001291240963619141,false,0.0],"torque":-0.17503326594150098,"active":true,"saturated":false,"log":[0.08543173223733902,0.17086346447467804,0.003163626417517662,-0.0,0.37515565752983093,-0.17503327131271362,0.2091810405254364,0.2809640169143677,1.032163381576538]},{"input":[true,15.0,-2.021171465579379,false,false,0.0013323183357528668,false,0.0],"torque":-0.18138470364657766,"active":true,"saturated":false,"log":[0.08889581263065338,0.17779162526130676,0.0034747617319226265,-0.0,0.38624292612075806,-0.18138469755649567,0.2158355712890625,0.2905292212963104,1.0245966911315918]},{"input":[true,15.0,-2.092437453607321,false,false,0.0013719154912449113,false,0.0],"torque":-0.1874937567482437,"active":true,"saturated":false,"log":[0.09226112812757492,0.18452225625514984,0.0037976757157593966,-0.0,0.3967171311378479,-0.18749375641345978,0.2222503125667572,0.29977163672447205,1.0152255296707153]},{"input":[true,15.0,-2.1609602069751936,false,false,0.0014099884373294702,false,0.0],"torque":-0.19334899235911346,"active":true,"saturated":false,"log":[0.09552394598722458,0.19104789197444916,0.004132009577006102,-0.0,0.4065656363964081,-0.1933489888906479,0.22841812670230865,0.3086809813976288,1.004106879234314]},{"input":[true,15.0,-2.226663596339769,false,false,0.0014464948746499401,true,0.0],"torque":-0.19893966597142995,"active":true,"saturated":false,"log":[0.09868063777685165,0.1973612755537033,0.004477391485124826,-0.0,0.4157773554325104,-0.19893966615200043,0.23433217406272888,0.3172473907470703,0.9912965297698975]},{"input":[true,15.0,-2.2894746246944444,false,false,0.0014813942442538548,false,0.0],"torque":-0.20425585424495907,"active":true,"saturated":false,"log":[0.1017276868224144,0.2034553736448288,0.00483343843370676,-0.0,0.4243428111076355,-0.2042558491230011,0.2399858683347702,0.32546135783195496,0.9768492579460144]},{"input":[true,15.0,-2.3493235084695163,false,false,0.0015146477726542137,false,0.0],"torque":-0.20928855496500046,"active":true,"saturated":false,"log":[0.104661725461483,0.209323450922966,0.005199754610657692,-0.0,0.43225422501564026,-0.20928855240345,0.24537293612957,0.3333137035369873,0.9608185291290283]},{"input":[true,15.0,-2.406143755062298,false,false,0.0015462185149071458,false,0.0],"torque":-0.21402969777530098,"active":true,"saturated":false,"log":[0.10747947543859482,0.21495895087718964,0.005575933028012514,-0.0,0.43950527906417847,-0.2140296995639801,0.25048738718032837,0.34079575538635254,0.9432570934295654]},{"input":[true,15.0,-2.459872236710953,false,false,0.0015760713956580457,false,0.0],"torque":-0.21847217260966734,"active":true,"saturated":false,"log":[0.11017782241106033,0.22035564482212067,0.005961555056273937,-0.0,0.4460911750793457,-0.2184721678495407,0.2553235590457916,0.3478991687297821,0.9242169857025146]},{"input":[true,15.0,-2.510449260629955,false,false,0.0016041732481105837,false,0.0],"torque":-0.22260977476999572,"active":true,"saturated":false,"log":[0.11275375634431839,0.22550751268863678,0.006356193218380213,-0.0,0.4520083963871002,-0.2226097732782364,0.2598760724067688,0.35461607575416565,0.9037495851516724]},{"input":[true,15.0,-2.557818635329264,false,false,0.001630492850875286,false,0.0],"torque":-0.22643716728775226,"active":true,"saturated":false,"log":[0.11520441621541977,0.23040883243083954,0.006759408861398697,-0.0,0.457254558801651,-0.22643716633319855,0.2641398310661316,0.3609389662742615,0.8819056153297424]},{"input":[true,15.0,-2.601927733043539,false,false,0.0016550009626567592,false,0.0],"torque":-0.22994981616371488,"active":true,"saturated":false,"log":[0.11752709001302719,0.23505418002605438,0.007170753553509712,-0.0,0.4618283212184906,-0.22994981706142426,0.26811015605926514,0.36686089634895325,0.8587355017662048]},{"input":[true,15.0,-2.642727548202012,false,false,0.0016776703547410072,false,0.0],"torque":-0.23314389956476797,"active":true,"saturated":false,"log":[0.11971918493509293,0.23943836987018585,0.007589770946651697,-0.0,0.46572911739349365,-0.2331438958644867,0.27178260684013367,0.3723752200603485,0.8342893123626709]},{"input":[true,15.0,-2.680172751874088,false,false,0.0016984758412467596,false,0.0],"torque":-0.23601624670615662,"active":true,"saturated":false,"log":[0.12177827209234238,0.24355654418468475,0.008015994913876057,-0.0,0.4689570665359497,-0.23601624369621277,0.27515310049057007,0.3774758279323578,0.8086166977882385]},{"input":[true,15.0,-2.7142217421301513,false,false,0.0017173943071071896,false,0.0],"torque":-0.23856425101496007,"active":true,"saturated":false,"log":[0.12370206415653229,0.24740412831306458,0.008448951877653599,-0.0,0.4715128540992737,-0.23856425285339355,0.2782178819179535,0.3821570575237274,0.7817672491073608]},{"input":[true,15.0,-2.7448366902616606,false,false,0.0017344047337509473,false,0.0],"torque":-0.2407857975769867,"active":true,"saturated":false,"log":[0.12548843026161194,0.2509768605232239,0.00888816174119711,-0.0,0.4733976125717163,-0.24078579246997833,0.28097355365753174,0.38641372323036194,0.7537903189659119]},{"input":[true,15.0,-2.7719835828091486,false,false,0.0017494882224539628,false,0.0],"torque":-0.24267917371492473,"active":true,"saturated":false,"log":[0.12713536620140076,0.2542707324028015,0.009333135560154915,-0.0,0.4746127128601074,-0.2426791787147522,0.28341710567474365,0.3902410566806793,0.7247351408004761]},{"input":[true,15.0,-2.7956322593514615,false,false,0.0017626280153360862,false,0.0],"torque":-0.24424302284419597,"active":true,"saturated":false,"log":[0.1286410540342331,0.2572821080684662,0.009783378802239895,-0.0,0.4751596748828888,-0.24424302577972412,0.2855457365512848,0.3936348557472229,0.6946508288383484]},{"input":[true,15.0,-2.8157564460142304,false,false,0.0017738095139792282,false,0.0],"torque":-0.24547626331443098,"active":true,"saturated":false,"log":[0.13000382483005524,0.2600076496601105,0.010238392278552055,-0.0,0.47504013776779175,-0.24547626078128815,0.2873571515083313,0.39659130573272705,0.6635865569114685]},{"input":[true,15.0,-2.8323337846603676,false,false,0.0017830202956463255,false,0.0],"torque":-0.2463780240060331,"active":true,"saturated":false,"log":[0.13122215867042542,0.26244431734085083,0.01069767028093338,-0.0,0.47425565123558044,-0.24637801945209503,0.2888492941856384,0.3991071283817291,0.6315913796424866]},{"input":[true,15.0,-2.8453458577301323,false,false,0.0017902501270831014,false,0.0],"torque":-0.24694760564802834,"active":true,"saturated":false,"log":[0.132294699549675,0.26458939909935,0.011160701513290405,-0.0,0.4728076159954071,-0.24694760143756866,0.29002052545547485,0.40117955207824707,0.5987144708633423]},{"input":[true,15.0,-2.8547782087032005,false,false,0.001795490975887298,false,0.0],"torque":-0.24718447823512973,"active":true,"saturated":false,"log":[0.13322027027606964,0.2664405405521393,0.011626971885561943,-0.0,0.47069743275642395,-0.24718448519706726,0.2908695340156555,0.40280628204345703,0.5650050044059753]},{"input":[true,15.0,-2.8606203581599723,false,false,0.0017987370194327387,false,0.0],"torque":-0.24708239197639428,"active":true,"saturated":false,"log":[0.1339978277683258,0.2679956555366516,0.012095964513719082,-0.0,0.46792662143707275,-0.2470823973417282,0.2913953959941864,0.40398547053337097,0.5305120944976807]},{"input":[true,15.0,-2.862865815424296,false,false,0.0017999846513383159,false,0.0],"torque":-0.2466316109085017,"active":true,"saturated":false,"log":[0.13462650775909424,0.2692530155181885,0.012567157857120037,-0.0,0.46449753642082214,-0.24663160741329193,0.29159751534461975,0.4047158360481262,0.4952850639820099]},{"input":[true,15.0,-2.861512085774659,false,false,0.001799232485474709,false,0.0],"torque":-0.24584560903026256,"active":true,"saturated":false,"log":[0.13510560989379883,0.27021121978759766,0.013040026649832726,-0.0,0.46041440963745117,-0.24584561586380005,0.29147565364837646,0.4049965441226959,0.4593731164932251]},{"input":[true,15.0,-2.856560673215853,false,false,0.0017964813575043867,false,0.0],"torque":-0.24472869730248822,"active":true,"saturated":false,"log":[0.13543462753295898,0.27086925506591797,0.013514048419892788,-0.0,0.455684632062912,-0.2447286993265152,0.29102998971939087,0.4048272967338562,0.4228256344795227]},{"input":[true,15.0,-2.8480170788079984,false,false,0.001791734323953177,false,0.0],"torque":-0.24328469751762846,"active":true,"saturated":false,"log":[0.13561315834522247,0.27122631669044495,0.013988694176077843,-0.0,0.4503200650215149,-0.2432847023010254,0.2902609705924988,0.40420830249786377,0.3856920003890991]},{"input":[true,15.0,-2.835890794554835,false,false,0.0017849966588144434,false,0.0],"torque":-0.24151868695604087,"active":true,"saturated":false,"log":[0.13564103841781616,0.2712820768356323,0.014463437721133232,-0.0,0.4443369209766388,-0.24151869118213654,0.28916946053504944,0.40314021706581116,0.34802159667015076]},{"input":[true,15.0,-2.8201952928580174,false,false,0.0017762758476896352,false,0.0],"torque":-0.2394367966582533,"active":true,"saturated":false,"log":[0.13551820814609528,0.27103641629219055,0.014937751926481724,-0.0,0.43775418400764465,-0.2394367903470993,0.28775668144226074,0.40162426233291626,0.3098639249801636]},{"input":[true,15.0,-2.8009480115491723,false,false,0.0017655815794717213,false,0.0],"torque":-0.23704539309777042,"active":true,"saturated":false,"log":[0.13524481654167175,0.2704896330833435,0.01541110873222351,-0.0,0.4305903911590576,-0.23704539239406586,0.28602421283721924,0.3996620774269104,0.271268367767334]},{"input":[true,15.0,-2.778170334516321,false,false,0.0017529257355807513,false,0.0],"torque":-0.23435011471652048,"active":true,"saturated":false,"log":[0.13482117652893066,0.26964235305786133,0.015882981941103935,-0.0,0.4228607416152954,-0.23435011506080627,0.28397396206855774,0.39725586771965027,0.23228439688682556]},{"input":[true,15.0,-2.751887567946205,false,false,0.0017383223767634996,false,0.0],"torque":-0.23135536600921267,"active":true,"saturated":false,"log":[0.13424775004386902,0.26849550008773804,0.016352849081158638,-0.0,0.4145756959915161,-0.23135536909103394,0.2816082239151001,0.39440828561782837,0.1929614394903183]},{"input":[true,15.0,-2.722128912208902,false,false,0.00172178772747186,false,0.0],"torque":-0.2280644531084134,"active":true,"saturated":false,"log":[0.13352516293525696,0.2670503258705139,0.016820188611745834,0.0,0.40574201941490173,-0.22806444764137268,0.27892962098121643,0.3911225199699402,0.15334883332252502]},{"input":[true,15.0,-2.6889274294159766,false,false,0.001703340157837346,false,0.0],"torque":-0.22448013400538977,"active":true,"saturated":false,"log":[0.13265423476696014,0.2653084695339203,0.01728447712957859,0.0,0.39636439085006714,-0.22448013722896576,0.2759411036968231,0.3874022364616394,0.11349587887525558]},{"input":[true,15.0,-2.6523200066882024,false,false,0.001683000163261724,false,0.0],"torque":-0.2206051873931663,"active":true,"saturated":false,"log":[0.13163591921329498,0.26327183842658997,0.017745204269886017,0.0,0.38644737005233765,-0.22060519456863403,0.27264603972435,0.3832515478134155,0.07345176488161087]},{"input":[true,15.0,-2.6123473151736643,false,false,0.0016607903416464523,false,0.0],"torque":-0.217660325948284,"active":true,"saturated":false,"log":[0.1304713636636734,0.2609427273273468,0.018201854079961777,0.0,0.38057181239128113,-0.2176603227853775,0.2690480351448059,0.37867504358291626,0.03326552361249924]},{"input":[true,15.0,-2.5690537648617813,false,false,0.001636735368286227,false,0.0],"torque":-0.21473344443009862,"active":true,"saturated":false,"log":[0.12916183471679688,0.25832366943359375,0.01865391992032528,0.0,0.37443670630455017,-0.21473345160484314,0.26515114307403564,0.3736778199672699,-0.007013937458395958]},{"input":[true,15.0,-2.5224874552434398,false,false,0.0016108619684545263,false,0.0],"torque":-0.21160090230288792,"active":true,"saturated":false,"log":[0.1277088224887848,0.2554176449775696,0.01910090073943138,0.0,0.36785101890563965,-0.2116008996963501,0.2609596252441406,0.36826545000076294,-0.04733793064951897]},{"input":[true,15.0,-2.472700121872065,false,false,0.001583198887711611,false,0.0],"torque":-0.20816949087685171,"active":true,"saturated":false,"log":[0.1261139214038849,0.2522278428077698,0.019542299211025238,0.0,0.36061835289001465,-0.20816949009895325,0.2564782202243805,0.3624439537525177,-0.08765798807144165]},{"input":[true,15.0,-2.419747078884995,false,false,0.0015537768599679727,false,0.0],"torque":-0.2034782951723132,"active":true,"saturated":false,"log":[0.12437891215085983,0.24875782430171967,0.019977625459432602,0.0,0.3491714298725128,-0.20347829163074493,0.2517118453979492,0.35621973872184753,-0.12792588770389557]},{"input":[true,15.0,-2.3636871575490175,false,false,0.0015226285733387025,false,0.0],"torque":-0.19836412871905443,"active":true,"saturated":false,"log":[0.12250570952892303,0.24501141905784607,0.020406395196914673,0.0,0.3373427987098694,-0.19836412370204926,0.2466658353805542,0.34959977865219116,-0.168093740940094]},{"input":[true,15.0,-2.3045826408983494,true,false,0.0014897886338267289,false,0.0],"torque":-0.19158871738841954,"active":true,"saturated":false,"log":[0.12049640715122223,0.24099281430244446,0.020406395196914673,0.0,0.3251485228538513,-0.19158871471881866,0.24134576320648193,0.34259143471717834,-0.20811396837234497]},{"input":[true,15.0,-2.24249919453767,true,false,0.001455293526875262,false,0.0],"torque":-0.1858144961957261,"active":true,"saturated":false,"log":[0.1183532327413559,0.2367064654827118,0.020406395196914673,0.0,0.31260189414024353,-0.1858144998550415,0.23575755953788757,0.3352024555206299,-0.2479393482208252]},{"input":[true,15.0,-2.177505793687099,true,false,0.0014191815768321676,false,0.0],"torque":-0.17984188920907174,"active":true,"saturated":false,"log":[0.11607856303453445,0.2321571260690689,0.020406395196914673,0.0,0.2997118830680847,-0.17984189093112946,0.22990742325782776,0.3274410367012024,-0.287523090839386]},{"input":[true,15.0,-2.109674646550155,true,false,0.0013814929043712983,false,0.0],"torque":-0.1736771737797895,"active":true,"saturated":false,"log":[0.11367493122816086,0.22734986245632172,0.020406395196914673,0.0,0.28648293018341064,-0.17367717623710632,0.223801851272583,0.3193158507347107,-0.3268188536167145]},{"input":[true,15.0,-2.0390811140898446,true,false,0.0013422693819180966,false,0.0],"torque":-0.16732603043434421,"active":true,"saturated":false,"log":[0.1111450046300888,0.2222900092601776,0.020406395196914673,0.0,0.27291661500930786,-0.16732603311538696,0.21744763851165771,0.31083589792251587,-0.365780770778656]},{"input":[true,15.0,-1.9658036263020164,true,false,0.0013015545871289842,false,0.0],"torque":-0.16079448102263863,"active":true,"saturated":false,"log":[0.10849159210920334,0.21698318421840668,0.020406395196914673,0.0,0.2590144872665405,-0.1607944816350937,0.21085184812545776,0.3020106256008148,-0.4043635129928589]},{"input":[true,15.0,-1.8899235950789912,true,false,0.0012593937544762292,false,0.0],"torque":-0.1540897560948521,"active":true,"saturated":false,"log":[0.10571765154600143,0.21143530309200287,0.020406395196914673,0.0,0.24478097259998322,-0.15408974885940552,0.20402179658412933,0.2928497791290283,-0.4425222873687744]},{"input":[true,15.0,-1.8115253237602735,true,false,0.001215833724992072,false,0.0],"torque":-0.14722055879159812,"active":true,"saturated":false,"log":[0.10282625257968903,0.20565250515937805,0.020406395196914673,0.0,0.23022451996803284,-0.14722055196762085,0.1969650685787201,0.28336358070373535,-0.480212926864624]},{"input":[true,15.0,-1.7306959134708493,true,false,0.0011709228942279536,false,0.0],"torque":-0.14019674031937912,"active":true,"saturated":false,"log":[0.09982061386108398,0.19964122772216797,0.020406395196914673,0.0,0.21535685658454895,-0.14019674062728882,0.1896895170211792,0.27356258034706116,-0.5173919200897217]},{"input":[true,15.0,-1.64752516635112,true,false,0.0011247111584866602,false,0.0],"torque":-0.13302863160714007,"active":true,"saturated":false,"log":[0.09670407325029373,0.19340814650058746,0.020406395196914673,0.0,0.20019127428531647,-0.13302862644195557,0.18220320343971252,0.26345765590667725,-0.5540164113044739]},{"input":[true,15.0,-1.562105485785987,true,false,0.0010772498593871215,false,0.0],"torque":-0.1257264688921751,"active":true,"saturated":false,"log":[0.09348009526729584,0.18696019053459167,0.020406395196914673,0.0,0.1847412884235382,-0.12572647631168365,0.17451447248458862,0.2530600130558014,-0.5900443196296692]},{"input":[true,15.0,-1.4745317737439276,true,false,0.0010285917268234453,false,0.0],"torque":-0.11830004483733149,"active":true,"saturated":false,"log":[0.09015225619077682,0.18030451238155365,0.020406395196914673,0.0,0.16901986300945282,-0.11830004304647446,0.16663186252117157,0.24238121509552002,-0.6254342794418335]},{"input":[true,15.0,-1.3849013253401212,true,false,0.0009787908203815724,false,0.0],"torque":-0.1107586063455909,"active":true,"saturated":false,"log":[0.08672425895929337,0.17344851791858673,0.020406395196914673,0.0,0.15303941071033478,-0.11075861006975174,0.15856412053108215,0.23143313825130463,-0.6601456999778748]},{"input":[true,15.0,-1.2933137207407897,true,false,0.0009279024692786355,false,0.0],"torque":-0.1031108726058476,"active":true,"saturated":false,"log":[0.08319991081953049,0.16639982163906097,0.020406395196914673,0.0,0.13681216537952423,-0.10311087220907211,0.15032020211219788,0.2202279418706894,-0.6941389441490173]},{"input":[true,15.0,-1.1998707145287968,true,false,0.0008759832108917369,false,0.0],"torque":-0.09536513814976963,"active":true,"saturated":false,"log":[0.07958313077688217,0.15916626155376434,0.020406395196914673,0.0,0.12035076320171356,-0.09536513686180115,0.14190928637981415,0.20877805352210999,-0.7273750901222229]},{"input":[true,15.0,-1.1046761226534794,true,false,0.0008230907279444615,false,0.0],"torque":-0.08752937314951365,"active":true,"saturated":false,"log":[0.07587792724370956,0.15175585448741913,0.020406395196914673,0.0,0.10366865247488022,-0.08752937614917755,0.13334070146083832,0.19709622859954834,-0.7598162889480591]},{"input":[true,15.0,-1.0078357070902662,false,false,0.0007692837844208935,true,0.0],"torque":-0.08007737736327752,"active":true,"saturated":false,"log":[0.07208842784166336,0.14417685568332672,0.01632511615753174,0.0,0.08678056299686432,-0.08007737994194031,0.12462397664785385,0.1851954162120819,-0.7914255261421204]},{"input":[true,15.0,-0.9094570583382604,false,false,0.0007146221602783498,false,0.0],"torque":-0.07218988838179417,"active":true,"saturated":false,"log":[0.06821883469820023,0.13643766939640045,0.01632511615753174,0.0,0.06970269232988358,-0.07218988984823227,0.11576879024505615,0.17308884859085083,-0.8221668601036072]},{"input":[true,15.0,-0.8096494758862993,false,false,0.0006591665850313498,false,0.0],"torque":-0.06416489858591745,"active":true,"saturated":false,"log":[0.06427344679832458,0.12854689359664917,0.01632511615753174,0.0,0.05245282128453255,-0.064164899289608,0.10678498446941376,0.16078998148441315,-0.8520053625106812]},{"input":[true,15.0,-0.7085238467803299,false,false,0.0006029786702806292,false,0.0],"torque":-0.05606541958443773,"active":true,"saturated":false,"log":[0.060256656259298325,0.12051331251859665,0.01632511615753174,0.0,0.035050246864557266,-0.05606541782617569,0.09768254309892654,0.14831247925758362,-0.8809070587158203]},{"input":[true,15.0,-1.6027387186557298,false,false,0.000546120841262141,false,0.0],"torque":0.020411427435922527,"active":true,"saturated":false,"log":[-0.05058295652270317,-0.10116591304540634,0.01632511615753174,-0.0,-0.037457700818777084,0.020411428064107895,0.17817193269729614,0.13567019999027252,-0.908839225769043]},{"input":[true,15.0,-1.3944556262220085,false,false,0.0004886562674921086,false,0.0],"torque":0.033522745952225055,"active":true,"saturated":false,"log":[-0.04349590837955475,-0.0869918167591095,0.01632511615753174,0.0,-0.04929627478122711,0.033522747457027435,0.15942411124706268,0.12287718802690506,-0.9357701539993286]},{"input":[true,15.0,-1.1842047626858079,false,false,0.00043064879258516834,false,0.0],"torque":0.036072852400443535,"active":true,"saturated":false,"log":[-0.03636053949594498,-0.07272107899188995,0.01632511615753174,0.0,-0.06118159368634224,0.036072853952646255,0.14049917459487915,0.10994765907526016,-0.961669385433197]},{"input":[true,15.0,-0.972219718487777,false,false,0.0003721628633235905,false,0.0],"torque":0.036072836265439945,"active":true,"saturated":false,"log":[-0.029184773564338684,-0.05836954712867737,0.01632511615753174,0.0,-0.07309234887361526,0.03607283532619476,0.12141813337802887,0.0968959778547287,-0.9865075349807739]},{"input":[true,15.0,-0.758736010757558,false,false,0.0003132634580563725,false,0.0],"torque":0.03540590602893783,"active":true,"saturated":false,"log":[-0.02197658084332943,-0.04395316168665886,0.01632511615753174,0.0,-0.08500392735004425,0.03540590777993202,0.10220220685005188,0.08373664319515228,-1.0102564096450806]},{"input":[true,15.0,-0.5439908216523264,false,false,0.00025401601450776096,false,0.0],"torque":0.034598610264784876,"active":true,"saturated":false,"log":[-0.01474397350102663,-0.02948794700205326,0.01632511615753174,0.0,-0.09689566493034363,0.034598611295223236,0.08287272602319717,0.07048428058624268,-1.0328893661499023]},{"input":[true,15.0,-0.328222734845443,false,false,0.00019448635707539666,false,0.0],"torque":0.03375083718222084,"active":true,"saturated":false,"log":[-0.0074949851259589195,-0.014989970251917839,0.01632511615753174,0.0,-0.10875008255243301,0.03375083580613136,0.06345117837190628,0.0571536049246788,-1.0543806552886963]},{"input":[true,15.0,-0.11167147045803855,false,false,0.00013474062369886897,false,0.0],"torque":0.03288103328010188,"active":true,"saturated":false,"log":[-0.0002376698685111478,-0.0004753397370222956,0.01632511615753174,0.0,-0.12054981291294098,0.032881032675504684,0.043959129601716995,0.043759431689977646,-1.074705958366394]},{"input":[true,15.0,0.10542238127200682,false,false,7.484519237992288e-05,false,0.0],"torque":0.03199203046726751,"active":true,"saturated":false,"log":[0.00701990956440568,0.01403981912881136,0.01632511615753174,0.0,-0.1322777420282364,0.03199202939867973,0.024418246001005173,0.030316639691591263,-1.0938423871994019]},{"input":[true,15.0,0.3228176272878158,false,false,1.4866607435947705e-05,false,0.0],"torque":0.031084135217449252,"active":true,"saturated":false,"log":[0.014269690029323101,0.028539380058646202,0.01632511615753174,0.0,-0.143917053937912,0.031084135174751282,0.004850231576710939,0.016840169206261635,-1.1117682456970215]},{"input":[true,15.0,0.5402727396809753,false,false,-4.5128494431304356e-05,false,0.0],"torque":0.030156908618335903,"active":true,"saturated":false,"log":[0.021503616124391556,0.04300723224878311,0.01632511615753174,0.0,-0.15545137226581573,0.030156908556818962,-0.014723170548677444,0.0033449865877628326,-1.1284633874893188]},{"input":[true,15.0,0.7575461240310368,false,false,-0.00010507345816964415,false,0.0],"torque":0.02924821705284042,"active":true,"saturated":false,"log":[0.0287136510014534,0.0574273020029068,0.016425613313913345,0.0,-0.1670418381690979,0.02924821712076664,-0.034280214458703995,-0.01015391107648611,-1.143908977508545]},{"input":[true,15.0,0.9743963878188955,false,false,-0.0001649016844307542,false,0.0],"torque":0.028341345594254972,"active":true,"saturated":false,"log":[0.035891786217689514,0.07178357243537903,0.01655123569071293,0.0,-0.17855821549892426,0.028341345489025116,-0.05379917472600937,-0.02364152856171131,-1.1580874919891357]},{"input":[true,15.0,1.1905826086158835,false,false,-0.0002245467035626178,false,0.0],"torque":0.027413626306096978,"active":true,"saturated":false,"log":[0.04303004592657089,0.08606009185314178,0.016701839864253998,0.0,-0.18993280827999115,0.0274136271327734,-0.07325836271047592,-0.03710287809371948,-1.170982837677002]},{"input":[true,15.0,1.4058646017505616,false,false,-0.00028394224945784677,false,0.0],"torque":0.02646242359485004,"active":true,"saturated":false,"log":[0.05012049525976181,0.10024099051952362,0.01687726192176342,0.0,-0.20115657150745392,0.02646242268383503,-0.09263616055250168,-0.05052300915122032,-1.1825804710388184]},{"input":[true,15.0,1.620003187155871,false,true,-0.0003430223331758737,false,0.0],"torque":0.02553956329208708,"active":true,"saturated":false,"log":[0.05715526267886162,0.11431052535772324,0.01687726192176342,0.0,-0.2122209519147873,0.025539563968777657,-0.11191103607416153,-0.06388700753450394,-1.192867398262024]},{"input":[true,15.0,1.8327604551001484,false,true,-0.00040172131625720533,false,0.0],"torque":0.024612445202430992,"active":true,"saturated":false,"log":[0.06412652879953384,0.1282530575990677,0.01687726192176342,0.0,-0.22311782836914062,0.024612445384263992,-0.1310615837574005,-0.07718002796173096,-1.2018319368362427]},{"input":[true,15.0,2.04390003050681,false,true,-0.0004599739836482962,false,0.0],"torque":0.02367664195512181,"active":true,"saturated":false,"log":[0.07102655619382858,0.14205311238765717,0.01687726192176342,0.0,-0.23383937776088715,0.023676641285419464,-0.15006650984287262,-0.09038729965686798,-1.2094638347625732]},{"input":[true,15.0,2.253187335569,false,true,-0.000517715616156007,false,0.0],"torque":0.02273270874718577,"active":true,"saturated":false,"log":[0.0778476670384407,0.1556953340768814,0.01687726192176342,0.0,-0.24437804520130157,0.022732708603143692,-0.16890472173690796,-0.10349414497613907,-1.215754508972168]},{"input":[true,15.0,2.4603898503674717,false,true,-0.0005748820623511629,false,0.0],"torque":0.021781681185744365,"active":true,"saturated":false,"log":[0.08458229154348373,0.16916458308696747,0.01687726192176342,0.0,-0.2547264099121094,0.0217816811054945,-0.18755526840686798,-0.11648601293563843,-1.2206966876983643]},{"input":[true,15.0,2.6652773712021176,false,true,-0.0006314098098413157,false,0.0],"torque":0.02082644871519876,"active":true,"saturated":false,"log":[0.09122294187545776,0.18244588375091553,0.01687726192176342,0.0,-0.2648769021034241,0.020826449617743492,-0.20599745213985443,-0.12934845685958862,-1.2242848873138428]},{"input":[true,15.0,2.867622266350196,false,true,-0.0006872360558335404,false,0.0],"torque":0.019869095263685486,"active":true,"saturated":false,"log":[0.09776224195957184,0.19552448391914368,0.01687726192176342,0.0,-0.27482178807258606,0.019869094714522362,-0.22421076893806458,-0.14206720888614655,-1.2265148162841797]},{"input":[true,15.0,3.0671997289670383,false,false,-0.0007422987769088523,false,0.0],"torque":0.018818363053697582,"active":true,"saturated":false,"log":[0.10419292747974396,0.20838585495948792,0.017241938039660454,0.0,-0.2845527231693268,0.018818363547325134,-0.24217496812343597,-0.15462811291217804,-1.2273839712142944]},{"input":[true,15.0,3.2637880268482986,false,false,-0.0007965367979307344,false,0.0],"torque":0.017744948466748584,"active":true,"saturated":false,"log":[0.11050785332918167,0.22101570665836334,0.017628714442253113,0.0,-0.29406067728996277,0.017744949087500572,-0.25987014174461365,-0.16701722145080566,-1.2268911600112915]},{"input":[true,15.0,3.4571687487762746,false,false,-0.000849889860011222,false,0.0],"torque":0.016663163773742773,"active":true,"saturated":false,"log":[0.11670000106096268,0.23340000212192535,0.018037164583802223,0.0,-0.30333566665649414,0.016663163900375366,-0.27727657556533813,-0.1792207807302475,-1.225036859512329]},{"input":[true,15.0,3.6471270471765553,false,false,-0.0009022986874590205,false,0.0],"torque":0.01557619757484364,"active":true,"saturated":false,"log":[0.12276250123977661,0.24552500247955322,0.018466833978891373,0.0,-0.3123665750026703,0.015576197765767574,-0.294374942779541,-0.19122521579265594,-1.2218230962753296]},{"input":[true,15.0,3.8334518768154555,false,false,-0.0009537050536352881,false,0.0],"torque":0.014484750057486217,"active":true,"saturated":false,"log":[0.12868860363960266,0.2573772072792053,0.01891724392771721,0.0,-0.32114100456237793,0.014484750106930733,-0.31114625930786133,-0.2030172049999237,-1.2172532081604004]},{"input":[true,15.0,4.0159362292729925,false,false,-0.001004051845643908,false,0.0],"torque":0.013388647511201493,"active":true,"saturated":false,"log":[0.13447174429893494,0.2689434885978699,0.019387895241379738,0.0,-0.3296452760696411,0.013388647697865963,-0.32757189869880676,-0.2145836353302002,-1.2113322019577026]},{"input":[true,15.0,4.194377362930963,false,false,-0.0010532831277843902,false,0.0],"torque":0.012287214934846505,"active":true,"saturated":false,"log":[0.1401054710149765,0.280210942029953,0.019878264516592026,0.0,-0.33786454796791077,0.012287215329706669,-0.34363362193107605,-0.22591166198253632,-1.2040666341781616]},{"input":[true,15.0,4.368577028220545,false,false,-0.001101344203696894,false,0.0],"torque":0.011179342966755207,"active":true,"saturated":false,"log":[0.14558355510234833,0.29116711020469666,0.020387805998325348,0.0,-0.3457828760147095,0.011179342865943909,-0.3593135476112366,-0.23698870837688446,-1.1954644918441772]},{"input":[true,15.0,2.188976238622411,false,false,-0.0005,false,0.0],"torque":0.12208512285894776,"active":true,"saturated":false,"log":[-0.10077792406082153,-0.20155584812164307,0.02003508433699608,-0.0,-0.30883708596229553,0.12208512425422668,-0.16312499344348907,-0.24780245125293732,-0.9363683462142944]},{"input":[true,15.0,2.1849504546902803,false,false,-0.0004988893003505612,false,0.0],"torque":0.041265938267852104,"active":true,"saturated":false,"log":[0.05981951579451561,0.11963903158903122,0.02003508433699608,-0.0,-0.21040864288806915,0.04126593843102455,-0.1627626270055771,-0.11249999701976776,-0.8689656257629395]},{"input":[true,15.0,2.1728909886410945,false,false,-0.0004955621360170897,false,0.0],"torque":0.02432074805301384,"active":true,"saturated":false,"log":[0.0588250569999218,0.1176501139998436,0.020240971818566322,-0.0,-0.20266318321228027,0.024320747703313828,-0.16167715191841125,-0.11225008964538574,-0.8027921319007874]},{"input":[true,15.0,2.1528514182537077,false,false,-0.0004900332889206208,false,0.0],"torque":0.019641782349992146,"active":true,"saturated":false,"log":[0.05756925418972969,0.11513850837945938,0.02044246345758438,-0.0,-0.1946704387664795,0.019641783088445663,-0.159873366355896,-0.11150147765874863,-0.7377849817276001]},{"input":[true,15.0,2.124920775303337,false,false,-0.00048232732261528193,false,0.0],"torque":0.0173727194817688,"active":true,"saturated":false,"log":[0.056057680398225784,0.11211536079645157,0.020638665184378624,-0.0,-0.18642985820770264,0.017372719943523407,-0.1573592871427536,-0.11025749146938324,-0.6739012002944946]},{"input":[true,15.0,2.0892231500113154,false,false,-0.00047247847315736883,false,0.0],"torque":0.015630430875900707,"active":true,"saturated":false,"log":[0.054297056049108505,0.10859411209821701,0.02082870528101921,-0.0,-0.17794853448867798,0.015630431473255157,-0.15414610505104065,-0.1085236445069313,-0.6111159324645996]},{"input":[true,15.0,2.045917139737235,false,false,-0.00046053049700144256,false,0.0],"torque":0.014056353043859557,"active":true,"saturated":false,"log":[0.052295200526714325,0.10459040105342865,0.02101173810660839,-0.0,-0.16924451291561127,0.014056352898478508,-0.15024808049201965,-0.10630765557289124,-0.5494213104248047]},{"input":[true,15.0,1.9951951443628146,false,false,-0.00044653647659921467,false,0.0],"torque":0.012585975941526434,"active":true,"saturated":false,"log":[0.05006100609898567,0.10012201219797134,0.0211869515478611,-0.0,-0.1603497713804245,0.012585976161062717,-0.14568252861499786,-0.10361935943365097,-0.4888244867324829]},{"input":[true,15.0,1.9372825114979797,false,false,-0.00043055858456490516,false,0.0],"torque":0.011213247378125507,"active":true,"saturated":false,"log":[0.04760440066456795,0.0952088013291359,0.021353567019104958,-0.0,-0.15130765736103058,0.011213247664272785,-0.1404697299003601,-0.10047070682048798,-0.42934665083885193]},{"input":[true,15.0,1.8724365353068144,false,false,-0.0004126678074548392,false,0.0],"torque":0.009941948465245487,"active":true,"saturated":false,"log":[0.044936299324035645,0.08987259864807129,0.02151084505021572,-0.0,-0.14215867221355438,0.00994194857776165,-0.134632870554924,-0.09687568247318268,-0.3710213303565979]},{"input":[true,15.0,1.8009453134014108,false,false,-0.00039294363038847404,false,0.0],"torque":0.00877169139897998,"active":true,"saturated":false,"log":[0.04206855595111847,0.08413711190223694,0.02165808528661728,-0.0,-0.13292092084884644,0.008771691471338272,-0.12819786369800568,-0.09285025298595428,-0.31389307975769043]},{"input":[true,15.0,1.7231264668822022,false,false,-0.00037147368391202205,false,0.0],"torque":0.007694005812446535,"active":true,"saturated":false,"log":[0.03901390731334686,0.07802781462669373,0.02179463393986225,-0.0,-0.12358593940734863,0.0076940059661865234,-0.1211932897567749,-0.0884123146533966,-0.2580161988735199]},{"input":[true,15.0,1.6393257292113825,false,false,-0.0003483533546735827,false,0.0],"torque":0.008110470313112288,"active":true,"saturated":false,"log":[0.03578593209385872,0.07157186418771744,0.02191988378763199,-0.0,-0.11983002722263336,0.0081104701384902,-0.11365027725696564,-0.0835815817117691,-0.2034534513950348]},{"input":[true,15.0,1.5499154101887673,false,false,-0.0003236853616394762,false,0.0],"torque":0.00888167655444426,"active":true,"saturated":false,"log":[0.03239896520972252,0.06479793041944504,0.02203327976167202,-0.0,-0.11600524187088013,0.008881676942110062,-0.10560234636068344,-0.07837950438261032,-0.15027473866939545]},{"input":[true,15.0,1.455292741854335,false,false,-0.0002975792997345639,false,0.0],"torque":0.00973951571862654,"active":true,"saturated":false,"log":[0.0288680549710989,0.0577361099421978,0.022134318947792053,-0.0,-0.1118881106376648,0.009739515371620655,-0.09708524495363235,-0.07282920926809311,-0.09855590760707855]},{"input":[true,15.0,1.3558781136662774,false,false,-0.00027015115293406986,false,0.0],"torque":0.010619639371863929,"active":true,"saturated":false,"log":[0.025208890438079834,0.05041778087615967,0.022222550585865974,-0.0,-0.10751024633646011,0.010619639419019222,-0.08813681453466415,-0.06695534288883209,-0.04837757349014282]},{"input":[true,15.0,1.2521132047953047,false,false,-0.00024152277897013832,false,0.0],"torque":0.011505812279856724,"active":true,"saturated":false,"log":[0.02143772877752781,0.04287545755505562,0.022297581657767296,-0.0,-0.10290049761533737,0.011505812406539917,-0.07879680395126343,-0.06078400835394859,0.00017604546155780554]},{"input":[true,15.0,1.1444590218330455,false,false,-0.0002118213679424723,false,0.0],"torque":0.012388830104702112,"active":true,"saturated":false,"log":[0.01757132261991501,0.03514264523983002,0.022359080612659454,-0.0,-0.09806954115629196,0.012388830073177814,-0.06910672038793564,-0.05434262380003929,0.04701821133494377]},{"input":[true,15.0,1.0333938506326064,false,false,-0.00018117887723833681,false,0.0],"torque":0.013269384068380222,"active":true,"saturated":false,"log":[0.013626852072775364,0.027253704145550728,0.022406775504350662,-0.0,-0.09306351840496063,0.013269384391605854,-0.059109605848789215,-0.04765980690717697,0.09206075221300125]},{"input":[true,15.0,0.9194111313808566,false,false,-0.00014973144527246395,true,0.0],"torque":0.01414529880184396,"active":true,"saturated":false,"log":[0.009621839039027691,0.019243678078055382,0.022440452128648758,-0.0,-0.08790715783834457,0.01414529886096716,-0.048849884420633316,-0.040765248239040375,0.13521508872509003]},{"input":[true,15.0,0.8030172663430561,false,false,-0.00011761878665149471,false,0.0],"torque":0.015014051022004057,"active":true,"saturated":false,"log":[0.005574078299105167,0.011148156598210335,0.022459961473941803,-0.0,-0.08262720704078674,0.015014051459729671,-0.038373127579689026,-0.03368957340717316,0.17639319598674774]},{"input":[true,15.0,0.6847293700195822,false,false,-8.498357145012052e-05,false,0.0],"torque":0.015873327755481885,"active":true,"saturated":false,"log":[0.0015015531098470092,0.0030031062196940184,0.022465215995907784,-0.0,-0.07725208252668381,0.01587332785129547,-0.027725888416171074,-0.026464227586984634,0.2155085653066635]},{"input":[true,15.0,0.5650729717103522,false,false,-5.197079135667549e-05,false,0.0],"torque":0.016720967062267236,"active":true,"saturated":false,"log":[-0.0025776433758437634,-0.005155286751687527,0.022456195205450058,-0.0,-0.07181090861558914,0.01672096736729145,-0.01695546880364418,-0.019121304154396057,0.25247710943222046]},{"input":[true,15.0,0.4445796806939861,false,false,-1.872711550426666e-05,false,0.0],"torque":0.017027288908969304,"active":true,"saturated":false,"log":[-0.0066453879699110985,-0.013290775939822197,0.02243293635547161,-0.0,-0.06410247832536697,0.017027288675308228,-0.006109720095992088,-0.01169342827051878,0.2872179448604584]},{"input":[true,15.0,0.3237848243948517,false,false,1.4599761150644409e-05,false,0.0],"torque":0.016890399693434303,"active":true,"saturated":false,"log":[-0.010683608241379261,-0.021367216482758522,0.0223955437541008,-0.0,-0.05515087768435478,0.016890399158000946,0.004763173405081034,-0.0042136008851230145,0.31965428590774536]},{"input":[true,15.0,0.20322507003113005,false,false,4.786177400718783e-05,false,0.0],"torque":0.016675975711580054,"active":true,"saturated":false,"log":[-0.014674363657832146,-0.02934872731566429,0.022344183176755905,-0.0,-0.04637288674712181,0.016675975173711777,0.015614904463291168,0.0032849463168531656,0.34971415996551514]},{"input":[true,15.0,0.08343604031045423,false,false,8.091114664128603e-05,false,0.0],"torque":0.01647070955680342,"active":true,"saturated":false,"log":[-0.018599923700094223,-0.037199847400188446,0.022279083728790283,-0.0,-0.03778791427612305,0.016470709815621376,0.026397263631224632,0.010768898762762547,0.3773309886455536]},{"input":[true,15.0,-0.03505006623390644,false,false,0.00011360104734654355,false,0.0],"torque":0.01630473919501293,"active":true,"saturated":false,"log":[-0.02244284749031067,-0.04488569498062134,0.022200534120202065,-0.0,-0.02950281836092472,0.01630473881959915,0.03706234320998192,0.018205007538199425,0.4024445116519928]},{"input":[true,15.0,-0.15170683969394294,false,false,0.0001457862414779463,false,0.0],"torque":0.016183875442508728,"active":true,"saturated":false,"log":[-0.026186062023043633,-0.052372124046087265,0.022108882665634155,-0.0,-0.021552734076976776,0.016183875501155853,0.04756276309490204,0.02556023560464382,0.4250010848045349]},{"input":[true,15.0,-0.2660159975201088,false,false,0.0001773237367001388,false,0.0],"torque":0.016109153999158722,"active":true,"saturated":false,"log":[-0.02981293946504593,-0.05962587893009186,0.022004537284374237,-0.0,-0.013970705680549145,0.016109153628349304,0.057851869612932205,0.032801903784275055,0.44495439529418945]},{"input":[true,15.0,0.002216927701837179,false,false,0.0002080734182735712,false,0.0],"torque":-0.009681045892679459,"active":true,"saturated":false,"log":[0.007366895209997892,0.014733790419995785,0.022030320018529892,-0.0,0.01376925129443407,-0.00968104600906372,0.03370789438486099,0.03989784047007561,0.4622659683227539]},{"input":[true,15.0,-0.051461830707659695,false,false,0.0002378986715560677,false,0.0],"torque":-0.01780245436218505,"active":true,"saturated":false,"log":[0.00985070038586855,0.0197014007717371,0.022064797580242157,-0.0,0.023519154638051987,-0.01780245453119278,0.0385395847260952,0.046816520392894745,0.47690549492836]},{"input":[true,15.0,-0.10323834186864167,false,false,0.00026666698895416686,false,0.0],"torque":-0.02311078069253975,"active":true,"saturated":false,"log":[0.012290740385651588,0.024581480771303177,0.022107815369963646,-0.0,0.032710250467061996,-0.023110780864953995,0.0432000532746315,0.05352720245718956,0.48885127902030945]},{"input":[true,15.0,-0.15288257316992637,false,false,0.0002942505586276729,false,0.0],"torque":-0.027578825382775507,"active":true,"saturated":false,"log":[0.014676175080239773,0.029352350160479546,0.022159183397889137,-0.0,0.04130321741104126,-0.027578825131058693,0.047668591141700745,0.06000007316470146,0.49809056520462036]},{"input":[true,15.0,-0.20017396529030146,false,false,0.0003205268323319191,false,0.0],"torque":-0.031667788111684865,"active":true,"saturated":false,"log":[0.01699640601873398,0.03399281203746796,0.022218670696020126,-0.0,0.04926186800003052,-0.031667787581682205,0.05192534625530243,0.06620637327432632,0.5046197772026062]},{"input":[true,15.0,-0.24490241209916924,false,false,0.00034537906987493823,false,0.0],"torque":-0.035475575240345364,"active":true,"saturated":false,"log":[0.019241126254200935,0.03848225250840187,0.02228601463139057,-0.0,0.05655293166637421,-0.03547557443380356,0.05595140904188156,0.07211853563785553,0.5084447860717773]},{"input":[true,15.0,-0.28686919411576667,false,false,0.0003686968577706227,false,0.0],"torque":-0.039013353385017664,"active":true,"saturated":false,"log":[0.021400362253189087,0.042800724506378174,0.022360915318131447,-0.0,0.06314598768949509,-0.03901335224509239,0.05972889065742493,0.07771029323339462,0.5095809698104858]},{"input":[true,15.0,-0.3258878613797989,false,false,0.0003903765997836073,false,0.0],"torque":-0.04227217646889971,"active":true,"saturated":false,"log":[0.023464519530534744,0.04692903906106949,0.02244304120540619,-0.0,0.06901345402002335,-0.0422721765935421,0.06324101239442825,0.08295679092407227,0.5080534815788269]},{"input":[true,15.0,-0.36178506181105835,false,false,0.0004103219771864765,false,0.0],"torque":-0.04523907859515038,"active":true,"saturated":false,"log":[0.025424430146813393,0.050848860293626785,0.02253202721476555,-0.0,0.07413064688444138,-0.045239079743623734,0.06647215783596039,0.08783473819494247,0.5038970708847046]},{"input":[true,15.0,-0.3944013113778065,false,false,0.00042844437668447364,false,0.0],"torque":-0.0479010076837597,"active":true,"saturated":false,"log":[0.027271384373307228,0.054542768746614456,0.02262747660279274,-0.0,0.07847587764263153,-0.047901008278131485,0.06940799206495285,0.0923224464058876,0.4971562623977661]},{"input":[true,15.0,-0.4235917026522031,false,false,0.0004446632841065207,false,0.0],"torque":-0.05024585500711475,"active":true,"saturated":false,"log":[0.028997177258133888,0.057994354516267776,0.022728966549038887,-0.0,0.08203068375587463,-0.05024585500359535,0.07203545421361923,0.09639998525381088,0.48788532614707947]},{"input":[true,15.0,-0.4492265486048274,false,false,0.0004589066421134661,false,0.0],"torque":-0.05226279661710131,"active":true,"saturated":false,"log":[0.03059414029121399,0.06118828058242798,0.022836046293377876,-0.0,0.08478002995252609,-0.0522627979516983,0.07434287667274475,0.10004924237728119,0.4761478900909424]},{"input":[true,15.0,-0.4711919587780264,false,false,0.000471111170334329,false,0.0],"torque":-0.05394249400852321,"active":true,"saturated":false,"log":[0.03205518051981926,0.06411036103963852,0.022948238998651505,-0.0,0.08671262860298157,-0.053942494094371796,0.07632000744342804,0.10325399786233902,0.46201711893081665]},{"input":[true,15.0,-0.48939034527828446,false,false,0.00048122264650824387,false,0.0],"torque":-0.05527727885573561,"active":true,"saturated":false,"log":[0.03337380662560463,0.06674761325120926,0.023065047338604927,-0.0,0.08782140910625458,-0.05527728050947189,0.07795806974172592,0.10600001364946365,0.4455752670764923]},{"input":[true,15.0,-0.503740856339576,false,false,0.0004891961473830436,false,0.0],"torque":-0.05626135811232154,"active":true,"saturated":false,"log":[0.03454415872693062,0.06908831745386124,0.02318595163524151,-0.0,0.0881039947271347,-0.05626135692000389,0.0792497768998146,0.10827509313821793,0.42691347002983093]},{"input":[true,15.0,-0.5141797355314814,false,false,0.0004949962483002228,false,0.0],"torque":-0.05689103378252602,"active":true,"saturated":false,"log":[0.03556104004383087,0.07112208008766174,0.02331041544675827,-0.0,0.08756332099437714,-0.05689103528857231,0.08018939197063446,0.11006913334131241,0.4061315059661865]},{"input":[true,15.0,-0.5206606050161644,false,false,0.0004985971805795595,false,0.0],"torque":-0.057164891314703606,"active":true,"saturated":false,"log":[0.03641992807388306,0.07283985614776611,0.02343788556754589,-0.0,0.0862080454826355,-0.05716489255428314,0.08077274262905121,0.1113741546869278,0.3833373785018921]},{"input":[true,15.0,-0.5231546715957662,false,false,0.0004999829460041724,false,0.0],"torque":-0.05707861449785038,"active":true,"saturated":false,"log":[0.03711701184511185,0.0742340236902237,0.023567795753479004,-0.0,0.08405277132987976,-0.057078614830970764,0.08099723607301712,0.11218436807394028,0.3586469292640686]},{"input":[true,15.0,-0.5216508546347844,false,false,0.0004991473878973766,false,0.0],"torque":-0.05662209269070197,"active":true,"saturated":false,"log":[0.03764919191598892,0.07529838383197784,0.0236995667219162,-0.0,0.08111749589443207,-0.05662209168076515,0.0808618813753128,0.11249615997076035,0.3321834206581116]},{"input":[true,15.0,-0.5161558352891047,false,false,0.000496094218475557,false,0.0],"torque":-0.055816119764030225,"active":true,"saturated":false,"log":[0.03801410272717476,0.07602820545434952,0.023832617327570915,-0.0,0.07742644101381302,-0.05581612139940262,0.08036726713180542,0.11230815947055817,0.3040769696235657]},{"input":[true,15.0,-0.5066940268229703,false,false,0.0004908370023555395,false,0.0],"torque":-0.05467058456523346,"active":true,"saturated":false,"log":[0.038210127502679825,0.07642025500535965,0.0239663515239954,-0.0,0.07300621271133423,-0.054670583456754684,0.07951559871435165,0.1116212010383606,0.2744641900062561]},{"input":[true,15.0,-0.4933074661457673,false,false,0.00048339909628973055,false,0.0],"torque":-0.05338256533587525,"active":true,"saturated":false,"log":[0.03823639079928398,0.07647278159856796,0.024100178852677345,-0.0,0.06862723082304001,-0.05338256433606148,0.07831065356731415,0.11043832451105118,0.24348755180835724]},{"input":[true,15.0,-0.4760556270505005,false,false,0.00047381354539676926,false,0.0],"torque":-0.05250733824476367,"active":true,"saturated":false,"log":[0.038092777132987976,0.07618555426597595,0.024233503267169,-0.0,0.06632916629314423,-0.05250733718276024,0.07675779610872269,0.10876479744911194,0.21129480004310608]},{"input":[true,15.0,-0.4550151559837116,false,false,0.0004621229363487218,false,0.0],"torque":-0.0514846704157696,"active":true,"saturated":false,"log":[0.037779923528432846,0.07555984705686569,0.024365734308958054,-0.0,0.06348540633916855,-0.051484670490026474,0.07486391812562943,0.10660804808139801,0.17803837358951569]},{"input":[true,15.0,-0.4302795315207519,false,false,0.00044837920816707353,false,0.0],"torque":-0.05021191313805354,"active":true,"saturated":false,"log":[0.037299223244190216,0.07459844648838043,0.024496281519532204,-0.0,0.06010928004980087,-0.050211913883686066,0.07263743132352829,0.10397765785455704,0.14387477934360504]},{"input":[true,15.0,-0.40195864905930007,false,false,0.00043264342146811797,false,0.0],"torque":-0.04867450429889213,"active":true,"saturated":false,"log":[0.036652810871601105,0.07330562174320221,0.024624565616250038,-0.0,0.05621521174907684,-0.048674505203962326,0.07008823752403259,0.10088532418012619,0.1089639663696289]},{"input":[true,15.0,-0.37017833257624316,false,false,0.00041498548718293574,false,0.0],"torque":-0.04687550685014069,"active":true,"saturated":false,"log":[0.03584355488419533,0.07168710976839066,0.02475001849234104,0.0,0.05181916058063507,-0.04687550663948059,0.06722764670848846,0.097344771027565,0.07346858084201813]},{"input":[true,15.0,-0.3350797756170885,false,false,0.00039548385595720845,false,0.0],"torque":-0.04482223781450374,"active":true,"saturated":false,"log":[0.0348750539124012,0.0697501078248024,0.024872081354260445,0.0,0.04693900793790817,-0.04482223838567734,0.06406838446855545,0.09337173402309418,0.03755338490009308]},{"input":[true,15.0,-0.29681891400147703,false,false,0.00037422516961079937,false,0.0],"torque":-0.042523749536802276,"active":true,"saturated":false,"log":[0.03375161066651344,0.06750322133302689,0.02499021217226982,0.0,0.041594840586185455,-0.04252374917268753,0.06062448024749756,0.08898387104272842,0.0013844863278791308]},{"input":[true,15.0,-0.25556573303174407,false,false,0.0003513038762055989,false,0.0],"torque":-0.03999566617570402,"active":true,"saturated":false,"log":[0.032478217035532,0.064956434071064,0.025103885680437088,0.0,0.035830020904541016,-0.03999566659331322,0.05691123008728027,0.0842006653547287,-0.034871336072683334]},{"input":[true,15.0,-0.2115035122824543,false,false,0.00032682181043180595,false,0.0],"torque":-0.037245444350583946,"active":true,"saturated":false,"log":[0.03106052801012993,0.06212105602025986,0.025212597101926804,0.0,0.029649505391716957,-0.03724544495344162,0.05294513329863548,0.07904337346553802,-0.0710473358631134]},{"input":[true,15.0,-0.16482801132616698,false,false,0.0003008877411769109,false,0.0],"torque":-0.03428496131374969,"active":true,"saturated":false,"log":[0.029504843056201935,0.05900968611240387,0.025315864011645317,0.0,0.02308070845901966,-0.034284960478544235,0.04874381422996521,0.07353490591049194,-0.10697753727436066]},{"input":[true,15.0,-0.11574660001307968,false,false,0.00027361688828743294,false,0.0],"torque":-0.03112776721394622,"active":true,"saturated":false,"log":[0.027818074449896812,0.055636148899793625,0.025413228198885918,0.0,0.016153300181031227,-0.031127767637372017,0.04432593658566475,0.06769974529743195,-0.14249739050865173]},{"input":[true,15.0,-0.06447733716855109,false,false,0.0002451304106703497,false,0.0],"torque":-0.027788210464782984,"active":true,"saturated":false,"log":[0.026007715612649918,0.052015431225299835,0.025504253804683685,0.0,0.008897921070456505,-0.027788210660219193,0.039711128920316696,0.061563801020383835,-0.17744456231594086]},{"input":[true,15.0,-0.01124800180165425,false,false,0.00021555486800847306,false,0.0],"torque":-0.024281132481396885,"active":true,"saturated":false,"log":[0.02408180944621563,0.04816361889243126,0.025588540360331535,0.0,0.0013457200257107615,-0.024281132966279984,0.03491989150643349,0.05515434220433235,-0.2116595357656479]},{"input":[false,15.0,0.04370491887108324,false,false,0.00018502165848126905,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.10013728009075473,false,false,0.0001536664349892097,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.1577983642420642,false,false,0.00012162850247525857,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.21643199474119962,false,false,8.905019902106331e-05,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.2757776741767982,false,false,5.6076263467527434e-05,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.33557174164748155,false,false,2.2853192369290225e-05,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.3955485441541351,false,false,-1.0471410859959058e-05,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.45544161684268525,false,false,-4.37494917197232e-05,true,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.5149848668537761,false,false,-7.683320239902244e-05,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.5739137555197533,false,false,-0.00010957555863444268,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.6319664736566829,false,false,-0.00014183109273161314,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.688885104729831,false,false,-0.00017345649984887746,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.7444167707248802,false,false,-0.00020431127467184204,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.7983147556340155,false,false,-0.00023425833565018857,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,15.0,0.8503396015654614,false,false,-0.00026316463402339594,false,0.0],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,15.0,0.9002601726066652,false,false,-0.00029090174492958534,false,0.0],"torque":0.0404513466351201,"active":true,"saturated":false,"log":[-0.014383972622454166,-0.028767945244908333,-5.034390414948575e-05,0.0,-0.13123178482055664,0.04045134782791138,-0.047126080840826035,-0.05921204388141632,-0.04375579208135605]},{"input":[true,15.0,0.9478546817146065,false,false,-0.00031734643797131734,false,0.0],"torque":0.05051558223526202,"active":true,"saturated":false,"log":[-0.016712848097085953,-0.033425696194171906,-0.00010883887443924323,0.0,-0.13831619918346405,0.05051558092236519,-0.051410119980573654,-0.06545289605855942,-0.0824049711227417]},{"input":[true,15.0,0.9929116760709586,false,false,-0.00034238122470342736,false,0.0],"torque":0.05564161485110392,"active":true,"saturated":false,"log":[-0.01896747201681137,-0.03793494403362274,-0.00017522502457723022,0.0,-0.14505374431610107,0.05564161390066147,-0.055465757846832275,-0.07140295207500458,-0.11612022668123245]},{"input":[true,15.0,1.0352309765243763,false,false,-0.0003658948806105272,false,0.0],"torque":0.05945307595399253,"active":true,"saturated":false,"log":[-0.021137826144695282,-0.042275652289390564,-0.0002492074272595346,0.0,-0.15141426026821136,0.059453077614307404,-0.05927496775984764,-0.07703577727079391,-0.14507202804088593]},{"input":[true,15.0,1.0746245669461476,false,false,-0.00038778293925512483,false,0.0],"torque":0.06281039365349814,"active":true,"saturated":false,"log":[-0.023214269429445267,-0.04642853885889053,-0.00033045734744518995,0.0,-0.15736843645572662,0.06281039118766785,-0.06282083690166473,-0.08232634514570236,-0.16942955553531647]},{"input":[true,15.0,1.1109174295479862,false,false,-0.0004079481564009665,false,0.0],"torque":0.06590906588376245,"active":true,"saturated":false,"log":[-0.025187578052282333,-0.05037515610456467,-0.0004186138685327023,0.0,-0.1628880351781845,0.06590906530618668,-0.06608760356903076,-0.08725116401910782,-0.18936152756214142]},{"input":[true,15.0,1.1439483224508153,false,false,-0.00042630094204959286,false,0.0],"torque":0.06878501229462994,"active":true,"saturated":false,"log":[-0.027048980817198753,-0.05409796163439751,-0.0005132853402756155,0.0,-0.16794638335704803,0.06878501176834106,-0.06906075030565262,-0.09178833663463593,-0.20503687858581543]},{"input":[true,15.0,1.1735704960499642,false,false,-0.00044275975847065947,false,0.0],"torque":0.07143639291770558,"active":true,"saturated":false,"log":[-0.02879021130502224,-0.05758042261004448,-0.0006140510668046772,0.0,-0.17251873016357422,0.07143639028072357,-0.07172708213329315,-0.095917709171772,-0.2166254222393036]},{"input":[true,15.0,1.1996523449941041,false,false,-0.00045725148245765016,false,0.0],"torque":0.07385306166366937,"active":true,"saturated":false,"log":[-0.030403533950448036,-0.06080706790089607,-0.0007204634021036327,0.0,-0.1765827089548111,0.07385306060314178,-0.07407473772764206,-0.09962094575166702,-0.224298357963562]},{"input":[true,15.0,1.222077992881319,false,false,-0.00046971173019955657,false,0.0],"torque":0.07602376510012761,"active":true,"saturated":false,"log":[-0.03188177943229675,-0.0637635588645935,-0.0008320496417582035,0.0,-0.1801186203956604,0.07602376490831375,-0.07609330117702484,-0.10288158059120178,-0.2282286286354065]},{"input":[true,15.0,1.2407478070746218,false,false,-0.000480085143325183,false,0.0],"torque":0.07793796072185988,"active":true,"saturated":false,"log":[-0.0332183800637722,-0.0664367601275444,-0.0009483139729127288,0.0,-0.1831096112728119,0.07793796062469482,-0.0777737945318222,-0.10568513721227646,-0.22859133780002594]},{"input":[true,15.0,1.2555788413496944,false,false,-0.0004883256348492412,false,0.0],"torque":0.07958631883497229,"active":true,"saturated":false,"log":[-0.03440739959478378,-0.06881479918956757,-0.0010687399189919233,0.0,-0.18554182350635529,0.07958631962537766,-0.07910875231027603,-0.108019158244133,-0.2255638837814331]},{"input":[true,15.0,1.2665052044082559,false,false,-0.0004943965939275431,false,0.0],"torque":0.08096088788072996,"active":true,"saturated":false,"log":[-0.03544355183839798,-0.07088710367679596,-0.0011927923187613487,0.0,-0.18740445375442505,0.08096088469028473,-0.08009224385023117,-0.10987326502799988,-0.2193262130022049]},{"input":[true,15.0,1.2734783526198317,false,false,-0.0004982710485116088,false,0.0],"torque":0.08205517351150506,"active":true,"saturated":false,"log":[-0.03632223606109619,-0.07264447212219238,-0.001319920178502798,0.0,-0.188689723610878,0.08205517381429672,-0.08071991056203842,-0.1112392321228981,-0.21006086468696594]},{"input":[true,15.0,1.2764673056913245,false,false,-0.0004999317851800449,false,0.0],"torque":0.08286417055827724,"active":true,"saturated":false,"log":[-0.03703954815864563,-0.07407909631729126,-0.0014495585346594453,0.0,-0.18939290940761566,0.0828641727566719,-0.08098895102739334,-0.11211098730564117,-0.19795292615890503]},{"input":[true,15.0,1.2754587843062206,false,false,-0.0004993714256143098,false,0.0],"torque":0.08338437032911704,"active":true,"saturated":false,"log":[-0.03759229928255081,-0.07518459856510162,-0.0015811315970495343,0.0,-0.1895121932029724,0.08338437229394913,-0.08089817315340042,-0.1124846488237381,-0.18319009244441986]},{"input":[true,15.0,1.2704572691219147,false,false,-0.0004965924593790963,false,0.0],"torque":0.0836137469824504,"active":true,"saturated":false,"log":[-0.03797803819179535,-0.0759560763835907,-0.0017140547279268503,0.0,-0.18904857337474823,0.08361374586820602,-0.0804479792714119,-0.11235857009887695,-0.16596241295337677]},{"input":[true,15.0,1.2614849808630548,false,false,-0.0004916072328616975,false,0.0],"torque":0.08354765707490239,"active":true,"saturated":false,"log":[-0.03819504752755165,-0.0763900950551033,-0.0018477373523637652,0.0,-0.18800559639930725,0.08354765921831131,-0.07964037358760834,-0.11173330247402191,-0.14646218717098236]},{"input":[true,15.0,1.2485817815993359,false,false,-0.0004844378944194944,false,0.0],"torque":0.08317523972323423,"active":true,"saturated":false,"log":[-0.038242362439632416,-0.07648472487926483,-0.0019815857522189617,0.0,-0.1863892823457718,0.08317524194717407,-0.07847893983125687,-0.11061162501573563,-0.12488368898630142]},{"input":[true,15.0,1.2318049976463534,false,false,-0.0004751162959792648,false,0.0],"torque":0.08251150257379954,"active":true,"saturated":false,"log":[-0.03811977803707123,-0.07623955607414246,-0.0021150049287825823,0.0,-0.18420787155628204,0.08251149952411652,-0.07696884125471115,-0.10899852961301804,-0.10142286866903305]}]},{"name":"delay_clamped","frames":[{"input":[false,18.0,2.8983315731449384,false,false,-0.0013323183357528668,true,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.820585963139843,false,false,-0.001291240963619141,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.740125178323684,false,false,-0.0012487290121424167,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.6570386112909286,false,false,-0.0012048297124508391,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.571418571901707,false,false,-0.0011595918370278437,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.483360184724799,false,false,-0.0011130656455255266,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.392961283353453,false,false,-0.0010653028289257553,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.3003223017114602,false,false,-0.0010163564521110636,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.205546162470241,false,false,-0.0009662808949091303,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.108738162700902,false,false,-0.000915131791676342,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,2.0100058568883306,false,false,-0.0008629659694875654,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.9094589374372715,false,false,-0.0008098413850007994,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.8072091128031653,false,false,-0.0007558170600668505,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.7033699833831322,false,false,-0.000700953016155571,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.5980569153049962,false,false,-0.0006453102076715109,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.4913869122545682,false,false,-0.000588950454233074,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.3834784854835853,false,false,-0.0005319363719904111,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.2744515221427377,false,false,-0.0004743313040583631,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.1644271520860527,false,false,-0.0004161992501417374,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,1.053527613294633,false,false,-0.00035760479543111017,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,18.0,0.9418761160692521,false,false,-0.000298613038848147,false,1.4],"torque":0.0009359411836002879,"active":true,"saturated":false,"log":[0.0710289403796196,0.12216977775096893,0.0,-0.0,-0.1274147778749466,0.0009359411778859794,-0.06966044753789902,-0.006783371325582266,-0.004845265299081802]},{"input":[true,18.0,0.8295967061426952,false,false,-0.00023928952072016532,false,1.4],"torque":0.005057132454742649,"active":true,"saturated":false,"log":[0.049792513251304626,0.08564312011003494,0.00017427379498258233,-0.0,-0.1166178435087204,0.005057132337242365,-0.055821456015110016,-0.01174353901296854,-0.00838824175298214]},{"input":[true,18.0,0.716814126863929,false,false,-0.00017970014996429066,false,1.4],"torque":0.01028483773074304,"active":true,"saturated":false,"log":[0.03040803223848343,0.05230181664228439,0.0002807019045576453,-0.0,-0.1050056517124176,0.01028483733534813,-0.04192044958472252,-0.015002291649580002,-0.010715922340750694]},{"input":[true,18.0,0.603653680607213,false,false,-0.00011991113086210739,false,1.4],"torque":0.015497085101440815,"active":true,"saturated":false,"log":[0.012763302773237228,0.021952880546450615,0.00032537346123717725,-0.0,-0.0926414281129837,0.015497084707021713,-0.027972867712378502,-0.016674384474754333,-0.011910274624824524]},{"input":[true,18.0,0.4902410895601319,false,false,-5.998888950615651e-05,false,1.4],"torque":0.020185195904845573,"active":true,"saturated":false,"log":[-0.0032464133109897375,-0.005583831109106541,0.00031401103478856385,-0.0,-0.07958593964576721,0.02018519677221775,-0.013994206674396992,-0.01686803437769413,-0.012048596516251564]},{"input":[true,18.0,0.37670235604520796,false,false,0.0,false,1.4],"torque":0.02413557631904144,"active":true,"saturated":false,"log":[-0.017718959599733353,-0.030476611107587814,0.00025199467199854553,-0.0,-0.06589777767658234,0.02413557656109333,1.5514901585689245e-09,-0.01568538509309292,-0.011203845962882042]},{"input":[true,18.0,0.263163622530284,false,false,5.998888950615651e-05,false,1.4],"torque":0.027275913566226603,"active":true,"saturated":false,"log":[-0.030745774507522583,-0.05288273096084595,0.00014438445214182138,-0.0,-0.05163351446390152,0.027275914326310158,0.013994209468364716,-0.01322292909026146,-0.009444949217140675]},{"input":[true,18.0,0.14975103148320296,false,false,0.00011991113086210739,false,1.4],"torque":0.029600854631929498,"active":true,"saturated":false,"log":[-0.042412374168634415,-0.07294928282499313,-4.058856575284153e-06,-0.0,-0.03684786334633827,0.029600854963064194,0.02797286957502365,-0.009571912698447704,-0.006837080232799053]},{"input":[true,18.0,0.03659058522648717,false,false,0.00017970014996429066,false,1.4],"torque":0.031136085592041524,"active":true,"saturated":false,"log":[-0.052798762917518616,-0.09081387519836426,-0.00018885452300310135,-0.0,-0.02159377560019493,0.031136086210608482,0.04192045331001282,-0.004818692337721586,-0.0034419228322803974]},{"input":[true,18.0,-0.07619199405227922,false,false,0.00023928952072016532,false,1.4],"torque":0.0319240236587157,"active":true,"saturated":false,"log":[-0.061979856342077255,-0.10660535097122192,-0.0004057840269524604,-0.0,-0.005925243720412254,0.03192402422428131,0.055821459740400314,0.0009549183305352926,0.0006820845301263034]},{"input":[true,18.0,-0.1884714039788361,false,false,0.000298613038848147,false,1.4],"torque":0.03202560510393668,"active":true,"saturated":false,"log":[-0.07002585381269455,-0.12044446915388107,-0.0006508745136670768,-0.0,0.010091430507600307,0.03202560544013977,0.06966044753789902,0.00767133804038167,0.005479527171701193]},{"input":[true,18.0,-0.300122901204217,false,false,0.00035760479543111017,false,1.4],"torque":0.03137252109983872,"active":true,"saturated":false,"log":[-0.07700256258249283,-0.13244441151618958,-0.0009203834924846888,-0.0,0.026420239359140396,0.031372521072626114,0.08342204988002777,0.015256929211318493,0.010897806845605373]},{"input":[true,18.0,-0.41102243999563665,false,false,0.0004161992501417374,false,1.4],"torque":0.030058255889802165,"active":true,"saturated":false,"log":[-0.08297175168991089,-0.14271141588687897,-0.001210784655995667,-0.0,0.04301513731479645,0.030058255419135094,0.0970909595489502,0.02364172600209713,0.01688694767653942]},{"input":[true,18.0,-0.5210468100523218,false,false,0.0004743313040583631,false,1.4],"torque":0.028162542689163392,"active":true,"saturated":false,"log":[-0.08799143135547638,-0.15134526789188385,-0.001518754637800157,-0.0,0.059831760823726654,0.02816254273056984,0.11065200716257095,0.03275919333100319,0.02339942380785942]},{"input":[true,18.0,-0.6300737733931694,false,false,0.0005319363719904111,false,1.4],"torque":0.02573857515461219,"active":true,"saturated":false,"log":[-0.09211613237857819,-0.1584397405385971,-0.0018411610508337617,-0.0,0.07682734727859497,0.02573857456445694,0.12409012019634247,0.04254598915576935,0.030389990657567978]},{"input":[true,18.0,-0.7379822001641521,false,false,0.000588950454233074,false,1.4],"torque":0.0228278832365585,"active":true,"saturated":false,"log":[-0.09539715945720673,-0.1640831083059311,-0.0021750512532889843,-0.0,0.09396065026521683,0.022827882319688797,0.1373903602361679,0.052941758185625076,0.03781554102897644]},{"input":[true,18.0,-0.84465220321458,false,false,0.0006453102076715109,false,1.4],"torque":0.01946608529034803,"active":true,"saturated":false,"log":[-0.09788285940885544,-0.16835851967334747,-0.002517641056329012,-0.0,0.11119192093610764,0.019466085359454155,0.15053796768188477,0.06388894468545914,0.045634958893060684]},{"input":[true,18.0,-0.9499652712927162,false,false,0.000700953016155571,false,1.4],"torque":0.01568510764194317,"active":true,"saturated":false,"log":[-0.09961878508329391,-0.1713443100452423,-0.002866306807845831,-0.0,0.12848280370235443,0.015685107558965683,0.16351832449436188,0.07533259689807892,0.05380899831652641]},{"input":[true,18.0,-1.0538044007127494,false,false,0.0007558170600668505,false,1.4],"torque":0.011514082590484358,"active":true,"saturated":false,"log":[-0.10064796358346939,-0.17311449348926544,-0.003218574682250619,-0.0,0.14579638838768005,0.011514082551002502,0.1763170063495636,0.0872202217578888,0.06230015680193901]},{"input":[true,18.0,-1.1560542253468555,false,false,0.0008098413850007994,false,1.4],"torque":0.006979705746350859,"active":true,"saturated":false,"log":[-0.10101103782653809,-0.1737389862537384,-0.0035721133463084698,-0.0,0.1630970984697342,0.006979705765843391,0.188919797539711,0.0995016098022461,0.07107257843017578]},{"input":[true,18.0,-1.2566011447979146,false,false,0.0008629659694875654,false,1.4],"torque":0.0021064931976542533,"active":true,"saturated":false,"log":[-0.10074646770954132,-0.1732839196920395,-0.003924726042896509,-0.0,0.18035075068473816,0.002106493106111884,0.20131270587444305,0.11212871968746185,0.0800919458270073]},{"input":[true,18.0,-1.3553334506104864,false,false,0.000915131791676342,false,1.4],"torque":-0.00308237866404078,"active":true,"saturated":false,"log":[-0.09989067912101746,-0.17181196808815002,-0.004274343606084585,-0.0,0.1975245177745819,-0.0030823787674307823,0.21348194777965546,0.12505553662776947,0.08932538330554962]},{"input":[true,18.0,-1.4521414503798251,false,false,0.0009662808949091303,false,1.4],"torque":-0.007681828031907103,"active":true,"saturated":false,"log":[-0.09847823530435562,-0.16938257217407227,-0.004619017243385315,-0.0,0.21458689868450165,-0.0076818279922008514,0.22541400790214539,0.13823793828487396,0.09874138236045837]},{"input":[true,18.0,-1.5469175896210448,false,false,0.0010163564521110636,false,1.4],"torque":-0.01297179300774276,"active":true,"saturated":false,"log":[-0.09654194861650467,-0.16605214774608612,-0.004956914111971855,-0.0,0.23150776326656342,-0.012971793301403522,0.2370956391096115,0.1516336351633072,0.10830973833799362]},{"input":[true,18.0,-1.6395565712630369,false,false,0.0010653028289257553,false,1.4],"torque":-0.01872427399177355,"active":true,"saturated":false,"log":[-0.09411302208900452,-0.16187439858913422,-0.005286309868097305,-0.0,0.24825827777385712,-0.01872427389025688,0.2485138475894928,0.16520200669765472,0.11800143122673035]},{"input":[true,18.0,-1.7299554726343829,false,false,0.0011130656455255266,false,1.4],"torque":-0.024821721917743577,"active":true,"saturated":false,"log":[-0.09122118353843689,-0.15690043568611145,-0.005605584010481834,-0.0,0.2648109197616577,-0.024821721017360687,0.2596559524536133,0.17890405654907227,0.12778861820697784]},{"input":[true,18.0,-1.8180138598112914,false,false,0.0011595918370278437,false,1.4],"torque":-0.031200454019091627,"active":true,"saturated":false,"log":[-0.08789476007223129,-0.1511789858341217,-0.005913215223699808,-0.0,0.28113940358161926,-0.031200453639030457,0.2705095708370209,0.19270233809947968,0.13764452934265137]},{"input":[true,18.0,-1.9036338992005135,false,false,0.0012048297124508391,false,1.4],"torque":-0.037823067877859655,"active":true,"saturated":false,"log":[-0.08416081964969635,-0.14475661516189575,-0.0062077781185507774,-0.0,0.29721859097480774,-0.037823066115379333,0.28106266260147095,0.20656083524227142,0.14754346013069153]},{"input":[true,18.0,-1.9867204662332687,false,false,0.0012487290121424167,false,1.4],"torque":-0.04466486310842446,"active":true,"saturated":false,"log":[-0.08004523813724518,-0.13767780363559723,-0.0064879367128014565,-0.0,0.3130244016647339,-0.044664863497018814,0.29130351543426514,0.220444917678833,0.15746065974235535]},{"input":[true,18.0,-2.0671812510494276,false,false,0.001291240963619141,false,1.4],"torque":-0.051707018493148786,"active":true,"saturated":false,"log":[-0.07557278126478195,-0.12998518347740173,-0.006752441171556711,-0.0,0.32853370904922485,-0.05170701816678047,0.30122068524360657,0.23432126641273499,0.16737233102321625]},{"input":[true,18.0,-2.1449268610545236,false,false,0.0013323183357528668,false,1.4],"torque":-0.05893304191272093,"active":true,"saturated":false,"log":[-0.07076720148324966,-0.12171958386898041,-0.007000126410275698,-0.0,0.34372422099113464,-0.058933041989803314,0.31080323457717896,0.24815784394741058,0.17725560069084167]},{"input":[true,18.0,-2.219870920235663,false,false,0.0013719154912449113,false,1.4],"torque":-0.06632684289310994,"active":true,"saturated":false,"log":[-0.0656513199210167,-0.11292026937007904,-0.007229906041175127,-0.0,0.3585742712020874,-0.06632684171199799,0.32004043459892273,0.26192381978034973,0.18708842992782593]},{"input":[true,18.0,-2.2919301651259882,false,false,0.0014099884373294702,false,1.4],"torque":-0.07396776005244658,"active":true,"saturated":false,"log":[-0.06024705618619919,-0.10362493991851807,-0.007440770976245403,-0.0,0.373662531375885,-0.07396776229143143,0.3289220929145813,0.2755894958972931,0.19684964418411255]},{"input":[true,18.0,-2.36102453731107,false,false,0.0014464948746499401,true,1.4],"torque":-0.08203791209856846,"active":true,"saturated":false,"log":[-0.054575517773628235,-0.0938698872923851,-0.007631785236299038,-0.0,0.3899124562740326,-0.08203791081905365,0.33743831515312195,0.28912633657455444,0.20651881396770477]},{"input":[true,18.0,-2.42707727237447,false,false,0.0014813942442538548,false,1.4],"torque":-0.09036822519289864,"active":true,"saturated":false,"log":[-0.04865705966949463,-0.08369014412164688,-0.007802085019648075,-0.0,0.40576183795928955,-0.09036822617053986,0.3455796539783478,0.30250686407089233,0.2160763442516327]},{"input":[true,18.0,-2.4900149851836364,false,false,0.0015146477726542137,false,1.4],"torque":-0.09886182807236765,"active":true,"saturated":false,"log":[-0.042511310428380966,-0.07311945408582687,-0.007950874045491219,-0.0,0.42116791009902954,-0.09886182844638824,0.35333701968193054,0.31570467352867126,0.2255033403635025]},{"input":[true,18.0,-2.549767751421393,false,false,0.0015462185149071458,false,1.4],"torque":-0.10745604571735896,"active":true,"saturated":false,"log":[-0.03615724295377731,-0.0621904581785202,-0.008077424950897694,-0.0,0.4360896646976471,-0.10745604336261749,0.36070185899734497,0.32869431376457214,0.2347816526889801]},{"input":[true,18.0,-2.6062691852724296,false,false,0.0015760713956580457,false,1.4],"torque":-0.11610451521915519,"active":true,"saturated":false,"log":[-0.02961321920156479,-0.05093473568558693,-0.008181070908904076,-0.0,0.4504888951778412,-0.11610451340675354,0.36766594648361206,0.34145137667655945,0.2438938319683075]},{"input":[true,18.0,-2.659456513178491,false,false,0.0016041732481105837,false,1.4],"torque":-0.12476969701058328,"active":true,"saturated":false,"log":[-0.022897005081176758,-0.039382848888635635,-0.008261210285127163,-0.0,0.46433117985725403,-0.12476969510316849,0.374221533536911,0.3539523780345917,0.2528231143951416]},{"input":[true,18.0,-2.709270643580313,false,false,0.001630492850875286,false,1.4],"torque":-0.13341818344384923,"active":true,"saturated":false,"log":[-0.016025830060243607,-0.027564426884055138,-0.00831730104982853,-0.0,0.4775862693786621,-0.13341818749904633,0.3803613781929016,0.3661747872829437,0.26155343651771545]},{"input":[true,18.0,-2.7556562325688385,false,false,0.0016550009626567592,false,1.4],"torque":-0.14201732175005277,"active":true,"saturated":false,"log":[-0.009016407653689384,-0.015508221462368965,-0.008348858915269375,-0.0,0.4902283251285553,-0.14201731979846954,0.3860786259174347,0.3780970275402069,0.27006930112838745]},{"input":[true,18.0,-2.798561745372749,false,false,0.0016776703547410072,false,1.4],"torque":-0.15053778980941962,"active":true,"saturated":false,"log":[-0.0018849711632356048,-0.003242150414735079,-0.008355455473065376,-0.0,0.5022356510162354,-0.15053778886795044,0.3913669288158417,0.3896982967853546,0.2783559262752533]},{"input":[true,18.0,-2.837939513614026,false,false,0.0016984758412467596,false,1.4],"torque":-0.15894083439695927,"active":true,"saturated":false,"log":[0.005352703854441643,0.009206650778651237,-0.008336720988154411,-0.0,0.5135902762413025,-0.15894083678722382,0.39622044563293457,0.4009588360786438,0.28639915585517883]},{"input":[true,18.0,-2.8737457882679034,false,false,0.0017173943071071896,false,1.4],"torque":-0.16720356540590534,"active":true,"saturated":false,"log":[0.012681274674832821,0.021811792626976967,-0.008292336948215961,-0.0,0.5242774486541748,-0.16720356047153473,0.4006337523460388,0.4118596017360687,0.29418542981147766]},{"input":[true,18.0,-2.9059407882684036,false,false,0.0017344047337509473,false,1.4],"torque":-0.17530921307651842,"active":true,"saturated":false,"log":[0.020085809752345085,0.0345475934445858,-0.008222036994993687,-0.0,0.5342851877212524,-0.1753092110157013,0.40460193157196045,0.42238253355026245,0.3017018139362335]},{"input":[true,18.0,-2.9344887447054253,false,false,0.0017494882224539628,false,1.4],"torque":-0.18324079965374995,"active":true,"saturated":false,"log":[0.027551770210266113,0.04738904535770416,-0.008125605061650276,-0.0,0.5436035990715027,-0.18324080109596252,0.40812060236930847,0.4325103163719177,0.3089359402656555]},{"input":[true,18.0,-2.9593579405642947,false,false,0.0017626280153360862,false,1.4],"torque":-0.19098218084806257,"active":true,"saturated":false,"log":[0.03506498783826828,0.060311779379844666,-0.008002878166735172,-0.0,0.552224338054657,-0.1909821778535843,0.4111858606338501,0.44222649931907654,0.3158760666847229]},{"input":[true,18.0,-2.9805207459636334,false,false,0.0017738095139792282,false,1.4],"torque":-0.19851843223960644,"active":true,"saturated":false,"log":[0.042611654847860336,0.07329204678535461,-0.007853737100958824,-0.0,0.5601406693458557,-0.19851842522621155,0.41379427909851074,0.45151546597480774,0.32251104712486267]},{"input":[true,18.0,-2.997953648852382,false,false,0.0017830202956463255,false,1.4],"torque":-0.20583592366081008,"active":true,"saturated":false,"log":[0.05017831176519394,0.08630669862031937,-0.00767811294645071,-0.0,0.5673466324806213,-0.20583592355251312,0.4159429669380188,0.46036240458488464,0.3288303017616272]},{"input":[true,18.0,-3.011637281131884,false,false,0.0017902501270831014,false,1.4],"torque":-0.21292225212511245,"active":true,"saturated":false,"log":[0.05775183439254761,0.09933315217494965,-0.007475981488823891,-0.0,0.5738372206687927,-0.21292224526405334,0.41762953996658325,0.46875330805778503,0.3348237872123718]},{"input":[true,18.0,-3.021556440174003,false,false,0.001795490975887298,false,1.4],"torque":-0.21976611564020662,"active":true,"saturated":false,"log":[0.06531941145658493,0.11234939098358154,-0.007247363682836294,-0.0,0.5796080231666565,-0.21976611018180847,0.41885215044021606,0.47667497396469116,0.34048211574554443]},{"input":[true,18.0,-3.027700105711366,false,false,0.0017987370194327387,false,1.4],"torque":-0.22635718350626047,"active":true,"saturated":false,"log":[0.07286856323480606,0.12533393502235413,-0.006992323789745569,-0.0,0.5846552848815918,-0.22635717689990997,0.4196093678474426,0.48411494493484497,0.3457963764667511]},{"input":[true,18.0,-3.030061452080972,false,false,0.0017999846513383159,false,1.4],"torque":-0.23268595955191412,"active":true,"saturated":false,"log":[0.08038710057735443,0.13826581835746765,-0.006710968911647797,-0.0,0.5889756679534912,-0.23268595337867737,0.41990041732788086,0.49106162786483765,0.35075831413269043]},{"input":[true,18.0,-3.02863785580756,false,false,0.001799232485474709,false,1.4],"torque":-0.23874366790512092,"active":true,"saturated":false,"log":[0.08786313235759735,0.15112458169460297,-0.006403448060154915,-0.0,0.5925664901733398,-0.23874366283416748,0.4197249412536621,0.49750420451164246,0.35536015033721924]},{"input":[true,18.0,-3.0234308985183134,false,false,0.0017964813575043867,false,1.4],"torque":-0.24452216415136366,"active":true,"saturated":false,"log":[0.0952850878238678,0.16389034688472748,-0.006069950293749571,-0.0,0.5954254269599915,-0.24452216923236847,0.4190831780433655,0.50343257188797,0.3595947027206421]},{"input":[true,18.0,-3.0144463651856555,false,false,0.001791734323953177,false,1.4],"torque":-0.25001384259882403,"active":true,"saturated":false,"log":[0.10264165699481964,0.17654365301132202,-0.005710704252123833,-0.0,0.5975507497787476,-0.2500138282775879,0.4179757833480835,0.5088374614715576,0.36345532536506653]},{"input":[true,18.0,-3.001694237700099,false,false,0.0017849966588144434,false,1.4],"torque":-0.2552115800956266,"active":true,"saturated":false,"log":[0.10992183536291122,0.18906556069850922,-0.005325977690517902,-0.0,0.5989412069320679,-0.25521159172058105,0.41640400886535645,0.5137103199958801,0.36693593859672546]},{"input":[true,18.0,-2.9851886837802866,false,false,0.0017762758476896352,false,1.4],"torque":-0.260108678327325,"active":true,"saturated":false,"log":[0.11711488664150238,0.20143760740756989,-0.004916075617074966,-0.0,0.5995961427688599,-0.26010867953300476,0.4143696427345276,0.5180434584617615,0.3700310289859772]},{"input":[true,18.0,-2.964948041232531,false,false,0.0017655815794717213,false,1.4],"torque":-0.26469883000354455,"active":true,"saturated":false,"log":[0.12421037256717682,0.21364183723926544,-0.0044813393615186214,-0.0,0.5995154976844788,-0.2646988332271576,0.4118748605251312,0.5218298435211182,0.3727356195449829]},{"input":[true,18.0,-2.940994797577373,false,false,0.0017529257355807513,false,1.4],"torque":-0.26897608180476384,"active":true,"saturated":false,"log":[0.13119812309741974,0.2256607711315155,-0.004022146109491587,-0.0,0.598699688911438,-0.268976092338562,0.4089225232601166,0.5250632762908936,0.37504521012306213]},{"input":[true,18.0,-2.9133555650657494,false,false,0.0017383223767634996,false,1.4],"torque":-0.2729348139344245,"active":true,"saturated":false,"log":[0.13806825876235962,0.2374774068593979,-0.0035389072727411985,-0.0,0.5971498489379883,-0.27293482422828674,0.4055158495903015,0.5277382731437683,0.37695589661598206]},{"input":[true,18.0,-2.8820610511125695,false,false,0.00172178772747186,false,1.4],"torque":-0.2765697202376605,"active":true,"saturated":false,"log":[0.14481118321418762,0.24907523393630981,-0.003032068023458123,0.0,0.5948677062988281,-0.2765697240829468,0.40165865421295166,0.5298501253128052,0.37846437096595764]},{"input":[true,18.0,-2.8471460241805215,false,false,0.001703340157837346,false,1.4],"torque":-0.2798757783517742,"active":true,"saturated":false,"log":[0.151417538523674,0.2604381740093231,-0.0025021065957844257,0.0,0.591855525970459,-0.279875785112381,0.39735519886016846,0.5313948392868042,0.3795677423477173]},{"input":[true,18.0,-2.8086492751520282,false,false,0.001683000163261724,false,1.4],"torque":-0.28284826066697205,"active":true,"saturated":false,"log":[0.1578783094882965,0.2715506851673126,-0.0019495325395837426,0.0,0.5881162881851196,-0.2828482687473297,0.3926102817058563,0.5323691964149475,0.38026368618011475]},{"input":[true,18.0,-2.7666135742322515,false,false,0.0016607903416464523,false,1.4],"torque":-0.28548269724093434,"active":true,"saturated":false,"log":[0.16418470442295074,0.28239768743515015,-0.0013748861383646727,0.0,0.5836535692214966,-0.2854827046394348,0.3874291777610779,0.5327706933021545,0.3805505037307739]},{"input":[true,18.0,-2.7210856234310463,false,false,0.001636735368286227,false,1.4],"torque":-0.28777488775809734,"active":true,"saturated":false,"log":[0.1703282743692398,0.2929646372795105,-0.0007787371287122369,0.0,0.5784716606140137,-0.2877748906612396,0.38181763887405396,0.5325976610183716,0.38042688369750977]},{"input":[true,18.0,-2.6721160046766426,false,false,0.0016108619684545263,false,1.4],"torque":-0.28972086426845617,"active":true,"saturated":false,"log":[0.1763007938861847,0.3032373785972595,-0.0001616843364899978,0.0,0.5725752711296082,-0.2897208631038666,0.3757818937301636,0.5318489670753479,0.37989211082458496]},{"input":[true,18.0,-2.619759123618704,false,false,0.001583198887711611,false,1.4],"torque":-0.2913168975458773,"active":true,"saturated":false,"log":[0.18209436535835266,0.3132023215293884,0.0004756459384225309,0.0,0.5659698843955994,-0.2913168966770172,0.36932864785194397,0.5305243730545044,0.3789459764957428]},{"input":[true,18.0,-2.564073149183207,false,false,0.0015537768599679727,false,1.4],"torque":-0.2925594739324278,"active":true,"saturated":false,"log":[0.18770135939121246,0.32284635305404663,0.0011326007079333067,0.0,0.5586614012718201,-0.2925594747066498,0.3624650537967682,0.5286242961883545,0.37758877873420715]},{"input":[true,18.0,-2.505119948946283,false,false,0.0015226285733387025,false,1.4],"torque":-0.29344528455759744,"active":true,"saturated":false,"log":[0.19311444461345673,0.33215683698654175,0.0018085011979565024,0.0,0.5506564378738403,-0.29344528913497925,0.35519880056381226,0.5261498689651489,0.3758213222026825]},{"input":[true,18.0,-2.4429650203988302,true,false,0.0014897886338267289,false,1.4],"torque":-0.29421327159074484,"active":true,"saturated":false,"log":[0.19832658767700195,0.34112173318862915,0.0018085011979565024,0.0,0.5419619679450989,-0.29421326518058777,0.34753790497779846,0.523102879524231,0.37364494800567627]},{"input":[true,18.0,-2.3776774181782647,true,false,0.001455293526875262,false,1.4],"torque":-0.2938138327793584,"active":true,"saturated":false,"log":[0.20333103835582733,0.34972938895225525,0.0018085011979565024,0.0,0.5325854420661926,-0.2938138246536255,0.3394908607006073,0.5194860100746155,0.37106141448020935]},{"input":[true,18.0,-2.3093296773482503,true,false,0.0014191815768321676,false,1.4],"torque":-0.29304064582228845,"active":true,"saturated":false,"log":[0.20812135934829712,0.3579687476158142,0.0018085011979565024,0.0,0.5225348472595215,-0.2930406332015991,0.33106666803359985,0.515302300453186,0.36807310581207275]},{"input":[true,18.0,-2.2379977328116425,true,false,0.0013814929043712983,false,1.4],"torque":-0.2918914290651524,"active":true,"saturated":false,"log":[0.21269138157367706,0.3658291697502136,0.0018085011979565024,0.0,0.5118184089660645,-0.2918914258480072,0.32227465510368347,0.5105558633804321,0.36468273401260376]},{"input":[true,18.0,-2.1637608349461854,true,false,0.0013422693819180966,false,1.4],"torque":-0.2903640553931455,"active":true,"saturated":false,"log":[0.21703527867794037,0.3733006715774536,0.0018085011979565024,0.0,0.5004446506500244,-0.2903640568256378,0.3131245970726013,0.5052511692047119,0.3608936667442322]},{"input":[true,18.0,-2.0867014615566943,true,false,0.0013015545871289842,false,1.4],"torque":-0.28845651229006064,"active":true,"saturated":false,"log":[0.22114750742912292,0.3803737163543701,0.0018085011979565024,0.0,0.48842236399650574,-0.2884564995765686,0.3036266565322876,0.4993934631347656,0.35670962929725647]},{"input":[true,18.0,-2.006905226241533,true,false,0.0012593937544762292,false,1.4],"torque":-0.2861669219897812,"active":true,"saturated":false,"log":[0.2250228375196457,0.38703927397727966,0.0018085011979565024,0.0,0.4757605195045471,-0.2861669361591339,0.29379138350486755,0.4929887652397156,0.3521348237991333]},{"input":[true,18.0,-1.9244607832752003,true,false,0.001215833724992072,false,1.4],"torque":-0.2834935493328187,"active":true,"saturated":false,"log":[0.2286563366651535,0.39328891038894653,0.0018085011979565024,0.0,0.4624681770801544,-0.2834935486316681,0.2836296856403351,0.4860435724258423,0.3471739888191223]},{"input":[true,18.0,-1.8394597291126995,true,false,0.0011709228942279536,false,1.4],"torque":-0.28043486211595303,"active":true,"saturated":false,"log":[0.23204341530799866,0.3991146683692932,0.0018085011979565024,0.0,0.4485543370246887,-0.28043487668037415,0.2731528878211975,0.4785650968551636,0.3418322205543518]},{"input":[true,18.0,-1.7519965006251252,true,false,0.0011247111584866602,false,1.4],"torque":-0.2769895488961537,"active":true,"saturated":false,"log":[0.23517975211143494,0.40450918674468994,0.0018085011979565024,0.0,0.4340280294418335,-0.27698954939842224,0.26237261295318604,0.4705612361431122,0.33611515164375305]},{"input":[true,18.0,-1.662168270179526,true,false,0.0010772498593871215,false,1.4],"torque":0.04772740438849123,"active":true,"saturated":false,"log":[-0.21538576483726501,-0.3704635202884674,0.0018085011979565024,0.0,0.20465348660945892,0.04772740602493286,0.2513008415699005,0.060634512454271317,0.35164690017700195]},{"input":[true,18.0,-1.5700748376795948,true,false,0.0010285917268234453,false,1.4],"torque":0.021923418408882702,"active":true,"saturated":false,"log":[-0.1669899970293045,-0.28722280263900757,0.0018085011979565024,0.0,0.21043968200683594,0.02192341908812523,0.23994988203048706,0.09212501347064972,0.36463361978530884]},{"input":[true,18.0,-1.4758185196871572,true,false,0.0009787908203815724,false,1.4],"torque":-0.002185894280671063,"active":true,"saturated":false,"log":[-0.12157690525054932,-0.2091122716665268,0.0018085011979565024,0.0,0.21474222838878632,-0.002185894176363945,0.22833232581615448,0.12070857733488083,0.3752119839191437]},{"input":[true,18.0,-1.3795040357476391,true,false,0.0009279024692786355,false,1.4],"torque":-0.02470578895397442,"active":true,"saturated":false,"log":[-0.07897479832172394,-0.1358366459608078,0.0018085011979565024,0.0,0.21761198341846466,-0.024705788120627403,0.21646109223365784,0.14655008912086487,0.38351064920425415]},{"input":[true,18.0,-1.2812383920457735,true,false,0.0008759832108917369,false,1.4],"torque":-0.045744770524335565,"active":true,"saturated":false,"log":[-0.03902445361018181,-0.06712205708026886,0.0018085011979565024,0.0,0.2190808802843094,-0.045744769275188446,0.2043493688106537,0.1698036789894104,0.38965103030204773]},{"input":[true,18.0,-1.1811307625208587,true,false,0.0008230907279444615,false,1.4],"torque":-0.06536380589926681,"active":true,"saturated":false,"log":[-0.001578188152052462,-0.0027144835330545902,0.0018085011979565024,0.0,0.21916607022285461,-0.065363809466362,0.19201061129570007,0.1906135380268097,0.3937477171421051]},{"input":[true,18.0,-1.079292367573608,false,false,0.0007692837844208935,true,1.4],"torque":-0.07475210719922082,"active":true,"saturated":false,"log":[0.033500999212265015,0.05762171745300293,0.0015640545170754194,0.0,0.2178768366575241,-0.07475210726261139,0.17945852875709534,0.209114670753479,0.3959091305732727]},{"input":[true,18.0,-0.975836350499375,false,false,0.0007146221602783498,false,1.4],"torque":-0.0880219535565902,"active":true,"saturated":false,"log":[0.06634022295475006,0.11410517990589142,0.0017962452257052064,0.0,0.21522219479084015,-0.08802195638418198,0.16670705378055573,0.22543352842330933,0.39623787999153137]},{"input":[true,18.0,-0.8708776517850112,false,false,0.0006591665850313498,false,1.4],"torque":-0.10246172153728692,"active":true,"saturated":false,"log":[0.09705743193626404,0.16693878173828125,0.0021359461825340986,0.0,0.21121712028980255,-0.10246171802282333,0.15377038717269897,0.2396887093782425,0.39483124017715454]},{"input":[true,18.0,-0.7645328814090462,false,false,0.0006029786702806292,false,1.4],"torque":-0.11676611747380627,"active":true,"saturated":false,"log":[0.12576210498809814,0.21631081402301788,0.0025761136785149574,0.0,0.20588617026805878,-0.11676611751317978,0.14066286385059357,0.25199148058891296,0.3917815685272217]},{"input":[true,18.0,-1.7048986033044529,false,false,0.000546120841262141,false,1.4],"torque":-0.07355241779658839,"active":true,"saturated":false,"log":[0.0066410256549716,0.011422564275562763,0.0025993571616709232,-0.0,0.13788926601409912,-0.07355241477489471,0.2565675675868988,0.26244643330574036,0.3871767222881317]},{"input":[true,18.0,-1.4858659225302335,false,false,0.0004886562674921086,false,1.4],"torque":-0.07004859450559373,"active":true,"saturated":false,"log":[0.046971987932920456,0.08079181611537933,0.0027637591119855642,0.0,0.1364731639623642,-0.07004859298467636,0.22957071661949158,0.2711518108844757,0.38110029697418213]},{"input":[true,18.0,-1.2647639130616395,false,false,0.00043064879258516834,false,1.4],"torque":-0.07936398591341685,"active":true,"saturated":false,"log":[0.0857192650437355,0.14743714034557343,0.0030637765303254128,0.0,0.1339358240365982,-0.07936398684978485,0.20231880247592926,0.2782002389431,0.3736321032047272]},{"input":[true,18.0,-1.0418382210518031,false,false,0.0003721628633235905,false,1.4],"torque":-0.09374227135083207,"active":true,"saturated":false,"log":[0.12294717133045197,0.2114691287279129,0.003494091797620058,0.0,0.13032019138336182,-0.09374227374792099,0.17484211921691895,0.2836788594722748,0.3648483157157898]},{"input":[true,18.0,-0.8173365187801724,false,false,0.0003132634580563725,false,1.4],"torque":-0.11000740202938647,"active":true,"saturated":false,"log":[0.1587142050266266,0.2729884386062622,0.00404959125444293,0.0,0.125671848654747,-0.1100074052810669,0.14717116951942444,0.2876700460910797,0.3548218309879303]},{"input":[true,18.0,-0.5915082294865843,false,false,0.00025401601450776096,false,1.4],"torque":-0.12663561488041225,"active":true,"saturated":false,"log":[0.19307348132133484,0.33208638429641724,0.004725348670035601,0.0,0.12003740668296814,-0.1266356110572815,0.11933672428131104,0.2902515232563019,0.34362247586250305]},{"input":[true,18.0,-0.3646042502599696,false,false,0.00019448635707539666,false,1.4],"torque":-0.14290004805187367,"active":true,"saturated":false,"log":[0.22607316076755524,0.388845831155777,0.005516604520380497,0.0,0.11346346884965897,-0.14290004968643188,0.09136968851089478,0.2914968430995941,0.33131730556488037]},{"input":[true,18.0,-0.1368766732896332,false,false,0.00013474062369886897,false,1.4],"torque":-0.15845857923035597,"active":true,"saturated":false,"log":[0.2577568590641022,0.44334179162979126,0.006418753881007433,0.0,0.10599619895219803,-0.15845857560634613,0.06330114603042603,0.29147571325302124,0.31797075271606445]},{"input":[true,18.0,0.09142149421123599,false,false,7.484519237992288e-05,false,1.4],"torque":-0.17315570790937113,"active":true,"saturated":false,"log":[0.28816401958465576,0.49564212560653687,0.007427327800542116,0.0,0.09768129885196686,-0.17315571010112762,0.03516227379441261,0.29025423526763916,0.3036448657512665]},{"input":[true,18.0,0.3200366110986804,false,false,1.4866607435947705e-05,false,1.4],"torque":-0.18692537054269448,"active":true,"saturated":false,"log":[0.31733033061027527,0.5458081960678101,0.008537983521819115,0.0,0.08856421709060669,-0.18692536652088165,0.006984333507716656,0.28789523243904114,0.28839942812919617]},{"input":[true,18.0,0.5487146840953611,false,false,-4.5128494431304356e-05,false,1.4],"torque":-0.19974680611874637,"active":true,"saturated":false,"log":[0.34528788924217224,0.5938951969146729,0.0097464919090271,0.0,0.07869063317775726,-0.19974680244922638,-0.02120136469602585,0.2844584584236145,0.27229219675064087]},{"input":[true,18.0,0.7772016499791863,false,false,-0.00010507345816964415,false,1.4],"torque":-0.2116216813831124,"active":true,"saturated":false,"log":[0.3720656633377075,0.6399529576301575,0.01104872114956379,0.0,0.06810685992240906,-0.21162168681621552,-0.04936350882053375,0.28000086545944214,0.2553790509700775]},{"input":[true,18.0,1.0052436578496313,false,false,-0.0001649016844307542,false,1.4],"torque":-0.22256362257326262,"active":true,"saturated":false,"log":[0.3976897597312927,0.6840263605117798,0.012440635822713375,0.0,0.05686032399535179,-0.22256362438201904,-0.0774708092212677,0.2745768427848816,0.23771405220031738]},{"input":[true,18.0,1.2325873511581973,false,false,-0.0002245467035626178,false,1.4],"torque":-0.2325932709471351,"active":true,"saturated":false,"log":[0.4221836030483246,0.7261558175086975,0.013918277807533741,0.0,0.04499983415007591,-0.23259326815605164,-0.10549204051494598,0.2682383358478546,0.219349667429924]},{"input":[true,18.0,1.4589801491896106,false,false,-0.00028394224945784677,false,1.4],"torque":-0.2417992316863949,"active":true,"saturated":false,"log":[0.4455682337284088,0.7663773894309998,0.015477767214179039,0.0,0.032972294837236404,-0.2417992353439331,-0.13339607417583466,0.26103511452674866,0.20033679902553558]},{"input":[true,18.0,1.6841705276810832,false,true,-0.0003430223331758737,false,1.4],"torque":-0.2502469962024595,"active":true,"saturated":false,"log":[0.4678625464439392,0.8047235608100891,0.015477767214179039,0.0,0.02250884287059307,-0.2502470016479492,-0.16115188598632812,0.2530149519443512,0.18072494864463806]},{"input":[true,18.0,1.907908298267829,false,true,-0.00040172131625720533,false,1.4],"torque":-0.25791716121375174,"active":true,"saturated":false,"log":[0.48908352851867676,0.8412236571311951,0.015477767214179039,0.0,0.011667251586914062,-0.2579171657562256,-0.1887286752462387,0.24422365427017212,0.16056232154369354]},{"input":[true,18.0,2.1299448864444126,false,true,-0.0004599739836482962,false,1.4],"torque":-0.2648175484871012,"active":true,"saturated":false,"log":[0.5092464089393616,0.875903844833374,0.015477767214179039,0.0,0.00048313208390027285,-0.2648175358772278,-0.21609577536582947,0.2347053736448288,0.13989582657814026]},{"input":[true,18.0,2.3500336077330624,false,true,-0.000517715616156007,false,1.4],"torque":-0.2709701439218616,"active":true,"saturated":false,"log":[0.528364896774292,0.9087876081466675,0.015477767214179039,0.0,-0.011007770895957947,-0.2709701359272003,-0.24322278797626495,0.22450262308120728,0.1187712699174881]},{"input":[true,18.0,2.5679299417521624,false,true,-0.0005748820623511629,false,1.4],"torque":-0.2764045502366593,"active":true,"saturated":false,"log":[0.5464512705802917,0.9398961663246155,0.015477767214179039,0.0,-0.022769777104258537,-0.27640455961227417,-0.2700795829296112,0.2136564552783966,0.09723332524299622]},{"input":[true,18.0,2.783391803880394,false,true,-0.0006314098098413157,false,1.4],"torque":-0.28115452327237117,"active":true,"saturated":false,"log":[0.5635166168212891,0.9692485928535461,0.015477767214179039,0.0,-0.03476748242974281,-0.2811545133590698,-0.29663631319999695,0.2022065371274948,0.07532565295696259]},{"input":[true,18.0,2.9961798142147678,false,true,-0.0006872360558335404,false,1.4],"torque":-0.2847951808151458,"active":true,"saturated":false,"log":[0.5795710682868958,0.9968622326850891,0.015477767214179039,0.0,-0.04993730038404465,-0.28479519486427307,-0.32286348938941956,0.19019128382205963,0.05309094488620758]},{"input":[true,18.0,3.2060575635236503,false,false,-0.0007422987769088523,false,1.4],"torque":-0.28721868149992796,"active":true,"saturated":false,"log":[0.5946237444877625,1.0227528810501099,0.015477767214179039,0.0,-0.067777618765831,-0.28721868991851807,-0.34873196482658386,0.1776479035615921,0.030570965260267258]},{"input":[true,18.0,3.412791875899368,false,false,-0.0007965367979307344,false,1.4],"torque":-0.2888423342677277,"active":true,"saturated":false,"log":[0.6086829900741577,1.046934723854065,0.015477767214179039,0.0,-0.08535073697566986,-0.2888423204421997,-0.37421298027038574,0.1646125763654709,0.007806598674505949]},{"input":[true,18.0,3.616153067818567,false,false,-0.000849889860011222,false,1.4],"torque":-0.2898813112411202,"active":true,"saturated":false,"log":[0.6217565536499023,1.0694212913513184,0.015477767214179039,0.0,-0.10263713449239731,-0.28988131880760193,-0.3992782533168793,0.15112042427062988,-0.015162110328674316]},{"input":[true,18.0,3.815915203322498,false,false,-0.0009022986874590205,false,1.4],"torque":-0.2904519800710179,"active":true,"saturated":false,"log":[0.6338515281677246,1.0902246236801147,0.015477767214179039,0.0,-0.11961758881807327,-0.2904519736766815,-0.42389991879463196,0.13720566034317017,-0.0382959246635437]},{"input":[true,18.0,4.011856345033722,false,false,-0.0009537050536352881,false,1.4],"torque":-0.2906215549768148,"active":true,"saturated":false,"log":[0.6449747681617737,1.1093566417694092,0.015477767214179039,0.0,-0.13627324998378754,-0.2906215488910675,-0.4480506181716919,0.1229015663266182,-0.0615563839673996]},{"input":[true,18.0,4.203758800730365,false,false,-0.001004051845643908,false,1.4],"torque":-0.29039523673733975,"active":true,"saturated":false,"log":[0.6551325917243958,1.1268280744552612,0.015477767214179039,0.0,-0.15258561074733734,-0.29039523005485535,-0.47170355916023254,0.10824067890644073,-0.08490578830242157]},{"input":[true,18.0,4.391409365203971,false,false,-0.0010532831277843902,false,1.4],"torque":-0.28979604788301644,"active":true,"saturated":false,"log":[0.6643311977386475,1.1426496505737305,0.015477767214179039,0.0,-0.16853654384613037,-0.28979605436325073,-0.49483242630958557,0.0932547077536583,-0.10830715298652649]},{"input":[true,18.0,4.574599557132251,false,false,-0.001101344203696894,false,1.4],"torque":-0.2888728609142128,"active":true,"saturated":false,"log":[0.6725765466690063,1.1568316221237183,0.015477767214179039,0.0,-0.18410834670066833,-0.2888728678226471,-0.5174115300178528,0.07797467708587646,-0.13172423839569092]},{"input":[true,18.0,2.282508545879374,false,false,-0.0005,false,1.4],"torque":-0.20850729533964707,"active":true,"saturated":false,"log":[0.3525123596191406,0.6063212752342224,0.016711561009287834,-0.0,-0.07946079969406128,-0.20850729942321777,-0.23489999771118164,0.07715516537427902,-0.1446041464805603]},{"input":[true,18.0,2.27827498934548,false,false,-0.0004988893003505612,false,1.4],"torque":-0.1773316759160233,"active":true,"saturated":false,"log":[0.3507642447948456,0.6033145189285278,0.017939234152436256,-0.0,-0.07967349141836166,-0.17733167111873627,-0.23437818884849548,0.0761294811964035,-0.1574094444513321]},{"input":[true,18.0,2.2655931285828284,false,false,-0.0004955621360170897,false,1.4],"torque":-0.16461896289112538,"active":true,"saturated":false,"log":[0.3476604223251343,0.5979759097099304,0.019156046211719513,-0.0,-0.0797026976943016,-0.16461896896362305,-0.23281508684158325,0.07494497299194336,-0.1700928807258606]},{"input":[true,18.0,2.2445193065446345,false,false,-0.0004900332889206208,false,1.4],"torque":-0.15861319175483551,"active":true,"saturated":false,"log":[0.34325844049453735,0.5904045104980469,0.02035745047032833,-0.0,-0.07953159511089325,-0.1586131900548935,-0.23021763563156128,0.07364566624164581,-0.18260987102985382]},{"input":[true,18.0,2.215147149977898,false,false,-0.00048232732261528193,false,1.4],"torque":-0.15482942409408335,"active":true,"saturated":false,"log":[0.3376173973083496,0.5807019472122192,0.02153911255300045,-0.0,-0.07914648205041885,-0.15482942759990692,-0.2265973687171936,0.07227230817079544,-0.1949184387922287]},{"input":[true,18.0,2.1776071534586268,false,false,-0.00047247847315736883,false,1.4],"torque":-0.1515693576053035,"active":true,"saturated":false,"log":[0.33079782128334045,0.5689722299575806,0.022696904838085175,-0.0,-0.07853672653436661,-0.15156935155391693,-0.22197037935256958,0.07086237519979477,-0.20697921514511108]},{"input":[true,18.0,2.1320660996307157,false,false,-0.00046053049700144256,false,1.4],"torque":-0.14820254609057223,"active":true,"saturated":false,"log":[0.32286158204078674,0.5553219318389893,0.023826919496059418,-0.0,-0.07769466936588287,-0.1482025533914566,-0.21635723114013672,0.06945013254880905,-0.21875543892383575]},{"input":[true,18.0,2.078726318224253,false,false,-0.00044653647659921467,false,1.4],"torque":-0.14450768896590285,"active":true,"saturated":false,"log":[0.31387200951576233,0.5398598313331604,0.024925472214818,-0.0,-0.07661550492048264,-0.14450769126415253,-0.20978283882141113,0.06806664913892746,-0.2302129864692688]},{"input":[true,18.0,2.0178247871452752,false,false,-0.00043055858456490516,false,1.4],"torque":-0.14042009324555185,"active":true,"saturated":false,"log":[0.3038935661315918,0.5226969122886658,0.025989098474383354,-0.0,-0.07529706507921219,-0.1404200941324234,-0.20227642357349396,0.06673982739448547,-0.24132026731967926]},{"input":[true,18.0,1.9496320796306608,false,false,-0.0004126678074548392,false,1.4],"torque":-0.13593494976482648,"active":true,"saturated":false,"log":[0.2929919362068176,0.5039461255073547,0.02701457031071186,-0.0,-0.07373964041471481,-0.1359349489212036,-0.19387133419513702,0.06549446284770966,-0.2520482838153839]},{"input":[true,18.0,1.8744511621457332,false,false,-0.00039294363038847404,false,1.4],"torque":-0.13107026133579977,"active":true,"saturated":false,"log":[0.28123387694358826,0.4837222695350647,0.02799888886511326,-0.0,-0.07194577902555466,-0.13107025623321533,-0.18460491299629211,0.06435225903987885,-0.26237061619758606]},{"input":[true,18.0,1.792616048365272,false,false,-0.00037147368391202205,false,1.4],"torque":-0.1258525904577852,"active":true,"saturated":false,"log":[0.2686869204044342,0.4621415138244629,0.028939293697476387,-0.0,-0.06992001831531525,-0.1258525848388672,-0.17451833188533783,0.06333188712596893,-0.27226337790489197]},{"input":[true,18.0,1.704490315218029,false,false,-0.0003483533546735827,false,1.4],"torque":-0.12031167253169944,"active":true,"saturated":false,"log":[0.25541946291923523,0.43932148814201355,0.029833262786269188,-0.0,-0.06766871362924576,-0.1203116700053215,-0.16365639865398407,0.06244904175400734,-0.2817051410675049]},{"input":[true,18.0,1.6104654875876567,false,false,-0.0003236853616394762,false,1.4],"torque":-0.11447829508303563,"active":true,"saturated":false,"log":[0.24150043725967407,0.4153807461261749,0.030678514391183853,-0.0,-0.0651998370885849,-0.11447829753160477,-0.1520673781633377,0.06171649321913719,-0.2906770408153534]},{"input":[true,18.0,1.510959298846505,false,false,-0.0002975792997345639,false,1.4],"torque":-0.10838348860188476,"active":true,"saturated":false,"log":[0.2269991934299469,0.39043861627578735,0.031473010778427124,-0.0,-0.06252280622720718,-0.10838349163532257,-0.1398027539253235,0.061144158244132996,-0.2991625666618347]},{"input":[true,18.0,1.4064138349503816,false,false,-0.00027015115293406986,false,1.4],"torque":-0.10205819617112157,"active":true,"saturated":false,"log":[0.21198531985282898,0.3646147549152374,0.03221495822072029,-0.0,-0.05964837595820427,-0.10205819457769394,-0.1269170045852661,0.060739148408174515,-0.30714765191078186]},{"input":[true,18.0,1.2972935703396855,false,false,-0.00024152277897013832,false,1.4],"torque":-0.09553312559360247,"active":true,"saturated":false,"log":[0.19652847945690155,0.33802899718284607,0.03290281072258949,-0.0,-0.05658852308988571,-0.09553312510251999,-0.11346740275621414,0.06050586700439453,-0.3146206736564636]},{"input":[true,18.0,1.1840833043730163,false,false,-0.0002118213679424723,false,1.4],"torque":-0.08872773389237082,"active":true,"saturated":false,"log":[0.18069815635681152,0.31080082058906555,0.03353525325655937,-0.0,-0.05397431179881096,-0.08872773498296738,-0.09951367974281311,0.060446083545684814,-0.3215721845626831]},{"input":[true,18.0,1.0672860074612616,false,false,-0.00018117887723833681,false,1.4],"torque":-0.08174662065431045,"active":true,"saturated":false,"log":[0.16456350684165955,0.28304922580718994,0.03411122411489487,-0.0,-0.05116714537143707,-0.08174662292003632,-0.08511783182621002,0.060559019446372986,-0.32799509167671204]},{"input":[true,18.0,0.947420586471363,false,false,-0.00014973144527246395,true,1.4],"torque":-0.07464950300323601,"active":true,"saturated":false,"log":[0.1481931209564209,0.25489217042922974,0.03462990000844002,-0.0,-0.04817303642630577,-0.07464950531721115,-0.07034382969141006,0.06084144115447998,-0.33388444781303406]},{"input":[true,18.0,0.8250195793276136,false,false,-0.00011761878665149471,false,1.4],"torque":-0.0674769731692361,"active":true,"saturated":false,"log":[0.13165485858917236,0.22644636034965515,0.03509069234132767,-0.0,-0.04500764608383179,-0.06747697293758392,-0.05525730550289154,0.0612877681851387,-0.3392375409603119]},{"input":[true,18.0,0.7006267890529156,false,false,-8.498357145012052e-05,false,1.4],"torque":-0.06026182059984554,"active":true,"saturated":false,"log":[0.11501558125019073,0.1978268027305603,0.03549324721097946,-0.0,-0.04168780893087387,-0.06026181951165199,-0.03992528095841408,0.06189017370343208,-0.34405359625816345]},{"input":[true,18.0,0.5747948677614723,false,false,-5.197079135667549e-05,false,1.4],"torque":-0.05303334048642343,"active":true,"saturated":false,"log":[0.09834100306034088,0.16914652287960052,0.03583744168281555,-0.0,-0.03823146969079971,-0.053033340722322464,-0.02441587671637535,0.06263870745897293,-0.3483338952064514]},{"input":[true,18.0,0.44808286133674957,false,false,-1.872711550426666e-05,false,1.4],"torque":-0.04581892318342706,"active":true,"saturated":false,"log":[0.08169543743133545,0.14051614701747894,0.036123376339673996,-0.0,-0.03465765342116356,-0.04581892490386963,-0.008797997608780861,0.06352140754461288,-0.35208168625831604]},{"input":[true,18.0,0.321053725703211,false,false,1.4599761150644409e-05,false,1.4],"torque":-0.03864463068532113,"active":true,"saturated":false,"log":[0.06514163315296173,0.11204361170530319,0.03635137155652046,-0.0,-0.030986392870545387,-0.03864463046193123,0.006858969107270241,0.06452441960573196,-0.35530194640159607]},{"input":[true,18.0,0.19427182572652285,false,false,4.786177400718783e-05,false,1.4],"torque":-0.03153536352103263,"active":true,"saturated":false,"log":[0.048740535974502563,0.0838337242603302,0.03652196377515793,-0.0,-0.02723865769803524,-0.03153536468744278,0.022485462948679924,0.06563213467597961,-0.3580014705657959]},{"input":[true,18.0,0.06830042785412241,false,false,8.091114664128603e-05,false,1.4],"torque":-0.024514894108379752,"active":true,"saturated":false,"log":[0.0325511209666729,0.05598792806267738,0.03663589060306549,-0.0,-0.02343626134097576,-0.02451489493250847,0.0380120575428009,0.06682734936475754,-0.36018866300582886]},{"input":[true,18.0,-0.056300802364165736,false,false,0.00011360104734654355,false,1.4],"torque":-0.017605800773042364,"active":true,"saturated":false,"log":[0.01663019135594368,0.0286039300262928,0.03669409826397896,-0.0,-0.019601767882704735,-0.017605800181627274,0.05336977541446686,0.06809134781360626,-0.3618735373020172]},{"input":[true,18.0,-0.1789782867574491,false,false,0.0001457862414779463,false,1.4],"torque":-0.010829584913180094,"active":true,"saturated":false,"log":[0.0010321956360712647,0.0017753764986991882,0.03669770807027817,-0.0,-0.015758376568555832,-0.010829584673047066,0.06849037855863571,0.06940411031246185,-0.36306747794151306]},{"input":[true,18.0,-0.2991869939700889,false,false,0.0001773237367001388,false,1.4],"torque":-0.004206394780818919,"active":true,"saturated":false,"log":[-0.014190943911671638,-0.024408424273133278,0.036648042500019073,-0.0,-0.011929807253181934,-0.004206394776701927,0.0833066925406456,0.07074441760778427,-0.3637832999229431]},{"input":[true,18.0,-0.017110441347512495,false,false,0.0002080734182735712,false,1.4],"torque":-0.016698166051213433,"active":true,"saturated":false,"log":[0.02660394087433815,0.04575878009200096,0.03674115613102913,-0.0,0.014862586744129658,-0.016698166728019714,0.04853937029838562,0.0720900222659111,-0.364035040140152]},{"input":[true,18.0,-0.07355958571963739,false,false,0.0002378986715560677,false,1.4],"torque":-0.022158478112300817,"active":true,"saturated":false,"log":[0.020244186744093895,0.03482000157237053,0.036812011152505875,-0.0,0.021886510774493217,-0.02215847745537758,0.055497001856565475,0.07341780513525009,-0.3638378381729126]},{"input":[true,18.0,-0.12800830696826654,false,false,0.00026666698895416686,false,1.4],"torque":-0.02424610064086909,"active":true,"saturated":false,"log":[0.014115884900093079,0.02427932247519493,0.03686141595244408,-0.0,0.028703419491648674,-0.024246100336313248,0.06220807507634163,0.07470390945672989,-0.36320793628692627]},{"input":[true,18.0,-0.18021470039098753,false,false,0.0002942505586276729,false,1.4],"torque":-0.024724924920158712,"active":true,"saturated":false,"log":[0.00822514295578003,0.01414724625647068,0.03689020499587059,-0.0,0.03527398779988289,-0.024724924936890602,0.06864277273416519,0.07592392712831497,-0.36216241121292114]},{"input":[true,18.0,-0.22994682349630752,false,false,0.0003205268323319191,false,1.4],"torque":-0.02438819512422967,"active":true,"saturated":false,"log":[0.002576227067038417,0.004431110341101885,0.03689922019839287,-0.0,0.041559699922800064,-0.02438819594681263,0.07477249950170517,0.07705306261777878,-0.3607192039489746]},{"input":[true,18.0,-0.27698372647743114,false,false,0.00034537906987493823,false,1.4],"torque":-0.023660040379640816,"active":true,"saturated":false,"log":[-0.002828403143212199,-0.004864853341132402,0.036889322102069855,-0.0,0.04752305895090103,-0.02366003952920437,0.0805700346827507,0.07806623727083206,-0.3588969111442566]},{"input":[true,18.0,-0.32111643384775074,false,false,0.0003686968577706227,false,1.4],"torque":-0.022789431412273728,"active":true,"saturated":false,"log":[-0.00798804871737957,-0.013739443384110928,0.0368613637983799,-0.0,0.05312776565551758,-0.022789431735873222,0.08600960671901703,0.078938327729702,-0.3567146956920624]},{"input":[true,18.0,-0.3621488728768471,false,false,0.0003903765997836073,false,1.4],"torque":-0.021865333632080226,"active":true,"saturated":false,"log":[-0.012903721071779728,-0.022194400429725647,0.036816198378801346,-0.0,0.05833891034126282,-0.02186533436179161,0.09106705337762833,0.07964427024126053,-0.35419222712516785]},{"input":[true,18.0,-0.3998987447021381,false,false,0.0004103219771864765,false,1.4],"torque":-0.020915161101191706,"active":true,"saturated":false,"log":[-0.017578087747097015,-0.030234310775995255,0.03675467520952225,-0.0,0.06312315911054611,-0.020915161818265915,0.09571991115808487,0.08015923202037811,-0.35134944319725037]},{"input":[true,18.0,-0.4341983342460139,false,false,0.00042844437668447364,false,1.4],"torque":-0.01994256318074189,"active":true,"saturated":false,"log":[-0.02201540768146515,-0.0378665030002594,0.03667762130498886,-0.0,0.06744889169931412,-0.01994256302714348,0.0999475046992302,0.08045876771211624,-0.3482065498828888]},{"input":[true,18.0,-0.4648952553401447,false,false,0.0004446632841065207,false,1.4],"torque":-0.018941959160033696,"active":true,"saturated":false,"log":[-0.026221461594104767,-0.04510091245174408,0.03658584877848625,-0.0,0.0712863877415657,-0.018941959366202354,0.10373105108737946,0.08051898330450058,-0.3447839021682739]},{"input":[true,18.0,-0.4918531277465386,false,false,0.0004589066421134661,false,1.4],"torque":-0.01790416019369258,"active":true,"saturated":false,"log":[-0.030203480273485184,-0.051949985325336456,0.0364801362156868,-0.0,0.0746079683303833,-0.017904160544276237,0.107053741812706,0.08031666278839111,-0.34110182523727417]},{"input":[true,18.0,-0.5149521830674697,false,false,0.000471111170334329,false,1.4],"torque":-0.016818578241947622,"active":true,"saturated":false,"log":[-0.03397005796432495,-0.05842849984765053,0.03636123985052109,-0.0,0.07738811522722244,-0.01681857742369175,0.1099008172750473,0.07982943952083588,-0.33718055486679077]},{"input":[true,18.0,-0.534089796852349,false,false,0.00048122264650824387,false,1.4],"torque":-0.015674120182689013,"active":true,"saturated":false,"log":[-0.03753108158707619,-0.06455346196889877,0.03622988238930702,-0.0,0.07960361242294312,-0.01567411981523037,0.11225961893796921,0.07903590798377991,-0.3330400884151459]},{"input":[true,18.0,-0.549180944537488,false,false,0.0004891961473830436,false,1.4],"torque":-0.014459587509856552,"active":true,"saturated":false,"log":[-0.04089762270450592,-0.07034391164779663,0.03608673810958862,-0.0,0.0812336727976799,-0.014459587633609772,0.11411967873573303,0.07791580259799957,-0.3287001848220825]},{"input":[true,18.0,-0.5601585791931134,false,false,0.0004949962483002228,false,1.4],"torque":-0.013163877366942144,"active":true,"saturated":false,"log":[-0.044081851840019226,-0.07582078874111176,0.035932451486587524,-0.0,0.08226000517606735,-0.01316387765109539,0.11547272652387619,0.07645006477832794,-0.32418012619018555]},{"input":[true,18.0,-0.5669739293993703,false,false,0.0004985971805795595,false,1.4],"torque":-0.011776106114328328,"active":true,"saturated":false,"log":[-0.04709694907069206,-0.08100675046443939,0.03576761484146118,-0.0,0.08266696333885193,-0.011776106432080269,0.11631274968385696,0.0746210366487503,-0.319498747587204]},{"input":[true,18.0,-0.5695967159279187,false,false,0.0004999829460041724,false,1.4],"torque":-0.01028571204974788,"active":true,"saturated":false,"log":[-0.04995697736740112,-0.08592600375413895,0.03559276461601257,-0.0,0.08244159817695618,-0.010285711847245693,0.11663602292537689,0.07241251319646835,-0.3146742284297943]},{"input":[true,18.0,-0.568015286266448,false,false,0.0004991473878973766,false,1.4],"torque":-0.008631540536480672,"active":true,"saturated":false,"log":[-0.05267680436372757,-0.09060410410165787,0.035408396273851395,-0.0,0.08128999918699265,-0.008631540462374687,0.11644110083580017,0.0698099210858345,-0.3097241222858429]},{"input":[true,18.0,-0.5622366663884402,false,false,0.000496094218475557,false,1.4],"torque":-0.006794658982888392,"active":true,"saturated":false,"log":[-0.05527197942137718,-0.09506780654191971,0.03521494194865227,-0.0,0.07926182448863983,-0.0067946589551866055,0.11572886258363724,0.06680034101009369,-0.3046652376651764]},{"input":[true,18.0,-0.5522865295381862,false,false,0.0004908370023555395,false,1.4],"torque":-0.004800887956849919,"active":true,"saturated":false,"log":[-0.057758621871471405,-0.09934482723474503,0.03501278907060623,-0.0,0.07656676322221756,-0.004800887778401375,0.11450245976448059,0.06337268650531769,-0.29951345920562744]},{"input":[true,18.0,-0.5382090821697323,false,false,0.00048339909628973055,false,1.4],"torque":-0.0026554809928666266,"active":true,"saturated":false,"log":[-0.06015332043170929,-0.10346370935440063,0.03480225056409836,-0.0,0.07320544123649597,-0.0026554809883236885,0.11276734620332718,0.05951771140098572,-0.2942838668823242]},{"input":[true,18.0,-0.5200668675465063,false,false,0.00047381354539676926,false,1.4],"torque":-0.00035624464132851886,"active":true,"saturated":false,"log":[-0.06247299909591675,-0.10745355486869812,0.034583594650030136,-0.0,0.06918147206306458,-0.00035624465090222657,0.11053122580051422,0.055228136479854584,-0.28899049758911133]},{"input":[true,18.0,-0.49794048787419715,false,false,0.0004621229363487218,false,1.4],"torque":0.0021014089460873993,"active":true,"saturated":false,"log":[-0.06473483145236969,-0.11134391278028488,0.03435702249407768,-0.0,0.06450150161981583,0.0021014090161770582,0.10780403763055801,0.050498705357313156,-0.28364643454551697]},{"input":[true,18.0,-0.4719282462013859,false,false,0.00044837920816707353,false,1.4],"torque":0.0042970110411635655,"active":true,"saturated":false,"log":[-0.06695609539747238,-0.11516448110342026,0.03412267938256264,-0.0,0.05917512997984886,0.004297011066228151,0.10459790378808975,0.04532623291015625,-0.2782635986804962]},{"input":[true,18.0,-0.44214570967890127,false,false,0.00043264342146811797,false,1.4],"torque":0.006853089129872164,"active":true,"saturated":false,"log":[-0.06915408372879028,-0.11894502490758896,0.03388063982129097,-0.0,0.05321494862437248,0.006853089202195406,0.10092706233263016,0.0397096611559391,-0.2728528678417206]},{"input":[true,18.0,-0.4087251961182428,false,false,0.00041498548718293574,false,1.4],"torque":0.00967358490949816,"active":true,"saturated":false,"log":[-0.0713459774851799,-0.12271507829427719,0.03363092616200447,0.0,0.046636469662189484,0.009673585183918476,0.0968078151345253,0.03365008533000946,-0.26742392778396606]},{"input":[true,18.0,-0.3718151861301943,false,false,0.00039548385595720845,false,1.4],"torque":0.012713479191600516,"active":true,"saturated":false,"log":[-0.07354872673749924,-0.1265038102865219,0.03337350860238075,0.0,0.03945808485150337,0.01271347887814045,0.09225847572088242,0.027150796726346016,-0.26198524236679077]},{"input":[true,18.0,-0.33157966345537343,false,false,0.00037422516961079937,false,1.4],"torque":0.015951959443722255,"active":true,"saturated":false,"log":[-0.07577896863222122,-0.1303398311138153,0.03310827910900116,0.0,0.031700991094112396,0.015951959416270256,0.08729925006628036,0.020217295736074448,-0.2565440833568573]},{"input":[true,18.0,-0.2881973864175003,false,false,0.0003513038762055989,false,1.4],"torque":0.019379427872958967,"active":true,"saturated":false,"log":[-0.07805286347866058,-0.1342509239912033,0.03283509612083435,0.0,0.023389117792248726,0.019379427656531334,0.08195216953754425,0.012857288122177124,-0.25110653042793274]},{"input":[true,18.0,-0.24186109373616554,false,false,0.00032682181043180595,false,1.4],"torque":0.02299123278061923,"active":true,"saturated":false,"log":[-0.08038606494665146,-0.1382640302181244,0.032553743571043015,0.0,0.014549030922353268,0.022991232573986053,0.07624099403619766,0.005080690141767263,-0.24567733705043793]},{"input":[true,18.0,-0.19277664822751983,false,false,0.0003008877411769109,false,1.4],"torque":0.02679351122361467,"active":true,"saturated":false,"log":[-0.08279354125261307,-0.14240488409996033,0.032263968139886856,0.0,0.005202849395573139,0.02679351158440113,0.07019109278917313,-0.003100387053564191,-0.2402600795030594]},{"input":[true,18.0,-0.14116212219723934,false,false,0.00027361688828743294,false,1.4],"torque":0.030799256072223547,"active":true,"saturated":false,"log":[-0.08528953790664673,-0.146697998046875,0.031965453177690506,0.0,-0.004624397959560156,0.03079925663769245,0.06382934749126434,-0.01167166419327259,-0.23485706746578217]},{"input":[true,18.0,-0.08724682858919663,false,false,0.0002451304106703497,false,1.4],"torque":0.035001326536106665,"active":true,"saturated":false,"log":[-0.08788742870092392,-0.1511663794517517,0.03165784850716591,0.0,-0.014888216741383076,0.035001326352357864,0.057184021919965744,-0.02061672881245613,-0.22946937382221222]},{"input":[true,18.0,-0.03127030219423055,false,false,0.00021555486800847306,false,1.4],"torque":0.03939530991491472,"active":true,"saturated":false,"log":[-0.0905996784567833,-0.155831441283226,0.03134074807167053,0.0,-0.02555299736559391,0.03939530998468399,0.05028464272618294,-0.029917078092694283,-0.22409687936306]},{"input":[false,18.0,0.026518764554684215,false,false,0.00018502165848126905,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.08586362647283086,false,false,0.0001536664349892097,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.1465006262908939,false,false,0.00012162850247525857,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.20816036603111016,false,false,8.905019902106331e-05,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.27056890388822397,false,false,5.6076263467527434e-05,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.33344897129775664,false,false,2.2853192369290225e-05,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.39652120478439257,false,false,-1.0471410859959058e-05,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.45950538711764094,false,false,-4.37494917197232e-05,true,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.5221216922605508,false,false,-7.683320239902244e-05,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.5840919285804376,false,false,-0.00010957555863444268,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.6451407747982747,false,false,-0.00014183109273161314,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.7049970031856923,false,false,-0.00017345649984887746,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.763394684575144,false,false,-0.00020431127467184204,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.8200743698296398,false,false,-0.00023425833565018857,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[false,18.0,0.8747842425230097,false,false,-0.00026316463402339594,false,1.4],"torque":-0.0,"active":false,"saturated":false,"log":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},{"input":[true,18.0,0.9272812377095689,false,false,-0.00029090174492958534,false,1.4],"torque":0.0069910883894795505,"active":true,"saturated":false,"log":[0.05383547395467758,0.09259701520204544,0.0,0.0,-0.13172805309295654,0.006991088390350342,-0.06786155700683594,-0.020204681903123856,-0.003987892065197229]},{"input":[true,18.0,0.9773321218127167,false,false,-0.00031734643797131734,false,1.4],"torque":0.016860562538868168,"active":true,"saturated":false,"log":[0.03385905921459198,0.05823758244514465,0.00011850670853164047,0.0,-0.14821185171604156,0.016860563308000565,-0.07403057813644409,-0.0440574586391449,-0.007152613252401352]},{"input":[true,18.0,1.0247145288347417,false,false,-0.00034238122470342736,false,1.4],"torque":0.028958852103363842,"active":true,"saturated":false,"log":[0.014797583222389221,0.025451842695474625,0.0001702982553979382,0.0,-0.163895845413208,0.028958851471543312,-0.07987069338560104,-0.06677139550447464,-0.00953089538961649]},{"input":[true,18.0,1.069217948284169,false,false,-0.0003658948806105272,false,1.4],"torque":0.04185374516123454,"active":true,"saturated":false,"log":[-0.0034068019594997168,-0.0058596995659172535,0.00015837444516364485,0.0,-0.17876973748207092,0.04185374453663826,-0.0853559598326683,-0.08837176859378815,-0.011156166903674603]},{"input":[true,18.0,1.1106446604314733,false,false,-0.00038778293925512483,false,1.4],"torque":0.05486853120847492,"active":true,"saturated":false,"log":[-0.02080555446445942,-0.035785552114248276,8.555500244256109e-05,0.0,-0.19282257556915283,0.05486853048205376,-0.09046199917793274,-0.10887974500656128,-0.012058895081281662]},{"input":[true,18.0,1.1488106147380175,false,false,-0.0004079481564009665,false,1.4],"torque":0.06768868708130421,"active":true,"saturated":false,"log":[-0.03744415193796158,-0.0644039437174797,-4.549953155219555e-05,0.0,-0.20604272186756134,0.06768868863582611,-0.0951661467552185,-0.12831290066242218,-0.012266903184354305]},{"input":[true,18.0,1.1835462475555256,false,false,-0.00042630094204959286,false,1.4],"torque":0.08017118386984859,"active":true,"saturated":false,"log":[-0.05336248129606247,-0.09178347140550613,-0.00023226821213029325,0.0,-0.2184179723262787,0.08017118275165558,-0.09944748133420944,-0.1466856449842453,-0.011805674061179161]},{"input":[true,18.0,1.214697235463224,false,false,-0.00044275975847065947,false,1.4],"torque":0.09225202078752252,"active":true,"saturated":false,"log":[-0.06859521567821503,-0.11798377335071564,-0.00047235147212632,0.0,-0.22993561625480652,0.09225202351808548,-0.1032869964838028,-0.16400966048240662,-0.01069863885641098]},{"input":[true,18.0,1.2421251808957177,false,false,-0.00045725148245765016,false,1.4],"torque":0.10390152123810971,"active":true,"saturated":false,"log":[-0.08317218720912933,-0.14305616915225983,-0.0007634541252627969,0.0,-0.2405826449394226,0.10390152037143707,-0.10666762292385101,-0.18029429018497467,-0.008967451751232147]},{"input":[true,18.0,1.2657082270155007,false,false,-0.00046971173019955657,false,1.4],"torque":0.1151029775688857,"active":true,"saturated":false,"log":[-0.09711871296167374,-0.16704419255256653,-0.0011033696355298162,0.0,-0.25034597516059875,0.11510297656059265,-0.1095743477344513,-0.1955469250679016,-0.006632243748754263]},{"input":[true,18.0,1.2853415990983401,false,false,-0.000480085143325183,false,1.4],"torque":0.12584283056446074,"active":true,"saturated":false,"log":[-0.1104559600353241,-0.1899842470884323,-0.0014899654779583216,0.0,-0.2592128813266754,0.12584282457828522,-0.11199425905942917,-0.20977339148521423,-0.0037118655163794756]},{"input":[true,18.0,1.3009380700262776,false,false,-0.0004883256348492412,false,1.4],"torque":0.13610657334396875,"active":true,"saturated":false,"log":[-0.12320122122764587,-0.21190610527992249,-0.0019211698090657592,0.0,-0.26717129349708557,0.13610658049583435,-0.11391660571098328,-0.22297824919223785,-0.00022411915415432304]},{"input":[true,18.0,1.3124283478201446,false,false,-0.0004943965939275431,false,1.4],"torque":0.1458785082022153,"active":true,"saturated":false,"log":[-0.13536830246448517,-0.23283347487449646,-0.0023949588648974895,0.0,-0.2742164433002472,0.14587850868701935,-0.11533283442258835,-0.23516516387462616,0.0038140302058309317]},{"input":[true,18.0,1.319761383489871,false,false,-0.0004982710485116088,false,1.4],"torque":0.15513974238952585,"active":true,"saturated":false,"log":[-0.14696772396564484,-0.25278449058532715,-0.0029093457851558924,0.0,-0.2803346812725067,0.1551397442817688,-0.11623667180538177,-0.2463371753692627,0.008386251516640186]},{"input":[true,18.0,1.322904597834865,false,false,-0.0004999317851800449,false,1.4],"torque":0.16387017293032918,"active":true,"saturated":false,"log":[-0.15800708532333374,-0.271772176027298,-0.003462370717898011,0.0,-0.28551816940307617,0.163870170712471,-0.11662408709526062,-0.25649699568748474,0.013476659543812275]},{"input":[true,18.0,1.3218440261868434,false,false,-0.0004993714256143098,false,1.4],"torque":0.17204945144075204,"active":true,"saturated":false,"log":[-0.16849134862422943,-0.2898051142692566,-0.004052090458571911,0.0,-0.2897617816925049,0.17204944789409637,-0.11649336665868759,-0.2656472623348236,0.019069641828536987]},{"input":[true,18.0,1.3165843804520363,false,false,-0.0004965924593790963,false,1.4],"torque":0.17965780893973513,"active":true,"saturated":false,"log":[-0.17842306196689606,-0.3068876564502716,-0.004676571115851402,0.0,-0.29306286573410034,0.17965780198574066,-0.11584508419036865,-0.2737908661365509,0.025149695575237274]},{"input":[true,18.0,1.3071490281771392,false,false,-0.0004916072328616975,false,1.4],"torque":0.1866767494793033,"active":true,"saturated":false,"log":[-0.18780264258384705,-0.3230205476284027,-0.005333880428224802,0.0,-0.2954215407371521,0.1866767555475235,-0.11468213051557541,-0.2809309959411621,0.03170127794146538]},{"input":[true,18.0,1.2935798887320078,false,false,-0.0004844378944194944,false,1.4],"torque":0.19308956107798894,"active":true,"saturated":false,"log":[-0.1966286450624466,-0.33820128440856934,-0.0060220807790756226,0.0,-0.2968409061431885,0.193089559674263,-0.11300966888666153,-0.2870716154575348,0.03870866447687149]},{"input":[true,18.0,1.2759372470703425,false,false,-0.0004751162959792648,false,1.4],"torque":0.19888163727576114,"active":true,"saturated":false,"log":[-0.20489798486232758,-0.3524245321750641,-0.006739223375916481,0.0,-0.2973268926143646,0.19888164103031158,-0.11083512753248215,-0.29221734404563904,0.046155836433172226]}]}],"corrections":{"pull_request_heads":{"180":"e4fdb7cbbeb616ea9a7c293df0d56911b80a2ce7","186":"ef4e81b1a8539d04c610a39d37d8d3671c9e061d"},"behavior":"Magnitude-based unwind; sent-torque angle taper sign.","original_output_sha256":"a5d7ec22d0b2bc714aa376a6cc90d2228700d8943b89d614303b9d49ffd5a4d5"}} diff --git a/openpilot/starpilot/lateral/tests/fixtures/gv70_385428f6.json b/openpilot/starpilot/lateral/tests/fixtures/gv70_385428f6.json new file mode 100644 index 0000000000..563b6767b1 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/fixtures/gv70_385428f6.json @@ -0,0 +1,3198 @@ +{ + "source_commit": "385428f6ea1c183de4ddcd8604f294abec02a9e8", + "scope": "default pure formulas; not physical or unrestricted original controller replay", + "vectors": [ + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 0, + 0, + 0 + ], + "expected": 0.39492426714720824 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 0, + 0 + ], + "expected": 0.016880903274337812 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0, + 0 + ], + "expected": 0.999875600422747 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0, + 0, + 0, + 0 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0, + 0, + 0, + 0 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0, + 0, + 0, + 0 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0, + 0, + 0 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0, + 0.8, + 0, + 0, + 0, + 0.01 + ], + "expected": 0.0013849595493005762 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 0, + 0.2, + -0.3 + ], + "expected": 0.39228881092436996 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 0, + 0.2 + ], + "expected": 0.013430112432951327 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.2, + 0 + ], + "expected": 0.9999045288176868 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 0 + ], + "expected": 0.9812819139455576 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 0 + ], + "expected": 0.9997240142286171 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 0 + ], + "expected": 0.9991076422047841 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.2, + 0.6, + 0 + ], + "expected": 0.9991387641009151 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.2, + 0.8, + 0.2, + -0.3, + 0, + 0.01 + ], + "expected": 0.20103807013837893 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 0, + -0.2, + 0.3 + ], + "expected": 0.39228881092436996 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 0, + -0.2 + ], + "expected": 0.013430112432951327 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.2, + 0 + ], + "expected": 0.9999045288176868 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 0 + ], + "expected": 0.9812819139455576 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 0 + ], + "expected": 0.9997240142286171 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 0 + ], + "expected": 0.9991076422047841 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.2, + -0.6, + 0 + ], + "expected": 0.9991387641009151 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.2, + -0.8, + -0.2, + 0.3, + 0, + 0.01 + ], + "expected": -0.20103807013837893 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 0, + 0.8, + -0.5 + ], + "expected": 0.3900131503689557 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 0, + 0.8 + ], + "expected": 3.328993667623615e-05 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.8, + 0 + ], + "expected": 0.9999991259600675 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 0 + ], + "expected": 0.9739354485658368 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 0 + ], + "expected": 0.9996220243143089 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 0 + ], + "expected": 0.9982154060645869 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.8, + 2.4, + 0 + ], + "expected": 0.9999988466965384 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.8, + 0.8, + 0.8, + -0.5, + 0, + 0.01 + ], + "expected": 0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 0, + -0.8, + 0.5 + ], + "expected": 0.3900131503689557 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 0, + -0.8 + ], + "expected": 3.328993667623615e-05 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.8, + 0 + ], + "expected": 0.9999991259600675 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 0 + ], + "expected": 0.9739354485658368 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 0 + ], + "expected": 0.9996220243143089 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 0 + ], + "expected": 0.9982154060645869 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.8, + -2.4, + 0 + ], + "expected": 0.9999988466965384 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.8, + -0.8, + -0.8, + 0.5, + 0, + 0.01 + ], + "expected": -0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 0, + 1.5, + -0.2 + ], + "expected": 0.39000017484007354 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 0, + 1.5 + ], + "expected": 5.285349330813823e-09 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 1.5, + 0 + ], + "expected": 0.9999999991976134 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 0 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 0 + ], + "expected": 0.9997092179690864 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 0 + ], + "expected": 0.9990630043624579 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 1.5, + -0.8, + 0 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 1.5, + 0.8, + 1.5, + -0.2, + 0, + 0.01 + ], + "expected": 1.4988055688737532 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 3, + 0, + 0 + ], + "expected": 0.4073140243249034 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 3, + 0 + ], + "expected": 0.09713438645654514 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0, + 3 + ], + "expected": 0.9988413762657682 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0, + 0, + 0, + 3 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0, + 0, + 0, + 3 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0, + 0, + 0, + 3 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0, + 0, + 3 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0, + 0.8, + 0, + 0, + 3, + 0.01 + ], + "expected": 0.0042227778008854825 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 3, + 0.2, + -0.3 + ], + "expected": 0.39804759913200743 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 3, + 0.2 + ], + "expected": 0.07727819477529223 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.2, + 3 + ], + "expected": 0.9991108074464085 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 3 + ], + "expected": 0.9246727942693547 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 3 + ], + "expected": 0.9993630619146152 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 3 + ], + "expected": 0.9966480987601576 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.2, + 0.6, + 3 + ], + "expected": 0.9943421440118672 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.2, + 0.8, + 0.2, + -0.3, + 3, + 0.01 + ], + "expected": 0.2031651029362716 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 3, + -0.2, + 0.3 + ], + "expected": 0.39804759913200743 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 3, + -0.2 + ], + "expected": 0.07727819477529223 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.2, + 3 + ], + "expected": 0.9991108074464085 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 3 + ], + "expected": 0.9246727942693547 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 3 + ], + "expected": 0.9993630619146152 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 3 + ], + "expected": 0.9966480987601576 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.2, + -0.6, + 3 + ], + "expected": 0.9943421440118672 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.2, + -0.8, + -0.2, + 0.3, + 3, + 0.01 + ], + "expected": -0.2031651029362716 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 3, + 0.8, + -0.5 + ], + "expected": 0.39004623750116996 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 3, + 0.8 + ], + "expected": 0.00019155358701326855 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.8, + 3 + ], + "expected": 0.9999918594304504 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 3 + ], + "expected": 0.895108408923452 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 3 + ], + "expected": 0.9991276828933616 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 3 + ], + "expected": 0.9932966544845625 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.8, + 2.4, + 3 + ], + "expected": 0.9999924234174357 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.8, + 0.8, + 0.8, + -0.5, + 3, + 0.01 + ], + "expected": 0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 3, + -0.8, + 0.5 + ], + "expected": 0.39004623750116996 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 3, + -0.8 + ], + "expected": 0.00019155358701326855 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.8, + 3 + ], + "expected": 0.9999918594304504 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 3 + ], + "expected": 0.895108408923452 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 3 + ], + "expected": 0.9991276828933616 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 3 + ], + "expected": 0.9932966544845625 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.8, + -2.4, + 3 + ], + "expected": 0.9999924234174357 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.8, + -0.8, + -0.8, + 0.5, + 3, + 0.01 + ], + "expected": -0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 3, + 1.5, + -0.2 + ], + "expected": 0.39000061474838704 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 3, + 1.5 + ], + "expected": 3.041242261233505e-08 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 1.5, + 3 + ], + "expected": 0.99999999252679 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 3 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 3 + ], + "expected": 0.9993289141352965 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 3 + ], + "expected": 0.996480428751738 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 1.5, + -0.8, + 3 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 1.5, + 0.8, + 1.5, + -0.2, + 3, + 0.01 + ], + "expected": 1.4963581483320947 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 4.4704, + 0, + 0 + ], + "expected": 0.4156049789924249 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 4.4704, + 0 + ], + "expected": 0.193938005436749 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0, + 4.4704 + ], + "expected": 0.9965733603501031 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0, + 0, + 0, + 4.4704 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0, + 0, + 0, + 4.4704 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0, + 0, + 0, + 4.4704 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0, + 0, + 4.4704 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0, + 0.8, + 0, + 0, + 4.4704, + 0.01 + ], + "expected": 0.007277246069214021 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 4.4704, + 0.2, + -0.3 + ], + "expected": 0.40190125431544677 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 4.4704, + 0.2 + ], + "expected": 0.15429323749501997 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.2, + 4.4704 + ], + "expected": 0.9973702053820345 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 4.4704 + ], + "expected": 0.876624471815693 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 4.4704 + ], + "expected": 0.999041314997728 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 4.4704 + ], + "expected": 0.9936767084604614 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.2, + 0.6, + 4.4704 + ], + "expected": 0.9863678491850723 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.2, + 0.8, + 0.2, + -0.3, + 4.4704, + 0.01 + ], + "expected": 0.20545452164137323 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 4.4704, + -0.2, + 0.3 + ], + "expected": 0.40190125431544677 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 4.4704, + -0.2 + ], + "expected": 0.15429323749501997 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.2, + 4.4704 + ], + "expected": 0.9973702053820345 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 4.4704 + ], + "expected": 0.876624471815693 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 4.4704 + ], + "expected": 0.999041314997728 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 4.4704 + ], + "expected": 0.9936767084604614 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.2, + -0.6, + 4.4704 + ], + "expected": 0.9863678491850723 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.2, + -0.8, + -0.2, + 0.3, + 4.4704, + 0.01 + ], + "expected": -0.20545452164137323 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 4.4704, + 0.8, + -0.5 + ], + "expected": 0.39006837868677446 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 4.4704, + 0.8 + ], + "expected": 0.00038245488497760336 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.8, + 4.4704 + ], + "expected": 0.9999759241956062 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 4.4704 + ], + "expected": 0.8282021040653491 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 4.4704 + ], + "expected": 0.9986870351348918 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 4.4704 + ], + "expected": 0.987354278974412 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.8, + 2.4, + 4.4704 + ], + "expected": 0.9999817448311878 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.8, + 0.8, + 0.8, + -0.5, + 4.4704, + 0.01 + ], + "expected": 0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 4.4704, + -0.8, + 0.5 + ], + "expected": 0.39006837868677446 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 4.4704, + -0.8 + ], + "expected": 0.00038245488497760336 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.8, + 4.4704 + ], + "expected": 0.9999759241956062 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 4.4704 + ], + "expected": 0.8282021040653491 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 4.4704 + ], + "expected": 0.9986870351348918 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 4.4704 + ], + "expected": 0.987354278974412 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.8, + -2.4, + 4.4704 + ], + "expected": 0.9999817448311878 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.8, + -0.8, + -0.8, + 0.5, + 4.4704, + 0.01 + ], + "expected": -0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 4.4704, + 1.5, + -0.2 + ], + "expected": 0.39000090912541424 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 4.4704, + 1.5 + ], + "expected": 6.072128313256375e-08 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 1.5, + 4.4704 + ], + "expected": 0.9999999778979171 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 4.4704 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 4.4704 + ], + "expected": 0.9989899175940478 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 4.4704 + ], + "expected": 0.9933604024986178 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 1.5, + -0.8, + 4.4704 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 1.5, + 0.8, + 1.5, + -0.2, + 4.4704, + 0.01 + ], + "expected": 1.4937238822442025 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 20, + 0, + 0 + ], + "expected": 0.4240142300292847 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 20, + 0 + ], + "expected": 0.5373159015394275 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0, + 20 + ], + "expected": 0.8095824357599336 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0, + 0, + 0, + 20 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0, + 0, + 0, + 20 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0, + 0, + 0, + 20 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0, + 0, + 20 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0, + 0.8, + 0, + 0, + 20, + 0.01 + ], + "expected": 0.5894369800517895 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 20, + 0.2, + -0.3 + ], + "expected": 0.4058098939289263 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 20, + 0.2 + ], + "expected": 0.42747789335758674 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.2, + 20 + ], + "expected": 0.8538629278921411 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 20 + ], + "expected": 0.7532906763164031 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 20 + ], + "expected": 0.9511170597790063 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 20 + ], + "expected": 0.8691943127476092 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.2, + 0.6, + 20 + ], + "expected": 0.987509389757693 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.2, + 0.8, + 0.2, + -0.3, + 20, + 0.01 + ], + "expected": 0.641801298642277 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 20, + -0.2, + 0.3 + ], + "expected": 0.4058098939289263 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 20, + -0.2 + ], + "expected": 0.42747789335758674 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.2, + 20 + ], + "expected": 0.8538629278921411 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 20 + ], + "expected": 0.7532906763164031 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 20 + ], + "expected": 0.9511170597790063 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 20 + ], + "expected": 0.8691943127476092 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.2, + -0.6, + 20 + ], + "expected": 0.987509389757693 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.2, + -0.8, + -0.2, + 0.3, + 20, + 0.01 + ], + "expected": -0.641801298642277 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 20, + 0.8, + -0.5 + ], + "expected": 0.39009083578556086 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 20, + 0.8 + ], + "expected": 0.0010596122758771002 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.8, + 20 + ], + "expected": 0.9986621131784511 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 20 + ], + "expected": 0.656462320039787 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 20 + ], + "expected": 0.9330524803650377 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 20 + ], + "expected": 0.7384064582170529 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.8, + 2.4, + 20 + ], + "expected": 0.9999832734979509 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.8, + 0.8, + 0.8, + -0.5, + 20, + 0.01 + ], + "expected": 0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 20, + -0.8, + 0.5 + ], + "expected": 0.39009083578556086 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 20, + -0.8 + ], + "expected": 0.0010596122758771002 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.8, + 20 + ], + "expected": 0.9986621131784511 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 20 + ], + "expected": 0.656462320039787 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 20 + ], + "expected": 0.9330524803650377 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 20 + ], + "expected": 0.7384064582170529 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.8, + -2.4, + 20 + ], + "expected": 0.9999832734979509 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.8, + -0.8, + -0.8, + 0.5, + 20, + 0.01 + ], + "expected": -0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 20, + 1.5, + -0.2 + ], + "expected": 0.3900012077026493 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 20, + 1.5 + ], + "expected": 1.6823165173597263e-07 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 1.5, + 20 + ], + "expected": 0.9999987717924211 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 20 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 20 + ], + "expected": 0.9484963280416182 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 20 + ], + "expected": 0.8626511036511633 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 1.5, + -0.8, + 20 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 1.5, + 0.8, + 1.5, + -0.2, + 20, + 0.01 + ], + "expected": 0.9916516686062474 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 15.6464, + 0, + 0 + ], + "expected": 0.4282261267501504 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 15.6464, + 0 + ], + "expected": 0.5366113000510188 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0, + 15.6464 + ], + "expected": 0.811953051773843 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0, + 0, + 0, + 15.6464 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0, + 0, + 0, + 15.6464 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0, + 0, + 0, + 15.6464 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0, + 0, + 15.6464 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0, + 0.8, + 0, + 0, + 15.6464, + 0.01 + ], + "expected": 0.294919314931818 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 15.6464, + 0.2, + -0.3 + ], + "expected": 0.4077675934076195 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 15.6464, + 0.2 + ], + "expected": 0.42691732636327545 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.2, + 15.6464 + ], + "expected": 0.8556822710013092 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 15.6464 + ], + "expected": 0.7537243674449042 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 15.6464 + ], + "expected": 0.9809541816546917 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 15.6464 + ], + "expected": 0.8825633280303556 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.2, + 0.6, + 15.6464 + ], + "expected": 0.9345257931074067 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.2, + 0.8, + 0.2, + -0.3, + 15.6464, + 0.01 + ], + "expected": 0.4210511738169528 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 15.6464, + -0.2, + 0.3 + ], + "expected": 0.4077675934076195 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 15.6464, + -0.2 + ], + "expected": 0.42691732636327545 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.2, + 15.6464 + ], + "expected": 0.8556822710013092 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 15.6464 + ], + "expected": 0.7537243674449042 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 15.6464 + ], + "expected": 0.9809541816546917 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 15.6464 + ], + "expected": 0.8825633280303556 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.2, + -0.6, + 15.6464 + ], + "expected": 0.9345257931074067 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.2, + -0.8, + -0.2, + 0.3, + 15.6464, + 0.01 + ], + "expected": -0.4210511738169528 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 15.6464, + 0.8, + -0.5 + ], + "expected": 0.3901020837528678 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 15.6464, + 0.8 + ], + "expected": 0.0010582227685415082 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.8, + 15.6464 + ], + "expected": 0.9986787692885983 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 15.6464 + ], + "expected": 0.6570662260530663 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 15.6464 + ], + "expected": 0.9739158427894882 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 15.6464 + ], + "expected": 0.7651426661864817 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.8, + 2.4, + 15.6464 + ], + "expected": 0.9999123217813614 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.8, + 0.8, + 0.8, + -0.5, + 15.6464, + 0.01 + ], + "expected": 0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 15.6464, + -0.8, + 0.5 + ], + "expected": 0.3901020837528678 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 15.6464, + -0.8 + ], + "expected": 0.0010582227685415082 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.8, + 15.6464 + ], + "expected": 0.9986787692885983 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 15.6464 + ], + "expected": 0.6570662260530663 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 15.6464 + ], + "expected": 0.9739158427894882 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 15.6464 + ], + "expected": 0.7651426661864817 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.8, + -2.4, + 15.6464 + ], + "expected": 0.9999123217813614 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.8, + -0.8, + -0.8, + 0.5, + 15.6464, + 0.01 + ], + "expected": -0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 15.6464, + 1.5, + -0.2 + ], + "expected": 0.3900013572494368 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 15.6464, + 1.5 + ], + "expected": 1.6801104357628292e-07 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 1.5, + 15.6464 + ], + "expected": 0.9999987870830723 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 15.6464 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 15.6464 + ], + "expected": 0.9799330896259303 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 15.6464 + ], + "expected": 0.8766888686209153 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 1.5, + -0.8, + 15.6464 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 1.5, + 0.8, + 1.5, + -0.2, + 15.6464, + 0.01 + ], + "expected": 1.2456526368124956 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 26.8224, + 0, + 0 + ], + "expected": 0.41070612843773946 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 26.8224, + 0 + ], + "expected": 0.5373618519449428 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0, + 26.8224 + ], + "expected": 0.8094857756209797 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0, + 0, + 0, + 26.8224 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0, + 0, + 0, + 26.8224 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0, + 0, + 0, + 26.8224 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0, + 0, + 26.8224 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0, + 0.8, + 0, + 0, + 26.8224, + 0.01 + ], + "expected": 0.7616891208447087 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 26.8224, + 0.2, + -0.3 + ], + "expected": 0.3996242570829192 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 26.8224, + 0.2 + ], + "expected": 0.42751445059047777 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.2, + 26.8224 + ], + "expected": 0.8537887455038061 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 26.8224 + ], + "expected": 0.7532498631835662 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 26.8224 + ], + "expected": 0.8886594475797394 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 26.8224 + ], + "expected": 0.8667914732202072 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.2, + 0.6, + 26.8224 + ], + "expected": 0.9995420784374567 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.2, + 0.8, + 0.2, + -0.3, + 26.8224, + 0.01 + ], + "expected": 0.7709096207728932 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 26.8224, + -0.2, + 0.3 + ], + "expected": 0.3996242570829192 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 26.8224, + -0.2 + ], + "expected": 0.42751445059047777 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.2, + 26.8224 + ], + "expected": 0.8537887455038061 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 26.8224 + ], + "expected": 0.7532498631835662 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 26.8224 + ], + "expected": 0.8886594475797394 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 26.8224 + ], + "expected": 0.8667914732202072 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.2, + -0.6, + 26.8224 + ], + "expected": 0.9995420784374567 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.2, + -0.8, + -0.2, + 0.3, + 26.8224, + 0.01 + ], + "expected": -0.7709096207728932 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 26.8224, + 0.8, + -0.5 + ], + "expected": 0.3900552961934151 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 26.8224, + 0.8 + ], + "expected": 0.001059702892241935 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.8, + 26.8224 + ], + "expected": 0.998661434037708 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 26.8224 + ], + "expected": 0.6564054885882742 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 26.8224 + ], + "expected": 0.8475137995868405 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 26.8224 + ], + "expected": 0.7336011067410401 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.8, + 2.4, + 26.8224 + ], + "expected": 0.9999993867852887 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.8, + 0.8, + 0.8, + -0.5, + 26.8224, + 0.01 + ], + "expected": 0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 26.8224, + -0.8, + 0.5 + ], + "expected": 0.3900552961934151 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 26.8224, + -0.8 + ], + "expected": 0.001059702892241935 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.8, + 26.8224 + ], + "expected": 0.998661434037708 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 26.8224 + ], + "expected": 0.6564054885882742 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 26.8224 + ], + "expected": 0.8475137995868405 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 26.8224 + ], + "expected": 0.7336011067410401 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.8, + -2.4, + 26.8224 + ], + "expected": 0.9999993867852887 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.8, + -0.8, + -0.8, + 0.5, + 26.8224, + 0.01 + ], + "expected": -0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 26.8224, + 1.5, + -0.2 + ], + "expected": 0.3900007351877773 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 26.8224, + 1.5 + ], + "expected": 1.6824603864057686e-07 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 1.5, + 26.8224 + ], + "expected": 0.999998771168956 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 26.8224 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 26.8224 + ], + "expected": 0.8826902133629158 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 26.8224 + ], + "expected": 0.8601280684213936 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 1.5, + -0.8, + 26.8224 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 1.5, + 0.8, + 1.5, + -0.2, + 26.8224, + 0.01 + ], + "expected": 0.8430961973438427 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 40, + 0, + 0 + ], + "expected": 0.3920641236281949 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 40, + 0 + ], + "expected": 0.5373624464187049 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0, + 40 + ], + "expected": 0.8094851746679897 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0, + 0, + 0, + 40 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0, + 0, + 0, + 40 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0, + 0, + 0, + 40 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0, + 0, + 40 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0, + 0.8, + 0, + 0, + 40, + 0.01 + ], + "expected": 0.7810123066644695 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 40, + 0.2, + -0.3 + ], + "expected": 0.39095940950566443 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 40, + 0.2 + ], + "expected": 0.42751492354203335 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.2, + 40 + ], + "expected": 0.8537882842989232 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 40 + ], + "expected": 0.753248944210954 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 40 + ], + "expected": 0.8577827783970037 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.2, + 0.6, + -0.3, + 40 + ], + "expected": 0.8666703373365854 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.2, + 0.6, + 40 + ], + "expected": 0.9999993434486603 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.2, + 0.8, + 0.2, + -0.3, + 40, + 0.01 + ], + "expected": 0.7853929478765409 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 40, + -0.2, + 0.3 + ], + "expected": 0.39095940950566443 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 40, + -0.2 + ], + "expected": 0.42751492354203335 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.2, + 40 + ], + "expected": 0.8537882842989232 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 40 + ], + "expected": 0.753248944210954 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 40 + ], + "expected": 0.8577827783970037 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.2, + -0.6, + 0.3, + 40 + ], + "expected": 0.8666703373365854 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.2, + -0.6, + 40 + ], + "expected": 0.9999993434486603 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.2, + -0.8, + -0.2, + 0.3, + 40, + 0.01 + ], + "expected": -0.7853929478765409 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 40, + 0.8, + -0.5 + ], + "expected": 0.3900055122897417 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 40, + 0.8 + ], + "expected": 0.0010597040645722053 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 0.8, + 40 + ], + "expected": 0.9986614298153712 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 40 + ], + "expected": 0.6564042089377347 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 40 + ], + "expected": 0.8052267275116287 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 0.8, + 2.4, + -0.5, + 40 + ], + "expected": 0.733358851488235 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 0.8, + 2.4, + 40 + ], + "expected": 0.999999999120795 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 0.8, + 0.8, + 0.8, + -0.5, + 40, + 0.01 + ], + "expected": 0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 40, + -0.8, + 0.5 + ], + "expected": 0.3900055122897417 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 40, + -0.8 + ], + "expected": 0.0010597040645722053 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + -0.8, + 40 + ], + "expected": 0.9986614298153712 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 40 + ], + "expected": 0.6564042089377347 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 40 + ], + "expected": 0.8052267275116287 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + -0.8, + -2.4, + 0.5, + 40 + ], + "expected": 0.733358851488235 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + -0.8, + -2.4, + 40 + ], + "expected": 0.999999999120795 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + -0.8, + -0.8, + -0.8, + 0.5, + 40, + 0.01 + ], + "expected": -0.8 + }, + { + "function": "get_genesis_gv70_friction_threshold", + "input": [ + 40, + 1.5, + -0.2 + ], + "expected": 0.39000007328837294 + }, + { + "function": "get_genesis_gv70_friction_jerk_deadzone", + "input": [ + 40, + 1.5 + ], + "expected": 1.6824622476814655e-07 + }, + { + "function": "get_genesis_gv70_center_output_scale", + "input": [ + 1.5, + 40 + ], + "expected": 0.9999987711650797 + }, + { + "function": "get_genesis_gv70_unwind_ff_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 40 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_high_speed_error_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 40 + ], + "expected": 0.850158171845656 + }, + { + "function": "get_genesis_gv70_reversal_output_scale", + "input": [ + 1.5, + -0.8, + -0.2, + 40 + ], + "expected": 0.8600008730350676 + }, + { + "function": "get_genesis_gv70_low_speed_center_overshoot_scale", + "input": [ + 1.5, + -0.8, + 40 + ], + "expected": 1.0 + }, + { + "function": "get_genesis_gv70_stabilized_output", + "input": [ + 1.5, + 0.8, + 1.5, + -0.2, + 40, + 0.01 + ], + "expected": 0.8264312957494029 + } + ] +} diff --git a/openpilot/starpilot/lateral/tests/fixtures/oct2_turns.json b/openpilot/starpilot/lateral/tests/fixtures/oct2_turns.json new file mode 100644 index 0000000000..69f6020e12 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/fixtures/oct2_turns.json @@ -0,0 +1 @@ +{"route":"recorded-intersection-turns-v1","commit":"eeb2404fa7e4d8afe5d06c7363909057a13fe52e","fields":["time_from_bookmark","speed","model_action_curvature","observed_desired_curvature","lane_change_state"],"turns":[[[-12.99123771,0.7308415770530701,0.00809478759765625,0.00809478759765625,"preLaneChange"],[-12.984598699,0.7358285784721375,0.00809478759765625,0.00809478759765625,"preLaneChange"],[-12.977958231,0.7417363524436951,0.00809478759765625,0.00809478759765625,"preLaneChange"],[-12.965756252,0.7466133236885071,0.00809478759765625,0.00809478759765625,"preLaneChange"],[-12.959265262,0.7505409121513367,0.00809478759765625,0.00809478759765625,"preLaneChange"],[-12.942491929,0.7554920315742493,0.00860595703125,0.00860595703125,"preLaneChange"],[-12.935763491,0.7595148086547852,0.00860595703125,0.00860595703125,"preLaneChange"],[-12.92360047,0.7641995549201965,0.00860595703125,0.00860595703125,"preLaneChange"],[-12.9147087,0.7691305875778198,0.00860595703125,0.00860595703125,"preLaneChange"],[-12.904460627,0.7739176750183105,0.00860595703125,0.00860595703125,"preLaneChange"],[-12.891382293,0.7781983017921448,0.0094451904296875,0.0094451904296875,"preLaneChange"],[-12.884451147,0.7823948860168457,0.0094451904296875,0.0094451904296875,"preLaneChange"],[-12.874980054,0.7869008183479309,0.0094451904296875,0.0094451904296875,"preLaneChange"],[-12.865756252,0.7913229465484619,0.0094451904296875,0.0094451904296875,"preLaneChange"],[-12.854405002,0.7952952980995178,0.0094451904296875,0.0094451904296875,"preLaneChange"],[-12.842036512,0.7992365956306458,0.00949859619140625,0.00949859619140625,"preLaneChange"],[-12.835234689,0.8027806282043457,0.00949859619140625,0.00949859619140625,"preLaneChange"],[-12.821797085,0.8067225217819214,0.00949859619140625,0.00949859619140625,"preLaneChange"],[-12.81273422,0.8099091649055481,0.00949859619140625,0.00949859619140625,"preLaneChange"],[-12.792848491,0.8139174580574036,0.009796142578125,0.009796142578125,"preLaneChange"],[-12.786046408,0.8171846270561218,0.009796142578125,0.009796142578125,"preLaneChange"],[-12.776409429,0.8232579827308655,0.009796142578125,0.009796142578125,"preLaneChange"],[-12.773276877,0.8232579827308655,0.009796142578125,0.009796142578125,"preLaneChange"],[-12.763804481,0.8260731101036072,0.009796142578125,0.009796142578125,"preLaneChange"],[-12.746432554,0.8282773494720459,0.00983428955078125,0.009796142578125,"preLaneChange"],[-12.737871825,0.8318190574645996,0.00983428955078125,0.00983428955078125,"preLaneChange"],[-12.731269221,0.8347200751304626,0.00983428955078125,0.00983428955078125,"preLaneChange"],[-12.712743804,0.8389219641685486,0.00983428955078125,0.00983428955078125,"preLaneChange"],[-12.710477085,0.8389219641685486,0.00983428955078125,0.00983428955078125,"preLaneChange"],[-12.69620995,0.8428192138671875,0.01087188720703125,0.00983428955078125,"laneChangeStarting"],[-12.688145731,0.8475643396377563,0.01087188720703125,0.01087188720703125,"laneChangeStarting"],[-12.680970002,0.8515781760215759,0.01087188720703125,0.01087188720703125,"laneChangeStarting"],[-12.673632137,0.8564202785491943,0.01087188720703125,0.01087188720703125,"laneChangeStarting"],[-12.664479637,0.8612683415412903,0.01087188720703125,0.01087188720703125,"laneChangeStarting"],[-12.653007919,0.8664907813072205,0.01087188720703125,0.01087188720703125,"laneChangeStarting"],[-12.641140783,0.8724278211593628,0.0109100341796875,0.0109100341796875,"laneChangeStarting"],[-12.634224846,0.8782568573951721,0.0109100341796875,0.0109100341796875,"laneChangeStarting"],[-12.625134481,0.883970320224762,0.0109100341796875,0.0109100341796875,"laneChangeStarting"],[-12.605067815,0.8914521932601929,0.0109100341796875,0.0109100341796875,"laneChangeStarting"],[-12.598158856,0.8979078531265259,0.01207733154296875,0.0109100341796875,"laneChangeStarting"],[-12.585586356,0.9056581258773804,0.01207733154296875,0.01207733154296875,"laneChangeStarting"],[-12.578894013,0.9123202562332153,0.01207733154296875,0.01207733154296875,"laneChangeStarting"],[-12.566856148,0.9202214479446411,0.01207733154296875,0.01207733154296875,"laneChangeStarting"],[-12.560360419,0.9269850850105286,0.01207733154296875,0.01207733154296875,"laneChangeStarting"],[-12.542715002,0.9349445104598999,0.01053619384765625,0.01053619384765625,"laneChangeStarting"],[-12.535990055,0.9417290091514587,0.01053619384765625,0.01053619384765625,"laneChangeStarting"],[-12.529426148,0.9481664896011353,0.01053619384765625,0.01053619384765625,"laneChangeStarting"],[-12.517828648,0.9565431475639343,0.01053619384765625,0.01053619384765625,"laneChangeStarting"],[-12.500192503,0.9651918411254883,0.01053619384765625,0.01053619384765625,"laneChangeStarting"],[-12.488959013,0.9736924767494202,0.00975799560546875,0.00975799560546875,"laneChangeStarting"],[-12.482462763,0.9816576242446899,0.00975799560546875,0.00975799560546875,"laneChangeStarting"],[-12.473326201,0.9906224012374878,0.00975799560546875,0.00975799560546875,"laneChangeStarting"],[-12.463789274,0.998985767364502,0.00975799560546875,0.00975799560546875,"laneChangeStarting"],[-12.451835888,1.0086643695831299,0.00975799560546875,0.00975799560546875,"laneChangeStarting"],[-12.439892659,1.0176538228988647,0.01212310791015625,0.01212310791015625,"laneChangeStarting"],[-12.433446461,1.0271188020706177,0.01212310791015625,0.01212310791015625,"laneChangeStarting"],[-12.424288388,1.036629557609558,0.01212310791015625,0.01212310791015625,"laneChangeStarting"],[-12.415179013,1.0457874536514282,0.01212310791015625,0.01212310791015625,"laneChangeStarting"],[-12.391300368,1.0561151504516602,0.012914329767227173,0.012914329767227173,"laneChangeStarting"],[-12.388979378,1.0561151504516602,0.012914329767227173,0.012914329767227173,"laneChangeStarting"],[-12.382053597,1.0671311616897583,0.012914329767227173,0.012914329767227173,"laneChangeStarting"],[-12.371565368,1.0846867561340332,0.012914329767227173,0.012914329767227173,"laneChangeStarting"],[-12.368987764,1.0846867561340332,0.012914329767227173,0.012914329767227173,"laneChangeStarting"],[-12.360512711,1.093682050704956,0.012914329767227173,0.012914329767227173,"laneChangeStarting"],[-12.344963701,1.1045535802841187,0.01144362147897482,0.012914329767227173,"laneChangeStarting"],[-12.338417191,1.1137499809265137,0.01144362147897482,0.01144362147897482,"laneChangeStarting"],[-12.326227712,1.1236488819122314,0.01144362147897482,0.01144362147897482,"laneChangeStarting"],[-12.316615993,1.1319248676300049,0.01144362147897482,0.01144362147897482,"laneChangeStarting"],[-12.307366514,1.14058518409729,0.01144362147897482,0.01144362147897482,"laneChangeStarting"],[-12.285610993,1.1507385969161987,0.010031563229858875,0.010031563229858875,"laneChangeStarting"],[-12.283242712,1.1507385969161987,0.010031563229858875,0.010031563229858875,"laneChangeStarting"],[-12.27584667,1.159247875213623,0.010031563229858875,0.010031563229858875,"laneChangeStarting"],[-12.264981722,1.1769211292266846,0.010031563229858875,0.010031563229858875,"laneChangeStarting"],[-12.262847972,1.1769211292266846,0.010031563229858875,0.010031563229858875,"laneChangeStarting"],[-12.251036358,1.185715675354004,0.010031563229858875,0.006930196192115545,"laneChangeStarting"],[-12.23867917,1.195611834526062,0.009667822159826756,0.003856693161651492,"laneChangeStarting"],[-12.232272504,1.203879952430725,0.009667822159826756,0.0008100114646367729,"laneChangeStarting"],[-12.223146358,1.2129055261611938,0.009667822159826756,-0.0021991063840687275,"laneChangeStarting"],[-12.213703493,1.221502423286438,0.009667822159826756,-0.003948941361159086,"laneChangeStarting"],[-12.199803389,1.2323476076126099,0.009667822159826756,-0.004186348523944616,"laneChangeStarting"],[-12.180989118,1.2418675422668457,0.010258007794618607,-0.004621667321771383,"laneChangeStarting"],[-12.178665421,1.2418675422668457,0.010258007794618607,-0.004621667321771383,"laneChangeStarting"],[-12.168498129,1.252037763595581,0.010258007794618607,-0.004880011081695557,"laneChangeStarting"],[-12.160866879,1.2609144449234009,0.010258007794618607,-0.005117399618029594,"laneChangeStarting"],[-12.150684431,1.2701008319854736,0.010258007794618607,-0.005354775115847588,"laneChangeStarting"],[-12.139506567,1.2788203954696655,0.006535832770168781,-0.005592145957052708,"laneChangeStarting"],[-12.133118285,1.2874877452850342,0.006535832770168781,-0.005829507019370794,"laneChangeStarting"],[-12.122448494,1.2964890003204346,0.006535832770168781,-0.006066854111850262,"laneChangeStarting"],[-12.113919067,1.3054245710372925,0.006535832770168781,-0.00632107350975275,"laneChangeStarting"],[-12.101105577,1.3146781921386719,0.006535832770168781,-0.006558395456522703,"laneChangeStarting"],[-12.090593546,1.3234710693359375,0.005556262098252773,-0.00679571321234107,"laneChangeStarting"],[-12.07644115,1.3329708576202393,0.005556262098252773,-0.007270453497767448,"laneChangeStarting"],[-12.066297765,1.3423691987991333,0.005556262098252773,-0.007523100357502699,"laneChangeStarting"],[-12.052802869,1.3509148359298706,0.005556262098252773,-0.007760385051369667,"laneChangeStarting"],[-12.045828911,1.3598053455352783,0.0019121663644909859,-0.007760385051369667,"laneChangeStarting"],[-12.035961307,1.369396686553955,0.0019121663644909859,-0.008234894834458828,"laneChangeStarting"],[-12.027368807,1.3781245946884155,0.0019121663644909859,-0.008234894834458828,"laneChangeStarting"],[-12.016933234,1.3860520124435425,0.0019121663644909859,-0.008486592210829258,"laneChangeStarting"],[-12.008782036,1.394378900527954,0.0019121663644909859,-0.008723831735551357,"laneChangeStarting"],[-11.992982296,1.4015731811523438,0.002192907501012087,-0.00923805870115757,"laneChangeStarting"],[-11.986449015,1.4099961519241333,0.002192907501012087,-0.00923805870115757,"laneChangeStarting"],[-11.973544901,1.4191890954971313,0.002192907501012087,-0.009791582822799683,"laneChangeStarting"],[-11.964290317,1.4283387660980225,0.002192907501012087,-0.010081381537020206,"laneChangeStarting"],[-11.947484849,1.438199520111084,0.0016961977817118168,-0.010358083061873913,"laneChangeStarting"],[-11.940366776,1.4487119913101196,0.0016961977817118168,-0.010634751059114933,"laneChangeStarting"],[-11.934008078,1.4594372510910034,0.0016961977817118168,-0.010911396704614162,"laneChangeStarting"],[-11.924932505,1.4695886373519897,0.0016961977817118168,-0.011227602139115334,"laneChangeStarting"],[-11.914391255,1.4791959524154663,0.0016961977817118168,-0.011515123769640923,"laneChangeStarting"],[-11.902691516,1.4882935285568237,0.0016961977817118168,-0.011791767552495003,"laneChangeStarting"],[-11.887784953,1.5054585933685303,0.0004662119026761502,-0.012384599074721336,"laneChangeStarting"],[-11.885468703,1.5054585933685303,0.0004662119026761502,-0.012384599074721336,"laneChangeStarting"],[-11.867508182,1.5177597999572754,0.0004662119026761502,-0.012710035778582096,"laneChangeStarting"],[-11.859289849,1.5282182693481445,0.0004662119026761502,-0.013026122003793716,"laneChangeStarting"],[-11.846153859,1.5392168760299683,-0.001893542124889791,-0.013302616775035858,"laneChangeStarting"],[-11.839525526,1.5484377145767212,-0.001893542124889791,-0.0136187095195055,"laneChangeStarting"],[-11.831084537,1.5575188398361206,-0.001893542124889791,-0.013934789225459099,"laneChangeStarting"],[-11.821288443,1.566475510597229,-0.001893542124889791,-0.014250856824219227,"laneChangeStarting"],[-11.812611568,1.5753226280212402,-0.001893542124889791,-0.014615396037697792,"laneChangeStarting"],[-11.79399787,1.5855858325958252,-0.0015711212763562799,-0.014931373298168182,"laneChangeStarting"],[-11.786495943,1.5941379070281982,-0.0015711212763562799,-0.015247402712702751,"laneChangeStarting"],[-11.779884276,1.6026173830032349,-0.0015711212763562799,-0.015602965839207172,"laneChangeStarting"],[-11.761963912,1.6148163080215454,-0.0015711212763562799,-0.016281355172395706,"laneChangeStarting"],[-11.755332974,1.625917911529541,-0.0015711212763562799,-0.016281355172395706,"laneChangeStarting"],[-11.736790422,1.6390047073364258,-0.0026539366226643324,-0.016992012038826942,"laneChangeStarting"],[-11.728175995,1.661683440208435,-0.0026539366226643324,-0.017386309802532196,"laneChangeStarting"],[-11.725287818,1.661683440208435,-0.0026539366226643324,-0.017386309802532196,"laneChangeStarting"],[-11.716097037,1.67173433303833,-0.0026539366226643324,-0.01778659038245678,"laneChangeStarting"],[-11.70392136,1.681899070739746,-0.0026539366226643324,-0.01849774271249771,"laneChangeStarting"],[-11.692715266,1.6925488710403442,-0.0037034566048532724,-0.018892530351877213,"laneChangeStarting"],[-11.686310319,1.704402208328247,-0.0037034566048532724,-0.018892530351877213,"laneChangeStarting"],[-11.677898496,1.716227650642395,-0.0037034566048532724,-0.019247012212872505,"laneChangeStarting"],[-11.657563339,1.7283912897109985,-0.0037034566048532724,-0.019997425377368927,"laneChangeStarting"],[-11.655300527,1.7283912897109985,-0.0037034566048532724,-0.019997425377368927,"laneChangeStarting"],[-11.645056933,1.7397207021713257,-0.005446965806186199,-0.020431585609912872,"laneChangeStarting"],[-11.634481465,1.7498897314071655,-0.005446965806186199,-0.020826254040002823,"laneChangeStarting"],[-11.62759334,1.7612507343292236,-0.005446965806186199,-0.021220818161964417,"laneChangeStarting"],[-11.617752819,1.773337721824646,-0.005446965806186199,-0.021654827520251274,"laneChangeStarting"],[-11.609241308,1.786087989807129,-0.005446965806186199,-0.022088753059506416,"laneChangeStarting"],[-11.594157663,1.7990599870681763,-0.006577678490430117,-0.02252262458205223,"laneChangeStarting"],[-11.58756959,1.811841607093811,-0.006577678490430117,-0.022956466302275658,"laneChangeStarting"],[-11.579250215,1.8248087167739868,-0.006577678490430117,-0.023390255868434906,"laneChangeStarting"],[-11.56901808,1.835666537284851,-0.006577678490430117,-0.02386367879807949,"laneChangeStarting"],[-11.552304434,1.8464555740356445,-0.006577678490430117,-0.024771766737103462,"laneChangeStarting"],[-11.539747142,1.8571879863739014,-0.007990160025656223,-0.025284625589847565,"laneChangeStarting"],[-11.533199694,1.8674966096878052,-0.007990160025656223,-0.025284625589847565,"laneChangeStarting"],[-11.524244434,1.8781769275665283,-0.007990160025656223,-0.0257570743560791,"laneChangeStarting"],[-11.514399799,1.8903422355651855,-0.007990160025656223,-0.02623019739985466,"laneChangeStarting"],[-11.503396674,1.9019923210144043,-0.007990160025656223,-0.027256470173597336,"laneChangeStarting"],[-11.491830736,1.9116480350494385,-0.008852285332977772,-0.02780873328447342,"laneChangeStarting"],[-11.485292455,1.9205964803695679,-0.008852285332977772,-0.02780873328447342,"laneChangeStarting"],[-11.476540059,1.9289085865020752,-0.008852285332977772,-0.028320729732513428,"laneChangeStarting"],[-11.467039174,1.9374101161956787,-0.008852285332977772,-0.028833499178290367,"laneChangeStarting"],[-11.446223341,1.9498876333236694,-0.010771812871098518,-0.029973221942782402,"laneChangeStarting"],[-11.437247143,1.9603819847106934,-0.010771812871098518,-0.030569395050406456,"laneChangeStarting"],[-11.430782664,1.9709199666976929,-0.010771812871098518,-0.03116082027554512,"laneChangeStarting"],[-11.420969851,1.9818778038024902,-0.010771812871098518,-0.031752150505781174,"laneChangeStarting"],[-11.41268657,1.9924681186676025,-0.010771812871098518,-0.03234347328543663,"laneChangeStarting"],[-11.396243862,2.0057389736175537,-0.01106958743184805,-0.03289494290947914,"laneChangeStarting"],[-11.38864355,2.0169472694396973,-0.01106958743184805,-0.033525582402944565,"laneChangeStarting"],[-11.382225841,2.028120279312134,-0.01106958743184805,-0.03407718241214752,"laneChangeStarting"],[-11.370392196,2.041147470474243,-0.01106958743184805,-0.03466799855232239,"laneChangeStarting"],[-11.362004956,2.052103042602539,-0.01106958743184805,-0.03521951660513878,"laneChangeStarting"],[-11.340529019,2.0641531944274902,-0.014755887910723686,-0.03624613955616951,"laneChangeStarting"],[-11.33202131,2.0757031440734863,-0.014755887910723686,-0.036322180181741714,"laneChangeStarting"],[-11.324494904,2.0875391960144043,-0.014755887910723686,-0.03687342628836632,"laneChangeStarting"],[-11.313897248,2.1000137329101562,-0.014755887910723686,-0.03742453083395958,"laneChangeStarting"],[-11.307264071,2.111938238143921,-0.014755887910723686,-0.038015130907297134,"laneChangeStarting"],[-11.294032821,2.123345375061035,-0.016860585659742355,-0.038566261529922485,"laneChangeStarting"],[-11.287436988,2.1346490383148193,-0.016860585659742355,-0.03911735489964485,"laneChangeStarting"],[-11.279043863,2.146613597869873,-0.016860585659742355,-0.03970777243375778,"laneChangeStarting"],[-11.269050894,2.15881085395813,-0.016860585659742355,-0.04025861620903015,"laneChangeStarting"],[-11.260617457,2.170462131500244,-0.016860585659742355,-0.04080948606133461,"laneChangeStarting"],[-11.237376832,2.1808488368988037,-0.022167928516864777,-0.041727788746356964,"laneChangeStarting"],[-11.229996155,2.1908252239227295,-0.022167928516864777,-0.04191151633858681,"laneChangeStarting"],[-11.218864905,2.2115478515625,-0.022167928516864777,-0.042802534997463226,"laneChangeStarting"],[-11.216329905,2.2115478515625,-0.022167928516864777,-0.04297382012009621,"laneChangeStarting"],[-11.203829436,2.2219085693359375,-0.022167928516864777,-0.04385610669851303,"laneChangeStarting"],[-11.191631207,2.2319018840789795,-0.020173510536551476,-0.04458848014473915,"laneChangeStarting"],[-11.185151884,2.2415637969970703,-0.020173510536551476,-0.04458681121468544,"laneChangeStarting"],[-11.173450218,2.251307964324951,-0.020173510536551476,-0.04544080048799515,"laneChangeStarting"],[-11.164885478,2.260006904602051,-0.020173510536551476,-0.045688483864068985,"laneChangeStarting"],[-11.153709801,2.2669968605041504,-0.020173510536551476,-0.04653347656130791,"laneChangeStarting"],[-11.131875947,2.277719736099243,-0.026383820921182632,-0.04730304330587387,"laneChangeStarting"],[-11.129517093,2.277719736099243,-0.026383820921182632,-0.04730304330587387,"laneChangeStarting"],[-11.120484593,2.29040265083313,-0.026383820921182632,-0.04785304144024849,"laneChangeStarting"],[-11.112465999,2.303753614425659,-0.026383820921182632,-0.04836339130997658,"laneChangeStarting"],[-11.098489281,2.3161871433258057,-0.026383820921182632,-0.04883439838886261,"laneChangeStarting"],[-11.0915285,2.326615810394287,-0.032802119851112366,-0.0493057444691658,"laneChangeStarting"],[-11.084951104,2.335937976837158,-0.032802119851112366,-0.04977726563811302,"laneChangeStarting"],[-11.076355583,2.344622850418091,-0.032802119851112366,-0.05020942538976669,"laneChangeStarting"],[-11.066525219,2.354623556137085,-0.032802119851112366,-0.05064127966761589,"laneChangeStarting"],[-11.055699333,2.3662326335906982,-0.032802119851112366,-0.05107274651527405,"laneChangeStarting"],[-11.04362876,2.37819504737854,-0.03469758480787277,-0.05154353380203247,"laneChangeStarting"],[-11.028557042,2.3885841369628906,-0.03469758480787277,-0.05230887234210968,"laneChangeStarting"],[-11.018682406,2.3975112438201904,-0.03469758480787277,-0.052919745445251465,"laneChangeStarting"],[-11.006182302,2.4066054821014404,-0.03469758480787277,-0.0533909872174263,"laneChangeStarting"],[-10.999249177,2.413980722427368,-0.03469758480787277,-0.053862567991018295,"laneChangeStarting"],[-10.98629673,2.4254515171051025,-0.040597930550575256,-0.05433318018913269,"laneChangeStarting"],[-10.979726157,2.435056686401367,-0.040597930550575256,-0.05480416491627693,"laneChangeStarting"],[-10.967786417,2.444063901901245,-0.040597930550575256,-0.05523581802845001,"laneChangeStarting"],[-10.959743032,2.4525258541107178,-0.040597930550575256,-0.05570698156952858,"laneChangeStarting"],[-10.945167146,2.4612514972686768,-0.04448869451880455,-0.056138619780540466,"laneChangeStarting"],[-10.938689751,2.4683456420898438,-0.04448869451880455,-0.05661003664135933,"laneChangeStarting"],[-10.92592574,2.4777252674102783,-0.04448869451880455,-0.05708085745573044,"laneChangeStarting"],[-10.91652397,2.4869682788848877,-0.04448869451880455,-0.057512249797582626,"laneChangeStarting"],[-10.903496418,2.496464729309082,-0.04448869451880455,-0.057982951402664185,"laneChangeStarting"],[-10.891968866,2.506196975708008,-0.05189169943332672,-0.058453548699617386,"laneChangeStarting"],[-10.885336157,2.5157687664031982,-0.05189169943332672,-0.05892413482069969,"laneChangeStarting"],[-10.876373397,2.5263237953186035,-0.05189169943332672,-0.059394415467977524,"laneChangeStarting"],[-10.866827251,2.5370287895202637,-0.05189169943332672,-0.05986461415886879,"laneChangeStarting"],[-10.856470585,2.546729326248169,-0.05189169943332672,-0.06033501401543617,"laneChangeStarting"],[-10.843114543,2.5558722019195557,-0.055971380323171616,-0.06084492430090904,"laneChangeStarting"],[-10.836571939,2.5626132488250732,-0.055971380323171616,-0.06131603196263313,"laneChangeStarting"],[-10.822595585,2.5697906017303467,-0.055971380323171616,-0.06174756959080696,"laneChangeStarting"],[-10.814087408,2.5789074897766113,-0.055971380323171616,-0.06221795454621315,"laneChangeStarting"],[-10.799718866,2.5875649452209473,-0.055971380323171616,-0.06287935376167297,"laneChangeStarting"],[-10.788945585,2.596932888031006,-0.05795888602733612,-0.06315863132476807,"laneChangeStarting"],[-10.782503762,2.607337713241577,-0.05795888602733612,-0.06358910351991653,"laneChangeStarting"],[-10.772728033,2.616807222366333,-0.05795888602733612,-0.06405919045209885,"laneChangeStarting"],[-10.764038294,2.6254031658172607,-0.05795888602733612,-0.06449007987976074,"laneChangeStarting"],[-10.751204075,2.6316816806793213,-0.05795888602733612,-0.06512686610221863,"laneChangeStarting"],[-10.739214283,2.6377203464508057,-0.0627722516655922,-0.06535319983959198,"laneChangeStarting"],[-10.732557356,2.644697666168213,-0.0627722516655922,-0.06578446179628372,"laneChangeStarting"],[-10.716524179,2.6548328399658203,-0.0627722516655922,-0.0664074644446373,"laneChangeStarting"],[-10.708264648,2.6641149520874023,-0.0627722516655922,-0.06702712923288345,"laneChangeStarting"],[-10.696676836,2.6725964546203613,-0.06375841051340103,-0.06750921159982681,"laneChangeStarting"],[-10.687187669,2.682223320007324,-0.06375841051340103,-0.06793948262929916,"laneChangeStarting"],[-10.678842305,2.69101881980896,-0.06375841051340103,-0.06836996227502823,"laneChangeStarting"],[-10.668906419,2.697906732559204,-0.06375841051340103,-0.06876163184642792,"laneChangeStarting"],[-10.660742982,2.7034149169921875,-0.06375841051340103,-0.06875986605882645,"laneChangeStarting"],[-10.64489194,2.7095580101013184,-0.06763281673192978,-0.06934953480958939,"laneChangeStarting"],[-10.638002982,2.7144253253936768,-0.06763281673192978,-0.06958335638046265,"laneChangeStarting"],[-10.627534232,2.7249362468719482,-0.06763281673192978,-0.07016852498054504,"laneChangeStarting"],[-10.60879267,2.736143112182617,-0.06763281673192978,-0.07075118273496628,"laneChangeStarting"],[-10.606515951,2.736143112182617,-0.06763281673192978,-0.07123015075922012,"laneChangeStarting"],[-10.595854128,2.745699882507324,-0.067594975233078,-0.07122693210840225,"laneChangeStarting"],[-10.585398295,2.752197027206421,-0.067594975233078,-0.07179862260818481,"laneChangeStarting"],[-10.578743035,2.757375478744507,-0.067594975233078,-0.07208921760320663,"laneChangeStarting"],[-10.56693392,2.76400089263916,-0.067594975233078,-0.07265830039978027,"laneChangeStarting"],[-10.558599702,2.7742538452148438,-0.067594975233078,-0.07322443276643753,"laneChangeStarting"],[-10.544065483,2.7844619750976562,-0.072456493973732,-0.07369402796030045,"laneChangeStarting"],[-10.535651264,2.793095588684082,-0.072456493973732,-0.07369096577167511,"laneChangeStarting"],[-10.528990691,2.8021483421325684,-0.072456493973732,-0.07404204457998276,"laneChangeStarting"],[-10.517995744,2.811206102371216,-0.072456493973732,-0.07459047436714172,"laneChangeStarting"],[-10.500062983,2.8198843002319336,-0.072456493973732,-0.07509846240282059,"laneChangeStarting"],[-10.497555067,2.8198843002319336,-0.07360474020242691,-0.07509846240282059,"laneChangeStarting"],[-10.486312463,2.828206777572632,-0.07360474020242691,-0.07544966787099838,"laneChangeStarting"],[-10.477380796,2.8479549884796143,-0.07360474020242691,-0.07598379999399185,"laneChangeStarting"],[-10.474469442,2.8479549884796143,-0.07360474020242691,-0.0760720893740654,"laneChangeStarting"],[-10.465125536,2.8595728874206543,-0.07360474020242691,-0.076382577419281,"laneChangeStarting"],[-10.451523713,2.870955228805542,-0.07360474020242691,-0.07673245668411255,"laneChangeStarting"],[-10.439741578,2.8805837631225586,-0.06983509659767151,-0.07704360038042068,"laneChangeStarting"],[-10.433189286,2.8893299102783203,-0.06983509659767151,-0.07739438861608505,"laneChangeStarting"],[-10.424063192,2.8980154991149902,-0.06983509659767151,-0.07774516195058823,"laneChangeStarting"],[-10.414940172,2.9058940410614014,-0.06983509659767151,-0.07805686444044113,"laneChangeStarting"],[-10.39013663,2.919833183288574,-0.07826375216245651,-0.07856891304254532,"laneChangeStarting"],[-10.387809182,2.919833183288574,-0.07826375216245651,-0.07860220223665237,"laneChangeStarting"],[-10.380847203,2.9325685501098633,-0.07826375216245651,-0.07859715074300766,"laneChangeStarting"],[-10.372213297,2.943394660949707,-0.07826375216245651,-0.0789075419306755,"laneChangeStarting"],[-10.365274183,2.952432632446289,-0.07826375216245651,-0.07940441370010376,"laneChangeStarting"],[-10.358616318,2.9598140716552734,-0.07826375216245651,-0.07945164293050766,"laneChangeStarting"],[-10.345496787,2.9683213233947754,-0.0852230116724968,-0.07994262129068375,"laneChangeStarting"],[-10.338913193,2.9752440452575684,-0.0852230116724968,-0.08044401556253433,"laneChangeStarting"],[-10.326878766,2.984116554260254,-0.0852230116724968,-0.08095329254865646,"laneChangeStarting"],[-10.319010798,2.9914090633392334,-0.0852230116724968,-0.08146918565034866,"laneChangeStarting"],[-10.308969548,2.9987599849700928,-0.0852230116724968,-0.08198994398117065,"laneChangeStarting"],[-10.288307725,3.0069379806518555,-0.08235399425029755,-0.08235399425029755,"laneChangeStarting"],[-10.277398402,3.0332353115081787,-0.08235399425029755,-0.08235399425029755,"laneChangeStarting"],[-10.274968298,3.0332353115081787,-0.08235399425029755,-0.08235399425029755,"laneChangeStarting"],[-10.266053454,3.0486083030700684,-0.08235399425029755,-0.08235399425029755,"laneChangeStarting"],[-10.254887465,3.063366651535034,-0.08235399425029755,-0.08235399425029755,"laneChangeStarting"],[-10.245073038,3.075240135192871,-0.0764148160815239,-0.08254887163639069,"laneChangeStarting"],[-10.233145173,3.084791421890259,-0.0764148160815239,-0.08270192891359329,"laneChangeStarting"],[-10.225885955,3.090681314468384,-0.0764148160815239,-0.08285656571388245,"laneChangeStarting"],[-10.216664288,3.0962297916412354,-0.0764148160815239,-0.0829327255487442,"laneChangeStarting"],[-10.204454497,3.103773593902588,-0.0764148160815239,-0.08300799876451492,"laneChangeStarting"],[-10.186164861,3.11735200881958,-0.08250661194324493,-0.08311986178159714,"laneChangeStarting"],[-10.183713924,3.11735200881958,-0.08250661194324493,-0.08311986178159714,"laneChangeStarting"],[-10.176003768,3.128955364227295,-0.08250661194324493,-0.08311466872692108,"laneChangeStarting"],[-10.16480507,3.1525280475616455,-0.08250661194324493,-0.08318265527486801,"laneChangeStarting"],[-10.162590487,3.1525280475616455,-0.08250661194324493,-0.08318265527486801,"laneChangeStarting"],[-10.143200174,3.15621018409729,-0.08742741495370865,-0.08362607657909393,"laneChangeStarting"],[-10.136273976,3.15861439704895,-0.08742741495370865,-0.08407747745513916,"laneChangeStarting"],[-10.129797049,3.1648130416870117,-0.08742741495370865,-0.08453423529863358,"laneChangeStarting"],[-10.116582831,3.172684669494629,-0.08742741495370865,-0.08499441295862198,"laneChangeStarting"],[-10.108446633,3.180990695953369,-0.08742741495370865,-0.0854564681649208,"laneChangeStarting"],[-10.097274341,3.185171604156494,-0.08515812456607819,-0.08592026680707932,"laneChangeStarting"],[-10.075551581,3.18857741355896,-0.08515812456607819,-0.08549582213163376,"laneChangeStarting"],[-10.073356425,3.18857741355896,-0.08515812456607819,-0.08515812456607819,"laneChangeStarting"],[-10.06461304,3.190932273864746,-0.08515812456607819,-0.08515812456607819,"laneChangeStarting"],[-10.05834054,3.194628953933716,-0.08515812456607819,-0.08515812456607819,"laneChangeStarting"],[-10.045327519,3.203010320663452,-0.08023058623075485,-0.08515812456607819,"laneChangeStarting"],[-10.038725592,3.210073947906494,-0.08023058623075485,-0.08473934978246689,"laneChangeStarting"],[-10.024649342,3.2162601947784424,-0.08023058623075485,-0.08441059291362762,"laneChangeStarting"],[-10.022255332,3.2162601947784424,-0.08023058623075485,-0.08441059291362762,"laneChangeStarting"],[-10.01235304,3.217653274536133,-0.08023058623075485,-0.08452778309583664,"laneChangeStarting"],[-9.992925801,3.2243776321411133,-0.08960484713315964,-0.08495526760816574,"laneChangeStarting"],[-9.986385072,3.2300145626068115,-0.08960484713315964,-0.08539187163114548,"laneChangeStarting"],[-9.975372311,3.236530303955078,-0.08960484713315964,-0.08583575487136841,"laneChangeStarting"],[-9.965754447,3.2423477172851562,-0.08960484713315964,-0.08628556877374649,"laneChangeStarting"],[-9.951531426,3.24751615524292,-0.08960484713315964,-0.08674000948667526,"laneChangeStarting"],[-9.944548562,3.253221273422241,-0.10071713477373123,-0.08719751983880997,"laneChangeStarting"],[-9.934771166,3.260560989379883,-0.10071713477373123,-0.08768340945243835,"laneChangeStarting"],[-9.925659187,3.267897367477417,-0.10071713477373123,-0.08819489181041718,"laneChangeStarting"],[-9.91653377,3.271817684173584,-0.10071713477373123,-0.0887303352355957,"laneChangeStarting"],[-9.905858093,3.273711681365967,-0.10071713477373123,-0.08928783237934113,"laneChangeStarting"],[-9.892751583,3.2805585861206055,-0.0922250896692276,-0.08983317017555237,"laneChangeStarting"],[-9.885977781,3.2916271686553955,-0.0922250896692276,-0.09034854918718338,"laneChangeStarting"],[-9.872321948,3.3099725246429443,-0.0922250896692276,-0.09083375334739685,"laneChangeStarting"],[-9.863530542,3.3277957439422607,-0.0922250896692276,-0.09129049628973007,"laneChangeStarting"],[-9.844681063,3.33256196975708,-0.08927569538354874,-0.09172328561544418,"laneChangeStarting"],[-9.837561375,3.336158514022827,-0.08927569538354874,-0.09133556485176086,"laneChangeStarting"],[-9.829063407,3.3394651412963867,-0.08927569538354874,-0.09094860404729843,"laneChangeStarting"],[-9.821047834,3.3474555015563965,-0.08927569538354874,-0.09056349843740463,"laneChangeStarting"],[-9.812234605,3.3605546951293945,-0.08927569538354874,-0.0901813879609108,"laneChangeStarting"],[-9.799835386,3.3685319423675537,-0.08927569538354874,-0.08980108052492142,"laneChangeStarting"],[-9.789433876,3.3706045150756836,-0.09476117789745331,-0.09019289165735245,"laneChangeStarting"],[-9.782983042,3.368368625640869,-0.09476117789745331,-0.09059565514326096,"laneChangeStarting"],[-9.766742001,3.3780806064605713,-0.09476117789745331,-0.09100505709648132,"laneChangeStarting"],[-9.758088459,3.3886873722076416,-0.09476117789745331,-0.09141942113637924,"laneChangeStarting"],[-9.745369293,3.3978219032287598,-0.09807765483856201,-0.09183771908283234,"laneChangeStarting"],[-9.738810022,3.405564069747925,-0.09807765483856201,-0.09226704388856888,"laneChangeStarting"],[-9.728319241,3.405961036682129,-0.09807765483856201,-0.09270752966403961,"laneChangeStarting"],[-9.718724189,3.4056308269500732,-0.09807765483856201,-0.09315773099660873,"laneChangeStarting"],[-9.710348564,3.4058260917663574,-0.09807765483856201,-0.09361594915390015,"laneChangeStarting"],[-9.690523095,3.406010627746582,-0.09405352920293808,-0.09400731325149536,"laneChangeStarting"],[-9.688093512,3.406010627746582,-0.09405352920293808,-0.09405352920293808,"laneChangeStarting"],[-9.679773148,3.4061279296875,-0.09405352920293808,-0.09405352920293808,"laneChangeStarting"],[-9.663788929,3.409210681915283,-0.09405352920293808,-0.09405352920293808,"laneChangeStarting"],[-9.653393981,3.4124207496643066,-0.09405352920293808,-0.09405352920293808,"laneChangeStarting"],[-9.641677419,3.418401002883911,-0.09579664468765259,-0.0944269448518753,"laneChangeStarting"],[-9.634877679,3.4265623092651367,-0.09579664468765259,-0.09480160474777222,"laneChangeStarting"],[-9.625647419,3.4348249435424805,-0.09579664468765259,-0.09517642855644226,"laneChangeStarting"],[-9.6164432,3.440877914428711,-0.09579664468765259,-0.09555087983608246,"laneChangeStarting"],[-9.602559659,3.445993661880493,-0.09579664468765259,-0.09579664468765259,"laneChangeStarting"],[-9.591270701,3.45363450050354,-0.10338413715362549,-0.0961860865354538,"laneChangeStarting"],[-9.584675284,3.4598228931427,-0.10338413715362549,-0.09659022837877274,"laneChangeStarting"],[-9.575898097,3.4631378650665283,-0.10338413715362549,-0.09700791537761688,"laneChangeStarting"],[-9.555659295,3.4743731021881104,-0.10338413715362549,-0.09743552654981613,"laneChangeStarting"],[-9.553359868,3.4743731021881104,-0.10338413715362549,-0.09787410497665405,"laneChangeStarting"],[-9.540211639,3.4849913120269775,-0.10662717372179031,-0.09832730144262314,"laneChangeStarting"],[-9.531014399,3.504707098007202,-0.10662717372179031,-0.09879080951213837,"laneChangeStarting"],[-9.528572368,3.504707098007202,-0.10662717372179031,-0.09926784038543701,"laneChangeStarting"],[-9.516416952,3.51419997215271,-0.10662717372179031,-0.09975402057170868,"laneChangeStarting"],[-9.508112108,3.524937868118286,-0.10662717372179031,-0.10024725645780563,"laneChangeStarting"],[-9.4927169,3.5364208221435547,-0.10284121334552765,-0.10071950405836105,"laneChangeStarting"],[-9.483764348,3.5485572814941406,-0.10284121334552765,-0.1011657863855362,"laneChangeStarting"],[-9.477069556,3.558236598968506,-0.10284121334552765,-0.10158789157867432,"laneChangeStarting"],[-9.467142525,3.5648581981658936,-0.10284121334552765,-0.10198763012886047,"laneChangeStarting"],[-9.450368828,3.5686490535736084,-0.10284121334552765,-0.10236665606498718,"laneChangeStarting"],[-9.438601901,3.5724925994873047,-0.09376110136508942,-0.10202853381633759,"laneChangeStarting"],[-9.427566849,3.5822737216949463,-0.09376110136508942,-0.10169225931167603,"laneChangeStarting"],[-9.425150599,3.5822737216949463,-0.09376110136508942,-0.10135598480701447,"laneChangeStarting"],[-9.41469664,3.5840444564819336,-0.09376110136508942,-0.10102003812789917,"laneChangeStarting"],[-9.401984349,3.581991195678711,-0.09376110136508942,-0.10068371146917343,"laneChangeStarting"],[-9.390033047,3.579862117767334,-0.10260916501283646,-0.10102501511573792,"laneChangeStarting"],[-9.383390391,3.5800065994262695,-0.10260916501283646,-0.10136980563402176,"laneChangeStarting"],[-9.374181433,3.57967209815979,-0.10260916501283646,-0.10171716660261154,"laneChangeStarting"],[-9.365083204,3.5762956142425537,-0.10260916501283646,-0.10206672549247742,"laneChangeStarting"],[-9.341910183,3.574692964553833,-0.10744509100914001,-0.10242899507284164,"laneChangeStarting"],[-9.334675912,3.5729026794433594,-0.10744509100914001,-0.10280253738164902,"laneChangeStarting"],[-9.323676954,3.570500135421753,-0.10744509100914001,-0.10318604856729507,"laneChangeStarting"],[-9.32103185,3.570500135421753,-0.10744509100914001,-0.103577621281147,"laneChangeStarting"],[-9.311977892,3.564955234527588,-0.10744509100914001,-0.10397713631391525,"laneChangeStarting"],[-9.292269819,3.5589258670806885,-0.10396260023117065,-0.10396260023117065,"laneChangeStarting"],[-9.2854381,3.5532712936401367,-0.10396260023117065,-0.10396260023117065,"laneChangeStarting"],[-9.278801017,3.550670623779297,-0.10396260023117065,-0.10396260023117065,"laneChangeStarting"],[-9.266608674,3.550560712814331,-0.10396260023117065,-0.10396260023117065,"laneChangeStarting"],[-9.259838153,3.5505003929138184,-0.10396260023117065,-0.10396260023117065,"laneChangeStarting"],[-9.238983518,3.555393695831299,-0.1038769781589508,-0.1038769781589508,"laneChangeStarting"],[-9.231456278,3.5580480098724365,-0.1038769781589508,-0.1038769781589508,"laneChangeStarting"],[-9.221801539,3.5582144260406494,-0.1038769781589508,-0.1038769781589508,"laneChangeStarting"],[-9.213805341,3.55682110786438,-0.1038769781589508,-0.1038769781589508,"laneChangeStarting"],[-9.20745956,3.558525562286377,-0.1038769781589508,-0.1038769781589508,"laneChangeStarting"],[-9.1919243,3.560833692550659,-0.10699769109487534,-0.1042248010635376,"laneChangeStarting"],[-9.185224039,3.565194606781006,-0.10699769109487534,-0.10457805544137955,"laneChangeStarting"],[-9.175857112,3.5676517486572266,-0.10699769109487534,-0.10493592917919159,"laneChangeStarting"],[-9.166558362,3.5743637084960938,-0.10699769109487534,-0.10529644787311554,"laneChangeStarting"],[-9.157485394,3.5823371410369873,-0.10699769109487534,-0.10565829277038574,"laneChangeStarting"],[-9.137358102,3.591798782348633,-0.11224933713674545,-0.10603293776512146,"laneChangeStarting"],[-9.127050186,3.614809989929199,-0.11224933713674545,-0.10641592741012573,"laneChangeStarting"],[-9.124583311,3.614809989929199,-0.11224933713674545,-0.10681045800447464,"laneChangeStarting"],[-9.113988728,3.623522996902466,-0.11224933713674545,-0.1072131097316742,"laneChangeStarting"],[-9.100272426,3.633770227432251,-0.11224933713674545,-0.10762206465005875,"laneChangeStarting"],[-9.089071905,3.6465253829956055,-0.1087002381682396,-0.10799799859523773,"laneChangeStarting"],[-9.082598207,3.6611642837524414,-0.1087002381682396,-0.10835254937410355,"laneChangeStarting"],[-9.073466228,3.673316240310669,-0.1087002381682396,-0.10868725180625916,"laneChangeStarting"],[-9.064215968,3.6819663047790527,-0.1087002381682396,-0.1087002381682396,"laneChangeStarting"],[-9.051610187,3.689986228942871,-0.1087002381682396,-0.1087002381682396,"laneChangeStarting"],[-9.027939354,3.69514799118042,-0.10582920163869858,-0.10838337987661362,"preLaneChange"],[-9.024920968,3.69514799118042,-0.10582920163869858,-0.1080649197101593,"preLaneChange"],[-9.017324875,3.6991825103759766,-0.10582920163869858,-0.10774483531713486,"preLaneChange"],[-9.006673156,3.704467296600342,-0.10582920163869858,-0.10742264986038208,"preLaneChange"],[-8.996456542,3.707156181335449,-0.10829976201057434,-0.10709725320339203,"laneChangeStarting"],[-8.989691802,3.705958366394043,-0.10829976201057434,-0.10742511600255966,"laneChangeStarting"],[-8.982537115,3.7015955448150635,-0.10829976201057434,-0.10775509476661682,"laneChangeStarting"],[-8.973395709,3.6977858543395996,-0.10829976201057434,-0.1080862283706665,"laneChangeStarting"],[-8.964165032,3.697599411010742,-0.10829976201057434,-0.10829976201057434,"laneChangeStarting"],[-8.953239146,3.700073480606079,-0.10829976201057434,-0.10829976201057434,"laneChangeStarting"],[-8.941495605,3.6982369422912598,-0.10694163292646408,-0.10798424482345581,"laneChangeStarting"],[-8.925069459,3.695460319519043,-0.10694163292646408,-0.10766825079917908,"laneChangeStarting"],[-8.915803209,3.696021556854248,-0.10694163292646408,-0.1073523536324501,"laneChangeStarting"],[-8.901560293,3.698575973510742,-0.10694163292646408,-0.10703689604997635,"laneChangeStarting"],[-8.894807585,3.6969399452209473,-0.10431837290525436,-0.10694163292646408,"laneChangeStarting"],[-8.88320222,3.6952176094055176,-0.10431837290525436,-0.10662560164928436,"laneChangeStarting"],[-8.875923939,3.6960954666137695,-0.10431837290525436,-0.10630971193313599,"laneChangeStarting"],[-8.866859043,3.7005345821380615,-0.10431837290525436,-0.10599458962678909,"laneChangeStarting"],[-8.85829446,3.7052385807037354,-0.10431837290525436,-0.10568026453256607,"laneChangeStarting"],[-8.8433242,3.7044942378997803,-0.10706359148025513,-0.1059979647397995,"laneChangeStarting"],[-8.83681821,3.7024805545806885,-0.10706359148025513,-0.10631833225488663,"laneChangeStarting"],[-8.82348644,3.704988956451416,-0.10706359148025513,-0.10663969069719315,"laneChangeStarting"],[-8.814777117,3.712075710296631,-0.10706359148025513,-0.10696040093898773,"laneChangeStarting"],[-8.800280919,3.718837261199951,-0.10706359148025513,-0.10706359148025513,"laneChangeStarting"],[-8.787759565,3.72717022895813,-0.11020588129758835,-0.10738428682088852,"laneChangeStarting"],[-8.785466648,3.72717022895813,-0.11020588129758835,-0.10771125555038452,"laneChangeStarting"],[-8.775156805,3.731661796569824,-0.11020588129758835,-0.10804259777069092,"laneChangeStarting"],[-8.766014669,3.734779119491577,-0.11020588129758835,-0.10837750136852264,"laneChangeStarting"],[-8.752231128,3.7373697757720947,-0.11020588129758835,-0.10871503502130508,"laneChangeStarting"],[-8.738009357,3.7407262325286865,-0.11339031159877777,-0.1090618446469307,"laneChangeStarting"],[-8.735579045,3.7407262325286865,-0.11339031159877777,-0.10941721498966217,"laneChangeStarting"],[-8.717177378,3.743900775909424,-0.11339031159877777,-0.10977926850318909,"laneChangeStarting"],[-8.708988629,3.74770188331604,-0.11339031159877777,-0.11014662683010101,"laneChangeStarting"],[-8.698054097,3.7513203620910645,-0.11256997287273407,-0.11051814258098602,"laneChangeStarting"],[-8.685864827,3.758378267288208,-0.11256997287273407,-0.1108899936079979,"laneChangeStarting"],[-8.683377848,3.758378267288208,-0.11256997287273407,-0.11126258224248886,"laneChangeStarting"],[-8.670254098,3.762965440750122,-0.11256997287273407,-0.11163020879030228,"laneChangeStarting"],[-8.661560035,3.7665257453918457,-0.11256997287273407,-0.11197888106107712,"laneChangeStarting"],[-8.643914827,3.7721471786499023,-0.11745565384626389,-0.11233758181333542,"laneChangeStarting"],[-8.636990348,3.77701997756958,-0.11745565384626389,-0.11270501464605331,"laneChangeStarting"],[-8.628119411,3.785336494445801,-0.11745565384626389,-0.11307915300130844,"laneChangeStarting"],[-8.613730817,3.7919018268585205,-0.11745565384626389,-0.11345899105072021,"laneChangeStarting"],[-8.603175349,3.7968108654022217,-0.11745565384626389,-0.11384360492229462,"laneChangeStarting"],[-8.584570766,3.809384822845459,-0.12061668932437897,-0.114237941801548,"laneChangeStarting"],[-8.58230238,3.809384822845459,-0.12061668932437897,-0.11464297771453857,"laneChangeStarting"],[-8.572515401,3.8143064975738525,-0.12061668932437897,-0.11505618691444397,"laneChangeStarting"],[-8.564027224,3.8206610679626465,-0.12061668932437897,-0.11547578871250153,"laneChangeStarting"],[-8.55207535,3.826444387435913,-0.12061668932437897,-0.11590050160884857,"laneChangeStarting"],[-8.540333839,3.832064390182495,-0.12285378575325012,-0.11633449047803879,"laneChangeStarting"],[-8.533795246,3.83601450920105,-0.12285378575325012,-0.11677654087543488,"laneChangeStarting"],[-8.524645402,3.8387980461120605,-0.12285378575325012,-0.11722540110349655,"laneChangeStarting"],[-8.499960246,3.843160629272461,-0.12285378575325012,-0.11767925322055817,"laneChangeStarting"],[-8.497451652,3.843160629272461,-0.1200360432267189,-0.11813771724700928,"laneChangeStarting"],[-8.48794884,3.848997116088867,-0.1200360432267189,-0.11852142214775085,"laneChangeStarting"],[-8.480403892,3.853550672531128,-0.1200360432267189,-0.11888524889945984,"laneChangeStarting"],[-8.47263733,3.855778217315674,-0.1200360432267189,-0.11923055350780487,"laneChangeStarting"],[-8.465880351,3.8588874340057373,-0.1200360432267189,-0.11955811828374863,"laneChangeStarting"],[-8.45497707,3.8616981506347656,-0.1200360432267189,-0.11986888945102692,"laneChangeStarting"],[-8.445531393,3.865758180618286,-0.11944365501403809,-0.1200360432267189,"laneChangeStarting"],[-8.435825768,3.8694722652435303,-0.11944365501403809,-0.1197478324174881,"laneChangeStarting"],[-8.426674466,3.8724915981292725,-0.11944365501403809,-0.11946006864309311,"laneChangeStarting"],[-8.417732279,3.874497652053833,-0.11944365501403809,-0.11944365501403809,"laneChangeStarting"],[-8.399794206,3.876713991165161,-0.11944365501403809,-0.11944365501403809,"laneChangeStarting"],[-8.397163789,3.876713991165161,-0.1190960481762886,-0.11944365501403809,"laneChangeStarting"],[-8.384467383,3.8791427612304688,-0.1190960481762886,-0.11915688216686249,"laneChangeStarting"],[-8.37691629,3.881782054901123,-0.1190960481762886,-0.1190960481762886,"laneChangeStarting"],[-8.369062852,3.885004997253418,-0.1190960481762886,-0.1190960481762886,"laneChangeStarting"],[-8.362168998,3.8891515731811523,-0.1190960481762886,-0.1190960481762886,"laneChangeStarting"],[-8.350257019,3.89188551902771,-0.1190960481762886,-0.1190960481762886,"laneChangeStarting"],[-8.33888853,3.895200490951538,-0.1212935820221901,-0.11938570439815521,"laneChangeStarting"],[-8.332332645,3.899057626724243,-0.1212935820221901,-0.1196790561079979,"laneChangeStarting"],[-8.32302702,3.903038501739502,-0.1212935820221901,-0.11997516453266144,"laneChangeStarting"],[-8.313388062,3.9071290493011475,-0.1212935820221901,-0.12027312815189362,"laneChangeStarting"],[-8.288982176,3.911694288253784,-0.11739923059940338,-0.11999110132455826,"laneChangeStarting"],[-8.286543583,3.911694288253784,-0.11739923059940338,-0.1197090819478035,"laneChangeStarting"],[-8.278297854,3.91706919670105,-0.11739923059940338,-0.11942782998085022,"laneChangeStarting"],[-8.268407593,3.928068161010742,-0.11739923059940338,-0.11914815753698349,"laneChangeStarting"],[-8.266036552,3.928068161010742,-0.11739923059940338,-0.11886847764253616,"laneChangeStarting"],[-8.257644,3.9325613975524902,-0.11739923059940338,-0.11858943849802017,"laneChangeStarting"],[-8.243737906,3.938950777053833,-0.11621981114149094,-0.118311308324337,"laneChangeStarting"],[-8.235524729,3.945194959640503,-0.11621981114149094,-0.11803405731916428,"laneChangeStarting"],[-8.228756917,3.951663017272949,-0.11621981114149094,-0.11775770783424377,"laneChangeStarting"],[-8.218995563,3.959076166152954,-0.11621981114149094,-0.11748239398002625,"laneChangeStarting"],[-8.210508376,3.9654533863067627,-0.11621981114149094,-0.11720796674489975,"laneChangeStarting"],[-8.187456032,3.9719858169555664,-0.1167014017701149,-0.11693444103002548,"laneChangeStarting"],[-8.177873584,3.9847216606140137,-0.1167014017701149,-0.1167014017701149,"laneChangeStarting"],[-8.17486973,3.9847216606140137,-0.1167014017701149,-0.1167014017701149,"laneChangeStarting"],[-8.164205772,3.9893722534179688,-0.1167014017701149,-0.1167014017701149,"laneChangeStarting"],[-8.153148324,3.9946465492248535,-0.1167014017701149,-0.1167014017701149,"laneChangeStarting"],[-8.140905095,4.000124931335449,-0.11661799252033234,-0.11661799252033234,"laneChangeStarting"],[-8.134165668,4.006170749664307,-0.11661799252033234,-0.11661799252033234,"laneChangeStarting"],[-8.125159002,4.012360095977783,-0.11661799252033234,-0.11661799252033234,"laneChangeStarting"],[-8.115888065,4.019052982330322,-0.11661799252033234,-0.11661799252033234,"laneChangeStarting"],[-8.102401867,4.025826454162598,-0.11661799252033234,-0.11661799252033234,"laneChangeStarting"],[-8.084760148,4.033040523529053,-0.11494050174951553,-0.11635268479585648,"laneChangeStarting"],[-8.078083117,4.039516925811768,-0.11494050174951553,-0.11608822643756866,"laneChangeStarting"],[-8.064327961,4.046055793762207,-0.11494050174951553,-0.1158246248960495,"laneChangeStarting"],[-8.058078222,4.051891803741455,-0.11494050174951553,-0.11556178331375122,"laneChangeStarting"],[-8.045908951,4.060096263885498,-0.11077231913805008,-0.1152999997138977,"laneChangeStarting"],[-8.035980149,4.065573692321777,-0.11077231913805008,-0.1150389239192009,"laneChangeStarting"],[-8.029219576,4.070407390594482,-0.11077231913805008,-0.11477846652269363,"laneChangeStarting"],[-8.019561347,4.076920986175537,-0.11077231913805008,-0.11451883614063263,"laneChangeStarting"],[-8.01121166,4.083860397338867,-0.11077231913805008,-0.11426009237766266,"laneChangeStarting"],[-7.993339577,4.090054512023926,-0.11148804426193237,-0.11400213092565536,"laneChangeStarting"],[-7.990812858,4.090054512023926,-0.11148804426193237,-0.11374416947364807,"laneChangeStarting"],[-7.974249265,4.097064971923828,-0.11148804426193237,-0.11348708719015121,"laneChangeStarting"],[-7.966359629,4.1044487953186035,-0.11148804426193237,-0.11323093622922897,"laneChangeStarting"],[-7.952638223,4.110280513763428,-0.11148804426193237,-0.11297550797462463,"laneChangeStarting"],[-7.945910255,4.116559982299805,-0.11044929921627045,-0.11272085458040237,"laneChangeStarting"],[-7.934228901,4.124767780303955,-0.11044929921627045,-0.1124672144651413,"laneChangeStarting"],[-7.927561297,4.132108211517334,-0.11044929921627045,-0.11221447587013245,"laneChangeStarting"],[-7.917667859,4.137876033782959,-0.11044929921627045,-0.1119624450802803,"laneChangeStarting"],[-7.909685047,4.142942905426025,-0.11044929921627045,-0.11171102523803711,"laneChangeStarting"],[-7.891133537,4.149637222290039,-0.10739271342754364,-0.11146041750907898,"laneChangeStarting"],[-7.88445036,4.155955791473389,-0.10739271342754364,-0.11121057718992233,"laneChangeStarting"],[-7.87471312,4.161169052124023,-0.10739271342754364,-0.11096135526895523,"laneChangeStarting"],[-7.865532183,4.168008327484131,-0.10739271342754364,-0.11071295291185379,"laneChangeStarting"],[-7.852549006,4.173712730407715,-0.10739271342754364,-0.11046522855758667,"laneChangeStarting"],[-7.840711454,4.178743362426758,-0.10182910412549973,-0.1102181002497673,"laneChangeStarting"],[-7.83407135,4.184292793273926,-0.10182910412549973,-0.10997162759304047,"laneChangeStarting"],[-7.825375934,4.190330505371094,-0.10182910412549973,-0.10972586274147034,"laneChangeStarting"],[-7.815302496,4.19606876373291,-0.10182910412549973,-0.10948077589273453,"laneChangeStarting"],[-7.803049007,4.200775146484375,-0.10182910412549973,-0.1092362329363823,"laneChangeStarting"],[-7.79017859,4.206801414489746,-0.10157009214162827,-0.10899239033460617,"laneChangeStarting"],[-7.783585986,4.213299751281738,-0.10157009214162827,-0.10874929279088974,"laneChangeStarting"],[-7.77005208,4.220609664916992,-0.10157009214162827,-0.10850704461336136,"laneChangeStarting"],[-7.761616872,4.227151393890381,-0.10157009214162827,-0.10826554894447327,"laneChangeStarting"],[-7.742218174,4.236373424530029,-0.1000998243689537,-0.10802509635686874,"laneChangeStarting"],[-7.735423591,4.2442827224731445,-0.1000998243689537,-0.10778553783893585,"laneChangeStarting"],[-7.724273435,4.260191917419434,-0.1000998243689537,-0.10754777491092682,"laneChangeStarting"],[-7.721809633,4.260191917419434,-0.1000998243689537,-0.10731000453233719,"laneChangeStarting"],[-7.710303852,4.270014762878418,-0.1000998243689537,-0.1070733293890953,"laneChangeStarting"],[-7.695633279,4.278412342071533,-0.09568330645561218,-0.10683757811784744,"laneChangeStarting"],[-7.688986613,4.285470008850098,-0.09568330645561218,-0.10660260915756226,"laneChangeStarting"],[-7.678817082,4.295436382293701,-0.09568330645561218,-0.10636872053146362,"laneChangeStarting"],[-7.662575259,4.305440425872803,-0.09568330645561218,-0.10613592714071274,"laneChangeStarting"],[-7.651917863,4.315454006195068,-0.09568330645561218,-0.10590420663356781,"laneChangeStarting"],[-7.641050572,4.323564529418945,-0.08954579383134842,-0.10567335784435272,"laneChangeStarting"],[-7.632357916,4.331790447235107,-0.08954579383134842,-0.10544338077306747,"laneChangeStarting"],[-7.623142499,4.340120315551758,-0.08954579383134842,-0.10521429032087326,"laneChangeStarting"],[-7.614004218,4.348922252655029,-0.08954579383134842,-0.10498612374067307,"laneChangeStarting"],[-7.602635,4.357021808624268,-0.08954579383134842,-0.10475880652666092,"laneChangeStarting"],[-7.588949583,4.364468097686768,-0.08860168606042862,-0.10453226417303085,"laneChangeStarting"],[-7.582498958,4.3724446296691895,-0.08860168606042862,-0.10430654883384705,"laneChangeStarting"],[-7.572699792,4.380160808563232,-0.08860168606042862,-0.10408162325620651,"laneChangeStarting"],[-7.552945782,4.389148712158203,-0.08860168606042862,-0.10385762155056,"laneChangeStarting"],[-7.545958907,4.398177623748779,-0.08182203769683838,-0.1036345362663269,"laneChangeStarting"],[-7.534782657,4.406857013702393,-0.08182203769683838,-0.10341233015060425,"laneChangeStarting"],[-7.528071199,4.414073467254639,-0.08182203769683838,-0.10319085419178009,"laneChangeStarting"],[-7.516715626,4.422581195831299,-0.08182203769683838,-0.10297022759914398,"laneChangeStarting"],[-7.508422345,4.431155204772949,-0.08182203769683838,-0.10275045037269592,"laneChangeStarting"],[-7.488759012,4.446547985076904,-0.07839497923851013,-0.10253219306468964,"laneChangeStarting"],[-7.486461043,4.446547985076904,-0.07839497923851013,-0.10231393575668335,"laneChangeStarting"],[-7.478136148,4.454284191131592,-0.07839497923851013,-0.10209643840789795,"laneChangeStarting"],[-7.467494533,4.462149620056152,-0.07839497923851013,-0.10187970101833344,"laneChangeStarting"],[-7.443780419,4.470515251159668,-0.07275398820638657,-0.10166378319263458,"laneChangeStarting"],[-7.441497242,4.470515251159668,-0.07275398820638657,-0.10144785791635513,"laneChangeStarting"],[-7.432989221,4.477455139160156,-0.07275398820638657,-0.1012326031923294,"laneChangeStarting"],[-7.423611305,4.490462303161621,-0.07275398820638657,-0.10101859271526337,"laneChangeStarting"],[-7.421336253,4.490462303161621,-0.07275398820638657,-0.10080458968877792,"laneChangeStarting"],[-7.413067711,4.4962005615234375,-0.07275398820638657,-0.10059112310409546,"laneChangeStarting"],[-7.40017042,4.502641677856445,-0.07275398820638657,-0.10037827491760254,"laneChangeStarting"],[-7.38708792,4.510880947113037,-0.07319758087396622,-0.1001661941409111,"laneChangeStarting"],[-7.380625733,4.517764568328857,-0.07319758087396622,-0.0999547690153122,"laneChangeStarting"],[-7.368119379,4.524525165557861,-0.07319758087396622,-0.09974396973848343,"laneChangeStarting"],[-7.361160525,4.5300445556640625,-0.07319758087396622,-0.09953368455171585,"laneChangeStarting"],[-7.340551098,4.5370707511901855,-0.06872588396072388,-0.0993240475654602,"laneChangeStarting"],[-7.332159536,4.545502662658691,-0.06872588396072388,-0.09911519289016724,"laneChangeStarting"],[-7.323925005,4.553341388702393,-0.06872588396072388,-0.09890705347061157,"laneChangeStarting"],[-7.315002765,4.5598673820495605,-0.06872588396072388,-0.09869951009750366,"laneChangeStarting"],[-7.308574484,4.5678229331970215,-0.06872588396072388,-0.09849268943071365,"laneChangeStarting"],[-7.291447401,4.576725959777832,-0.06523063778877258,-0.09828667342662811,"laneChangeStarting"],[-7.28294761,4.586872577667236,-0.06523063778877258,-0.0980815663933754,"laneChangeStarting"],[-7.274042037,4.595884323120117,-0.06523063778877258,-0.09787725657224655,"laneChangeStarting"],[-7.266618964,4.604204177856445,-0.06523063778877258,-0.0976736918091774,"laneChangeStarting"],[-7.257212558,4.612630844116211,-0.06523063778877258,-0.09747087210416794,"laneChangeStarting"],[-7.234882558,4.622662544250488,-0.06126121059060097,-0.09726893156766891,"laneChangeStarting"],[-7.226497298,4.63114595413208,-0.06126121059060097,-0.09706772118806839,"laneChangeStarting"],[-7.218654121,4.63893985748291,-0.06126121059060097,-0.09686719626188278,"laneChangeStarting"],[-7.210073184,4.648739337921143,-0.06126121059060097,-0.09666751325130463,"laneChangeStarting"],[-7.200199694,4.656991004943848,-0.06126121059060097,-0.09646853804588318,"laneChangeStarting"],[-7.187092299,4.666823387145996,-0.05918142572045326,-0.09627039730548859,"laneChangeStarting"],[-7.180514643,4.675090312957764,-0.05918142572045326,-0.0960729569196701,"laneChangeStarting"],[-7.167902091,4.683034420013428,-0.05918142572045326,-0.09587618708610535,"laneChangeStarting"],[-7.161465945,4.689549446105957,-0.05918142572045326,-0.09567996114492416,"laneChangeStarting"],[-7.14860407,4.696641445159912,-0.05918142572045326,-0.09548433125019073,"laneChangeStarting"],[-7.130960321,4.704281806945801,-0.054883114993572235,-0.09528933465480804,"laneChangeStarting"],[-7.123543915,4.713192462921143,-0.054883114993572235,-0.09509507566690445,"laneChangeStarting"],[-7.110802404,4.723655700683594,-0.054883114993572235,-0.0949016734957695,"laneChangeStarting"],[-7.104490946,4.731386661529541,-0.054883114993572235,-0.09470890462398529,"laneChangeStarting"],[-7.089325061,4.739222526550293,-0.05268320441246033,-0.09451677650213242,"laneChangeStarting"],[-7.081435895,4.747539520263672,-0.05268320441246033,-0.09432531893253326,"laneChangeStarting"],[-7.074781988,4.754415035247803,-0.05268320441246033,-0.09413441270589828,"laneChangeStarting"],[-7.065157666,4.762228012084961,-0.05268320441246033,-0.09394413232803345,"laneChangeStarting"],[-7.053438291,4.769406318664551,-0.05268320441246033,-0.09375442564487457,"laneChangeStarting"],[-7.043638239,4.776002883911133,-0.04747975617647171,-0.09356524050235748,"laneChangeStarting"],[-7.028239385,4.785094261169434,-0.04747975617647171,-0.0933767706155777,"laneChangeStarting"],[-7.020427719,4.792718887329102,-0.04747975617647171,-0.09318890422582626,"laneChangeStarting"],[-7.007566885,4.802002906799316,-0.04747975617647171,-0.09300176799297333,"laneChangeStarting"],[-7.001023604,4.809797763824463,-0.04747975617647171,-0.09281522780656815,"laneChangeStarting"],[-6.987896053,4.8173418045043945,-0.0460662804543972,-0.09262927621603012,"laneChangeStarting"],[-6.981443292,4.823523998260498,-0.0460662804543972,-0.0924438014626503,"laneChangeStarting"],[-6.972121469,4.830722332000732,-0.0460662804543972,-0.09225888550281525,"laneChangeStarting"],[-6.962523136,4.83849573135376,-0.0460662804543972,-0.09207455068826675,"laneChangeStarting"],[-6.946246418,4.8464274406433105,-0.04119721055030823,-0.09189082682132721,"laneChangeStarting"],[-6.939752355,4.852993488311768,-0.04119721055030823,-0.09170760214328766,"laneChangeStarting"],[-6.922884595,4.860188961029053,-0.04119721055030823,-0.09152491390705109,"laneChangeStarting"],[-6.913898137,4.869486331939697,-0.04119721055030823,-0.09134291857481003,"laneChangeStarting"],[-6.903788398,4.878080368041992,-0.04119721055030823,-0.0911615714430809,"laneChangeStarting"],[-6.890923242,4.884877681732178,-0.03854162618517876,-0.09098073095083237,"laneChangeStarting"],[-6.884359023,4.893789768218994,-0.03854162618517876,-0.09080053865909576,"laneChangeStarting"],[-6.8758472,4.902771472930908,-0.03854162618517876,-0.09062101691961288,"laneChangeStarting"],[-6.866123659,4.91294002532959,-0.03854162618517876,-0.0904422327876091,"laneChangeStarting"],[-6.85452017,4.922302722930908,-0.03854162618517876,-0.09026412665843964,"laneChangeStarting"],[-6.841465222,4.929770469665527,-0.03404833748936653,-0.09008655697107315,"laneChangeStarting"],[-6.834852514,4.937370300292969,-0.03404833748936653,-0.08990953862667084,"laneChangeStarting"],[-6.820318608,4.945099353790283,-0.03404833748936653,-0.0897330716252327,"laneChangeStarting"],[-6.812042879,4.952952861785889,-0.03404833748936653,-0.08955716341733932,"laneChangeStarting"],[-6.792732723,4.961302757263184,-0.033610161393880844,-0.08938184380531311,"laneChangeStarting"],[-6.785991994,4.968223571777344,-0.033610161393880844,-0.08920701593160629,"laneChangeStarting"],[-6.779620327,4.974958896636963,-0.033610161393880844,-0.08903266489505768,"laneChangeStarting"],[-6.769721734,4.983423233032227,-0.033610161393880844,-0.08885890245437622,"laneChangeStarting"],[-6.761119859,4.991224765777588,-0.033610161393880844,-0.08868568390607834,"laneChangeStarting"],[-6.745935953,5.000299453735352,-0.031492769718170166,-0.0885130912065506,"laneChangeStarting"],[-6.739465432,5.007904529571533,-0.031492769718170166,-0.08834102004766464,"laneChangeStarting"],[-6.730845328,5.016410827636719,-0.031492769718170166,-0.08816953748464584,"laneChangeStarting"],[-6.713255068,5.026129722595215,-0.031492769718170166,-0.08799871057271957,"laneChangeStarting"],[-6.706596006,5.036583423614502,-0.031492769718170166,-0.08782859891653061,"laneChangeStarting"],[-6.687242568,5.047698497772217,-0.030606277287006378,-0.08765923231840134,"laneChangeStarting"],[-6.680366579,5.057136058807373,-0.030606277287006378,-0.0874904990196228,"laneChangeStarting"],[-6.670251058,5.073306560516357,-0.030606277287006378,-0.08732283860445023,"laneChangeStarting"],[-6.66790835,5.073306560516357,-0.030606277287006378,-0.08715517073869705,"laneChangeStarting"],[-6.656105121,5.082773685455322,-0.030606277287006378,-0.08698813617229462,"laneChangeStarting"],[-6.642131163,5.089162349700928,-0.02834336645901203,-0.08682151883840561,"laneChangeStarting"],[-6.635292517,5.096484184265137,-0.02834336645901203,-0.0866553783416748,"laneChangeStarting"],[-6.626739392,5.104309558868408,-0.02834336645901203,-0.08648975193500519,"laneChangeStarting"],[-6.611354132,5.111853122711182,-0.02834336645901203,-0.08632460981607437,"laneChangeStarting"],[-6.599272101,5.120275497436523,-0.02834336645901203,-0.08616001158952713,"laneChangeStarting"],[-6.58843356,5.129515171051025,-0.02814170904457569,-0.08599600195884705,"laneChangeStarting"],[-6.582027623,5.1372389793396,-0.02814170904457569,-0.08583249151706696,"laneChangeStarting"],[-6.572886529,5.143558025360107,-0.02814170904457569,-0.0856693759560585,"laneChangeStarting"],[-6.562893977,5.15085506439209,-0.02814170904457569,-0.08550672978162766,"laneChangeStarting"],[-6.55026054,5.159826278686523,-0.02814170904457569,-0.08534464240074158,"laneChangeStarting"],[-6.539748665,5.168458461761475,-0.025906147435307503,-0.08518309891223907,"laneChangeStarting"],[-6.531361999,5.1756367683410645,-0.025906147435307503,-0.08502200245857239,"laneChangeStarting"],[-6.522404968,5.183737754821777,-0.025906147435307503,-0.0848614051938057,"laneChangeStarting"],[-6.504705697,5.19194221496582,-0.025906147435307503,-0.08470132201910019,"laneChangeStarting"],[-6.502438874,5.19194221496582,-0.025906147435307503,-0.08454123884439468,"laneChangeStarting"],[-6.489079916,5.200996398925781,-0.02604730799794197,-0.08438170701265335,"laneChangeStarting"],[-6.482244395,5.210078239440918,-0.02604730799794197,-0.08422273397445679,"laneChangeStarting"],[-6.475680646,5.219931125640869,-0.02604730799794197,-0.08406435698270798,"laneChangeStarting"],[-6.465712573,5.229352951049805,-0.02604730799794197,-0.08390655368566513,"laneChangeStarting"],[-6.457149709,5.239119529724121,-0.02604730799794197,-0.08374933898448944,"laneChangeStarting"],[-6.441748667,5.248438358306885,-0.023682676255702972,-0.08359268307685852,"laneChangeStarting"],[-6.433651011,5.25657844543457,-0.023682676255702972,-0.0834365114569664,"laneChangeStarting"],[-6.42704372,5.266270160675049,-0.023682676255702972,-0.08328090608119965,"laneChangeStarting"],[-6.417315959,5.274752616882324,-0.023682676255702972,-0.08312580734491348,"laneChangeStarting"],[-6.391801064,5.283621788024902,-0.02277805656194687,-0.0829712301492691,"laneChangeStarting"],[-6.38363846,5.290961742401123,-0.02277805656194687,-0.08281707763671875,"laneChangeStarting"],[-6.376180023,5.298779487609863,-0.02277805656194687,-0.0826633870601654,"laneChangeStarting"],[-6.368728148,5.307050704956055,-0.02277805656194687,-0.08251016587018967,"laneChangeStarting"],[-6.3561832,5.316501617431641,-0.02277805656194687,-0.0823574960231781,"laneChangeStarting"],[-6.349181742,5.324398040771484,-0.02277805656194687,-0.08220527321100235,"laneChangeStarting"],[-6.335724034,5.334253311157227,-0.021349230781197548,-0.08205361664295197,"laneChangeStarting"],[-6.329154607,5.342519760131836,-0.021349230781197548,-0.08190242946147919,"laneChangeStarting"],[-6.316606222,5.351576805114746,-0.021349230781197548,-0.081751748919487,"laneChangeStarting"],[-6.309982941,5.359099864959717,-0.021349230781197548,-0.08160149306058884,"laneChangeStarting"],[-6.288964191,5.370115280151367,-0.020901145413517952,-0.08145185559988022,"laneChangeStarting"],[-6.278197577,5.389038562774658,-0.020901145413517952,-0.08130326122045517,"laneChangeStarting"],[-6.275292889,5.389038562774658,-0.020901145413517952,-0.08115467429161072,"laneChangeStarting"],[-6.263740598,5.398594856262207,-0.020901145413517952,-0.08100660890340805,"laneChangeStarting"],[-6.2573969,5.408438682556152,-0.020901145413517952,-0.08085907995700836,"laneChangeStarting"],[-6.242591588,5.4162750244140625,-0.019776327535510063,-0.0807119831442833,"laneChangeStarting"],[-6.236029609,5.424520492553711,-0.019776327535510063,-0.08056532591581345,"laneChangeStarting"],[-6.227436328,5.432021141052246,-0.019776327535510063,-0.08041907846927643,"laneChangeStarting"],[-6.217639974,5.439218521118164,-0.019776327535510063,-0.08027321845293045,"laneChangeStarting"],[-6.209431485,5.44766092300415,-0.019776327535510063,-0.08012781292200089,"laneChangeStarting"],[-6.185866173,5.457263946533203,-0.018186867237091064,-0.07998291403055191,"laneChangeStarting"],[-6.183670027,5.457263946533203,-0.018186867237091064,-0.07983801513910294,"laneChangeStarting"],[-6.176165912,5.465292930603027,-0.018186867237091064,-0.07969354093074799,"laneChangeStarting"],[-6.16555034,5.481886863708496,-0.018186867237091064,-0.07954993844032288,"laneChangeStarting"],[-6.16347159,5.481886863708496,-0.018186867237091064,-0.07940634340047836,"laneChangeStarting"],[-6.152128934,5.489668369293213,-0.018186867237091064,-0.07926315069198608,"laneChangeStarting"],[-6.13981383,5.498287677764893,-0.017543001100420952,-0.07912040501832962,"laneChangeStarting"],[-6.133510184,5.505420207977295,-0.017543001100420952,-0.078978031873703,"laneChangeStarting"],[-6.121974403,5.513824939727783,-0.017543001100420952,-0.07883609086275101,"laneChangeStarting"],[-6.113781591,5.521903991699219,-0.017543001100420952,-0.07869455963373184,"laneChangeStarting"],[-6.102065497,5.530815124511719,-0.017543001100420952,-0.07855349034070969,"laneChangeStarting"],[-6.082703362,5.540874004364014,-0.01749216578900814,-0.07841293513774872,"laneChangeStarting"],[-6.073817008,5.552361965179443,-0.01749216578900814,-0.07827295362949371,"laneChangeStarting"],[-6.06655081,5.560997009277344,-0.01749216578900814,-0.07813341170549393,"laneChangeStarting"],[-6.056834196,5.570754051208496,-0.01749216578900814,-0.07799436151981354,"laneChangeStarting"],[-6.043330446,5.579280376434326,-0.016712386161088943,-0.07785572856664658,"laneChangeStarting"],[-6.036891176,5.588552474975586,-0.016712386161088943,-0.07771755754947662,"laneChangeStarting"],[-6.026386436,5.5977606773376465,-0.016712386161088943,-0.07757984101772308,"laneChangeStarting"],[-6.017299926,5.608038425445557,-0.016712386161088943,-0.07744263112545013,"laneChangeStarting"],[-6.009229301,5.615899562835693,-0.016712386161088943,-0.07730580121278763,"laneChangeStarting"],[-5.994512114,5.623788833618164,-0.016447016969323158,-0.07716935873031616,"laneChangeStarting"],[-5.978093885,5.633981227874756,-0.016447016969323158,-0.07703340798616409,"laneChangeStarting"],[-5.970206906,5.6425275802612305,-0.016447016969323158,-0.07689786702394485,"laneChangeStarting"],[-5.957422844,5.652945041656494,-0.016447016969323158,-0.07676282525062561,"laneChangeStarting"],[-5.950033417,5.661690711975098,-0.016447016969323158,-0.0766282007098198,"laneChangeStarting"],[-5.938481386,5.670015335083008,-0.01579383760690689,-0.07649397850036621,"laneChangeStarting"],[-5.931597792,5.676822662353516,-0.01579383760690689,-0.0763600692152977,"laneChangeStarting"],[-5.922575918,5.685259819030762,-0.01579383760690689,-0.07622656226158142,"laneChangeStarting"],[-5.913073991,5.695596694946289,-0.01579383760690689,-0.07609353214502335,"laneChangeStarting"],[-5.899624304,5.7057976722717285,-0.01579383760690689,-0.07596098631620407,"laneChangeStarting"],[-5.888886595,5.7151031494140625,-0.015886684879660606,-0.07582886517047882,"laneChangeStarting"],[-5.870463002,5.725461006164551,-0.015886684879660606,-0.07569722086191177,"laneChangeStarting"],[-5.868007325,5.725461006164551,-0.015886684879660606,-0.07556558400392532,"laneChangeStarting"],[-5.859270242,5.734140872955322,-0.015886684879660606,-0.07543433457612991,"laneChangeStarting"],[-5.848944825,5.742019176483154,-0.015886684879660606,-0.07530345767736435,"laneChangeStarting"],[-5.837736388,5.750296115875244,-0.015681853517889977,-0.07517294585704803,"laneChangeStarting"],[-5.829714305,5.75744104385376,-0.015681853517889977,-0.07504276186227798,"laneChangeStarting"],[-5.818537014,5.764305114746094,-0.015681853517889977,-0.074912890791893,"laneChangeStarting"],[-5.811972847,5.77243709564209,-0.015681853517889977,-0.07478338479995728,"laneChangeStarting"],[-5.79672842,5.779866695404053,-0.01580059342086315,-0.07465420663356781,"laneChangeStarting"],[-5.788434983,5.7858967781066895,-0.01580059342086315,-0.07452530413866043,"laneChangeStarting"],[-5.782053264,5.793670177459717,-0.01580059342086315,-0.07439674437046051,"laneChangeStarting"],[-5.765740296,5.804582595825195,-0.01580059342086315,-0.07426866888999939,"laneChangeStarting"],[-5.757361911,5.813479423522949,-0.01580059342086315,-0.07414098083972931,"laneChangeStarting"],[-5.738834515,5.822383880615234,-0.015373378060758114,-0.07401368767023087,"laneChangeStarting"],[-5.732139203,5.83355712890625,-0.015373378060758114,-0.07388687878847122,"laneChangeStarting"],[-5.721380661,5.852330684661865,-0.015373378060758114,-0.07376088201999664,"laneChangeStarting"],[-5.718953109,5.852330684661865,-0.015373378060758114,-0.07363488525152206,"laneChangeStarting"],[-5.707082693,5.86166524887085,-0.015373378060758114,-0.07350929081439972,"laneChangeStarting"],[-5.695937224,5.8709306716918945,-0.015411756932735443,-0.07338409125804901,"laneChangeStarting"],[-5.686666131,5.882017135620117,-0.015411756932735443,-0.0732593685388565,"laneChangeStarting"],[-5.678138371,5.8917555809021,-0.015411756932735443,-0.07313504815101624,"laneChangeStarting"],[-5.662297277,5.899855613708496,-0.015411756932735443,-0.07301107794046402,"laneChangeStarting"],[-5.651462642,5.906440734863281,-0.015411756932735443,-0.0728873759508133,"laneChangeStarting"],[-5.640241236,5.916172504425049,-0.015580684877932072,-0.072764091193676,"laneChangeStarting"],[-5.631911236,5.923916339874268,-0.015580684877932072,-0.07264111936092377,"laneChangeStarting"],[-5.622300716,5.931708812713623,-0.015580684877932072,-0.0725184753537178,"laneChangeStarting"],[-5.614122539,5.939178943634033,-0.015580684877932072,-0.07239613682031631,"laneChangeStarting"],[-5.601689831,5.947118282318115,-0.015580684877932072,-0.07227412611246109,"laneChangeStarting"],[-5.588404102,5.957388877868652,-0.01540130190551281,-0.0721525326371193,"laneChangeStarting"],[-5.581993946,5.966036319732666,-0.01540130190551281,-0.07203129678964615,"laneChangeStarting"],[-5.572896602,5.973927974700928,-0.01540130190551281,-0.07191037386655807,"laneChangeStarting"],[-5.55235978,5.985279083251953,-0.01540130190551281,-0.07178991287946701,"laneChangeStarting"],[-5.54556202,5.996425151824951,-0.015010076574981213,-0.07166989892721176,"laneChangeStarting"],[-5.534497541,6.006224632263184,-0.015010076574981213,-0.07155027985572815,"laneChangeStarting"],[-5.527712228,6.015517711639404,-0.015010076574981213,-0.07143102586269379,"laneChangeStarting"],[-5.516394572,6.02584981918335,-0.015010076574981213,-0.07131218165159225,"laneChangeStarting"],[-5.508022854,6.035248279571533,-0.015010076574981213,-0.07119370996952057,"laneChangeStarting"],[-5.493053948,6.044532775878906,-0.015091825276613235,-0.07107559591531754,"laneChangeStarting"],[-5.486631031,6.05295467376709,-0.015091825276613235,-0.07095781713724136,"laneChangeStarting"],[-5.478357386,6.0590715408325195,-0.015091825276613235,-0.0708402693271637,"laneChangeStarting"],[-5.468404001,6.066094875335693,-0.015091825276613235,-0.07072299718856812,"laneChangeStarting"],[-5.451742022,6.073225975036621,-0.015091825276613235,-0.07060600072145462,"laneChangeStarting"],[-5.43850421,6.081984519958496,-0.015152896754443645,-0.07048934698104858,"laneChangeStarting"],[-5.431329991,6.088857173919678,-0.015152896754443645,-0.07037294656038284,"laneChangeStarting"],[-5.423346397,6.09625768661499,-0.015152896754443645,-0.07025682926177979,"laneChangeStarting"],[-5.414970721,6.105292797088623,-0.015152896754443645,-0.07014106214046478,"laneChangeStarting"],[-5.403442648,6.113577365875244,-0.015152896754443645,-0.0699516162276268,"laneChangeStarting"],[-5.390137804,6.12191915512085,-0.0152197927236557,-0.06976111233234406,"laneChangeStarting"],[-5.383476242,6.1288042068481445,-0.0152197927236557,-0.06960446387529373,"laneChangeStarting"],[-5.372395669,6.138124465942383,-0.0152197927236557,-0.06939324736595154,"laneChangeStarting"],[-5.364249159,6.148191928863525,-0.0152197927236557,-0.06912530958652496,"laneChangeStarting"],[-5.339099837,6.160070419311523,-0.014631020836532116,-0.06885897368192673,"laneChangeStarting"],[-5.336780358,6.160070419311523,-0.014631020836532116,-0.06874525547027588,"laneChangeStarting"],[-5.328373014,6.17057991027832,-0.014631020836532116,-0.06862462311983109,"laneChangeStarting"],[-5.318857494,6.191468715667725,-0.014631020836532116,-0.06810473650693893,"laneChangeStarting"],[-5.316633848,6.191468715667725,-0.014631020836532116,-0.06799216568470001,"laneChangeStarting"],[-5.306514004,6.200711250305176,-0.014631020836532116,-0.06787993013858795,"laneChangeStarting"],[-5.292623328,6.209476947784424,-0.014398882165551186,-0.06771028786897659,"laneChangeStarting"],[-5.286146557,6.218937397003174,-0.014398882165551186,-0.06750443577766418,"laneChangeStarting"],[-5.276233953,6.226775169372559,-0.014398882165551186,-0.0673345997929573,"laneChangeStarting"],[-5.266200412,6.234626770019531,-0.014398882165551186,-0.067063108086586,"laneChangeStarting"],[-5.258243172,6.243637561798096,-0.014398882165551186,-0.06686967611312866,"laneChangeStarting"],[-5.233215621,6.259988307952881,-0.014405333437025547,-0.0665208101272583,"laneChangeStarting"],[-5.230884423,6.259988307952881,-0.014405333437025547,-0.06641069054603577,"laneChangeStarting"],[-5.22143859,6.267833232879639,-0.014405333437025547,-0.06630084663629532,"laneChangeStarting"],[-5.211965257,6.276485919952393,-0.014405333437025547,-0.06603942811489105,"laneChangeStarting"],[-5.201071194,6.285890579223633,-0.014405333437025547,-0.06584196537733078,"laneChangeStarting"],[-5.189816455,6.295234203338623,-0.014487973414361477,-0.06564665585756302,"laneChangeStarting"],[-5.183411351,6.304137229919434,-0.014487973414361477,-0.06546136736869812,"laneChangeStarting"],[-5.171881976,6.312629222869873,-0.014487973414361477,-0.06528536230325699,"laneChangeStarting"],[-5.16347557,6.321497917175293,-0.014487973414361477,-0.06495577841997147,"laneChangeStarting"],[-5.152117862,6.32996129989624,-0.014487973414361477,-0.06478219479322433,"laneChangeStarting"],[-5.133192081,6.339561462402344,-0.014390937983989716,-0.06458614021539688,"laneChangeStarting"],[-5.12368703,6.3471903800964355,-0.014390937983989716,-0.06429468095302582,"laneChangeStarting"],[-5.117238071,6.357151031494141,-0.014390937983989716,-0.06409336626529694,"laneChangeStarting"],[-5.106714895,6.365871429443359,-0.014390937983989716,-0.06391788274049759,"laneChangeStarting"],[-5.092247812,6.3741936683654785,-0.013916629366576672,-0.06375108659267426,"laneChangeStarting"],[-5.082529322,6.381772518157959,-0.013916629366576672,-0.06359975785017014,"laneChangeStarting"],[-5.075223958,6.391315460205078,-0.013916629366576672,-0.06340997666120529,"laneChangeStarting"],[-5.066130677,6.4004130363464355,-0.013916629366576672,-0.06312695145606995,"laneChangeStarting"],[-5.053556458,6.410602569580078,-0.013916629366576672,-0.06292643398046494,"laneChangeStarting"],[-5.046823698,6.419907093048096,-0.013547709211707115,-0.06274416297674179,"laneChangeStarting"],[-5.025523855,6.437137126922607,-0.013547709211707115,-0.062408726662397385,"laneChangeStarting"],[-5.023057917,6.437137126922607,-0.013547709211707115,-0.06230458244681358,"laneChangeStarting"],[-5.014005261,6.445185661315918,-0.013547709211707115,-0.06218792125582695,"laneChangeStarting"],[-5.001754637,6.45515251159668,-0.013547709211707115,-0.06199603155255318,"laneChangeStarting"],[-4.989044481,6.465761184692383,-0.013375392183661461,-0.061792757362127304,"laneChangeStarting"],[-4.982679325,6.474684238433838,-0.013375392183661461,-0.061622556298971176,"laneChangeStarting"],[-4.973710991,6.48354959487915,-0.013375392183661461,-0.06145415082573891,"laneChangeStarting"],[-4.964507971,6.491986274719238,-0.013375392183661461,-0.061225105077028275,"laneChangeStarting"],[-4.951110159,6.50002908706665,-0.013375392183661461,-0.06107368692755699,"laneChangeStarting"],[-4.939906721,6.509225845336914,-0.013454984873533249,-0.060901228338479996,"laneChangeStarting"],[-4.925890732,6.5179829597473145,-0.013454984873533249,-0.060737691819667816,"laneChangeStarting"],[-4.916401982,6.527843952178955,-0.013454984873533249,-0.060486529022455215,"laneChangeStarting"],[-4.902643441,6.538345813751221,-0.013454984873533249,-0.060292378067970276,"laneChangeStarting"],[-4.89577667,6.546784400939941,-0.012930169701576233,-0.060137052088975906,"laneChangeStarting"],[-4.884171775,6.554820537567139,-0.012930169701576233,-0.05998968705534935,"laneChangeStarting"],[-4.877571827,6.563246726989746,-0.012930169701576233,-0.05983575060963631,"laneChangeStarting"],[-4.867956306,6.57317590713501,-0.012930169701576233,-0.05965511500835419,"laneChangeStarting"],[-4.859915005,6.584871768951416,-0.012930169701576233,-0.059413060545921326,"laneChangeStarting"],[-4.843354484,6.593276023864746,-0.012966837733983994,-0.05926169082522392,"laneChangeStarting"],[-4.836708078,6.602778434753418,-0.012966837733983994,-0.0590912401676178,"laneChangeStarting"],[-4.818147922,6.61405611038208,-0.012966837733983994,-0.058898359537124634,"laneChangeStarting"],[-4.816013964,6.61405611038208,-0.012966837733983994,-0.05879971385002136,"laneChangeStarting"],[-4.80708985,6.623566627502441,-0.012966837733983994,-0.058701351284980774,"laneChangeStarting"],[-4.793954954,6.632563591003418,-0.012891157530248165,-0.05857011675834656,"laneChangeStarting"],[-4.787046256,6.641841888427734,-0.012891157530248165,-0.05840659141540527,"laneChangeStarting"],[-4.780582298,6.649495601654053,-0.012891157530248165,-0.058272216469049454,"laneChangeStarting"],[-4.770913965,6.6556525230407715,-0.012891157530248165,-0.058164454996585846,"laneChangeStarting"],[-4.762430007,6.663468360900879,-0.012891157530248165,-0.05804244428873062,"laneChangeStarting"],[-4.745822039,6.673595905303955,-0.012839584611356258,-0.0578664131462574,"laneChangeStarting"],[-4.739173237,6.6820855140686035,-0.012839584611356258,-0.05771946907043457,"laneChangeStarting"],[-4.730838081,6.688676834106445,-0.012839584611356258,-0.057605765759944916,"laneChangeStarting"],[-4.712713706,6.69692325592041,-0.012839584611356258,-0.0574701726436615,"laneChangeStarting"],[-4.705786779,6.705581188201904,-0.012839584611356258,-0.05732186138629913,"laneChangeStarting"],[-4.687002769,6.715001106262207,-0.012390130199491978,-0.057161152362823486,"laneChangeStarting"],[-4.684785477,6.715001106262207,-0.012390130199491978,-0.057065449655056,"laneChangeStarting"],[-4.677778759,6.722856044769287,-0.012390130199491978,-0.05696997046470642,"laneChangeStarting"],[-4.668190061,6.739827632904053,-0.012390130199491978,-0.056716438382864,"laneChangeStarting"],[-4.666059488,6.739827632904053,-0.012390130199491978,-0.05662143975496292,"laneChangeStarting"],[-4.657339749,6.748539447784424,-0.012390130199491978,-0.05652668699622154,"laneChangeStarting"],[-4.641959124,6.755367279052734,-0.012258131988346577,-0.056432127952575684,"laneChangeStarting"],[-4.635148812,6.761214256286621,-0.012258131988346577,-0.05633772909641266,"laneChangeStarting"],[-4.6259685,6.771084308624268,-0.012258131988346577,-0.05619401857256889,"laneChangeStarting"],[-4.611798396,6.779018402099609,-0.012258131988346577,-0.056002914905548096,"laneChangeStarting"],[-4.598884021,6.788552761077881,-0.012258131988346577,-0.0558457151055336,"laneChangeStarting"],[-4.587426417,6.797677516937256,-0.011781379580497742,-0.05569588765501976,"laneChangeStarting"],[-4.580831834,6.805279731750488,-0.011781379580497742,-0.05557152256369591,"laneChangeStarting"],[-4.570879022,6.811851501464844,-0.011781379580497742,-0.055396854877471924,"laneChangeStarting"],[-4.562366731,6.8197479248046875,-0.011781379580497742,-0.05526864528656006,"laneChangeStarting"],[-4.5458572,6.829259395599365,-0.01194063387811184,-0.05511480197310448,"laneChangeStarting"],[-4.539293137,6.837239742279053,-0.01194063387811184,-0.05498621612787247,"laneChangeStarting"],[-4.530899856,6.846441745758057,-0.01194063387811184,-0.05483850836753845,"laneChangeStarting"],[-4.521120794,6.8529953956604,-0.01194063387811184,-0.05473367124795914,"laneChangeStarting"],[-4.504063555,6.8608784675598145,-0.01194063387811184,-0.0545734167098999,"laneChangeStarting"],[-4.491070587,6.871513366699219,-0.011729734018445015,-0.054404620081186295,"laneChangeStarting"],[-4.482867149,6.880531311035156,-0.011729734018445015,-0.054262105375528336,"laneChangeStarting"],[-4.47627965,6.887281894683838,-0.011729734018445015,-0.05415578559041023,"laneChangeStarting"],[-4.466455431,6.894204616546631,-0.011729734018445015,-0.05406499654054642,"laneChangeStarting"],[-4.458147254,6.900168418884277,-0.011729734018445015,-0.05397289618849754,"laneChangeStarting"],[-4.442148348,6.907527923583984,-0.011262861080467701,-0.0538579486310482,"laneChangeStarting"],[-4.433349859,6.913548469543457,-0.011262861080467701,-0.053764186799526215,"laneChangeStarting"],[-4.426027932,6.923252582550049,-0.011262861080467701,-0.0536135733127594,"laneChangeStarting"],[-4.416660589,6.93107795715332,-0.011262861080467701,-0.05352374538779259,"laneChangeStarting"],[-4.399848454,6.940554618835449,-0.011262861080467701,-0.05340714380145073,"laneChangeStarting"],[-4.387974548,6.947021007537842,-0.010910364799201488,-0.05330776423215866,"laneChangeStarting"],[-4.377478871,6.9668049812316895,-0.010910364799201488,-0.053005434572696686,"laneChangeStarting"],[-4.374518767,6.9668049812316895,-0.010910364799201488,-0.052916523069143295,"laneChangeStarting"],[-4.364758455,6.975708484649658,-0.010910364799201488,-0.05282784253358841,"laneChangeStarting"],[-4.351648872,6.9846343994140625,-0.010910364799201488,-0.05273938551545143,"laneChangeStarting"],[-4.338470487,6.995086193084717,-0.010603130795061588,-0.05265119671821594,"laneChangeStarting"],[-4.332039237,7.003915786743164,-0.010603130795061588,-0.0525287389755249,"laneChangeStarting"],[-4.32291507,7.009721755981445,-0.010603130795061588,-0.05244091525673866,"laneChangeStarting"],[-4.313953821,7.017650127410889,-0.010603130795061588,-0.05235329270362854,"laneChangeStarting"],[-4.287908717,7.026425838470459,-0.010318211279809475,-0.05226588621735573,"laneChangeStarting"],[-4.280532519,7.035608291625977,-0.010318211279809475,-0.05213182792067528,"laneChangeStarting"],[-4.268170332,7.052811145782471,-0.010318211279809475,-0.051938943564891815,"laneChangeStarting"],[-4.265919916,7.052811145782471,-0.010318211279809475,-0.05185219272971153,"laneChangeStarting"],[-4.257640593,7.062041282653809,-0.010318211279809475,-0.05176566541194916,"laneChangeStarting"],[-4.24214476,7.069738864898682,-0.01002857182174921,-0.051679324358701706,"laneChangeStarting"],[-4.235604812,7.07828426361084,-0.01002857182174921,-0.051565784960985184,"laneChangeStarting"],[-4.222967833,7.089507102966309,-0.01002857182174921,-0.051402654498815536,"laneChangeStarting"],[-4.215563615,7.100173473358154,-0.01002857182174921,-0.05131214112043381,"laneChangeStarting"],[-4.207442209,7.108790874481201,-0.01002857182174921,-0.05118781328201294,"laneChangeStarting"],[-4.185023251,7.1162590980529785,-0.009638519957661629,-0.051080428063869476,"laneChangeStarting"],[-4.17770898,7.122292518615723,-0.009638519957661629,-0.05099392309784889,"laneChangeStarting"],[-4.170112106,7.128150939941406,-0.009638519957661629,-0.05090899392962456,"laneChangeStarting"],[-4.161340231,7.1346282958984375,-0.009638519957661629,-0.05082421749830246,"laneChangeStarting"],[-4.151425596,7.142832279205322,-0.009638519957661629,-0.05073963850736618,"laneChangeStarting"],[-4.138480596,7.151887893676758,-0.00966127123683691,-0.050653789192438126,"laneChangeStarting"],[-4.131881117,7.159460067749023,-0.00966127123683691,-0.05054669827222824,"laneChangeStarting"],[-4.120316482,7.167923450469971,-0.00966127123683691,-0.050427403301000595,"laneChangeStarting"],[-4.111935805,7.1749467849731445,-0.00966127123683691,-0.050343576818704605,"laneChangeStarting"],[-4.100061639,7.182151794433594,-0.00966127123683691,-0.050259921699762344,"laneChangeStarting"],[-4.078075389,7.201559066772461,-0.009245454333722591,-0.0500459186732769,"laneChangeStarting"],[-4.075130181,7.201559066772461,-0.009245454333722591,-0.049962714314460754,"laneChangeStarting"],[-4.06545914,7.211559295654297,-0.009245454333722591,-0.049879737198352814,"laneChangeStarting"],[-4.056992265,7.219226360321045,-0.009245454333722591,-0.0497969351708889,"laneChangeStarting"],[-4.04115638,7.228505611419678,-0.008760669268667698,-0.0497143492102623,"laneChangeStarting"],[-4.03463888,7.237010478973389,-0.008760669268667698,-0.04963195323944092,"laneChangeStarting"],[-4.02602888,7.244417190551758,-0.008760669268667698,-0.04954968020319939,"laneChangeStarting"],[-4.016361693,7.250810623168945,-0.008760669268667698,-0.049467600882053375,"laneChangeStarting"],[-4.001096016,7.259680271148682,-0.008760669268667698,-0.04938571900129318,"laneChangeStarting"],[-3.994460339,7.26782751083374,-0.008395283482968807,-0.04930402338504791,"laneChangeStarting"],[-3.976043829,7.287209987640381,-0.008395283482968807,-0.04905225709080696,"laneChangeStarting"],[-3.973083621,7.287209987640381,-0.008395283482968807,-0.04897099360823631,"laneChangeStarting"],[-3.963389663,7.296478748321533,-0.008395283482968807,-0.04888993874192238,"laneChangeStarting"],[-3.950226643,7.305719375610352,-0.008395283482968807,-0.04880908876657486,"laneChangeStarting"],[-3.93585331,7.321949005126953,-0.008167090825736523,-0.04865116626024246,"off"],[-3.93362831,7.321949005126953,-0.008167090825736523,-0.04857005923986435,"off"],[-3.924239456,7.329752445220947,-0.008167090825736523,-0.04848853498697281,"off"],[-3.913754717,7.336126804351807,-0.008167090825736523,-0.04840638116002083,"off"],[-3.900484196,7.344213962554932,-0.008167090825736523,-0.04832347109913826,"off"],[-3.888186019,7.3508758544921875,-0.007728024385869503,-0.04823961481451988,"off"],[-3.875663207,7.358492851257324,-0.007728024385869503,-0.048154689371585846,"off"],[-3.865840187,7.366248607635498,-0.007728024385869503,-0.04806854948401451,"off"],[-3.852686333,7.372626304626465,-0.007728024385869503,-0.04798103868961334,"off"],[-3.846035812,7.380002498626709,-0.007319423370063305,-0.04789205268025398,"off"],[-3.836059614,7.387181282043457,-0.007319423370063305,-0.04780147224664688,"off"],[-3.827416177,7.394559860229492,-0.007319423370063305,-0.047709181904792786,"off"],[-3.817699459,7.403261661529541,-0.007319423370063305,-0.04761511832475662,"off"],[-3.80945623,7.413564205169678,-0.007319423370063305,-0.047519225627183914,"off"],[-3.790621959,7.4223103523254395,-0.006822639144957066,-0.04742136970162392,"off"],[-3.784097637,7.43186616897583,-0.006822639144957066,-0.047321487218141556,"off"],[-3.769855762,7.441028594970703,-0.006822639144957066,-0.04721948876976967,"off"],[-3.760883158,7.44868278503418,-0.006822639144957066,-0.047115255147218704,"off"],[-3.742681232,7.455694198608398,-0.006443565245717764,-0.0470087006688118,"off"],[-3.736105294,7.46136474609375,-0.006443565245717764,-0.046899713575839996,"off"],[-3.729768732,7.470343589782715,-0.006443565245717764,-0.046788327395915985,"off"],[-3.720024513,7.479372024536133,-0.006443565245717764,-0.04667448252439499,"off"],[-3.711513472,7.4884352684021,-0.006443565245717764,-0.046558134257793427,"off"],[-3.693196858,7.496389389038086,-0.006064928602427244,-0.046439189463853836,"off"],[-3.68647342,7.502934455871582,-0.006064928602427244,-0.046317558735609055,"off"],[-3.680159462,7.511206150054932,-0.006064928602427244,-0.04619324952363968,"off"],[-3.662624463,7.518061637878418,-0.006064928602427244,-0.046066172420978546,"off"],[-3.65597514,7.527764797210693,-0.006064928602427244,-0.04593638703227043,"off"],[-3.637021495,7.537836074829102,-0.005776728503406048,-0.045803871005773544,"off"],[-3.634819099,7.537836074829102,-0.005776728503406048,-0.04566822946071625,"off"],[-3.627889151,7.546341896057129,-0.005776728503406048,-0.04552973434329033,"off"],[-3.620770349,7.5564093589782715,-0.005776728503406048,-0.045388419181108475,"off"],[-3.612647589,7.566402912139893,-0.005776728503406048,-0.045244250446558,"off"],[-3.605963162,7.574800968170166,-0.005776728503406048,-0.045097146183252335,"off"],[-3.589204569,7.582849025726318,-0.005246411077678204,-0.044947072863578796,"off"],[-3.582247434,7.589443683624268,-0.005246411077678204,-0.044793952256441116,"off"],[-3.575872642,7.595458984375,-0.005246411077678204,-0.044637735933065414,"off"],[-3.561780403,7.602092266082764,-0.005246411077678204,-0.04447842761874199,"off"],[-3.550773997,7.608561038970947,-0.005246411077678204,-0.04431600868701935,"off"],[-3.53901681,7.617157459259033,-0.004827649332582951,-0.04415055364370346,"off"],[-3.532599779,7.625083923339844,-0.004827649332582951,-0.043982017785310745,"off"],[-3.5234978,7.633142471313477,-0.004827649332582951,-0.043810393661260605,"off"],[-3.513587748,7.6432108879089355,-0.004827649332582951,-0.0436357706785202,"off"],[-3.500460145,7.650219440460205,-0.004827649332582951,-0.0434579961001873,"off"],[-3.489098426,7.6623215675354,-0.004477089736610651,-0.043277304619550705,"off"],[-3.482492906,7.67270565032959,-0.004477089736610651,-0.04309360682964325,"off"],[-3.473408479,7.679958343505859,-0.004477089736610651,-0.042906753718853,"off"],[-3.453062386,7.688086032867432,-0.004477089736610651,-0.04271678254008293,"off"],[-3.446225511,7.697032451629639,-0.004179125186055899,-0.04252373054623604,"off"],[-3.434975563,7.705602169036865,-0.004179125186055899,-0.042327575385570526,"off"],[-3.427919261,7.715332508087158,-0.004179125186055899,-0.04212837666273117,"off"],[-3.417995616,7.721597194671631,-0.004179125186055899,-0.04192595183849335,"off"],[-3.409766241,7.729947566986084,-0.004179125186055899,-0.04172041639685631,"off"],[-3.392076502,7.737973213195801,-0.003766364650800824,-0.041511744260787964,"off"],[-3.38473921,7.7460784912109375,-0.003766364650800824,-0.04129994660615921,"off"],[-3.378209106,7.754260063171387,-0.003766364650800824,-0.041085027158260345,"off"],[-3.368381242,7.762514591217041,-0.003766364650800824,-0.04086699336767197,"off"],[-3.35119093,7.7697038650512695,-0.003766364650800824,-0.0406457893550396,"off"],[-3.338544264,7.779312610626221,-0.0032469765283167362,-0.04042155295610428,"off"],[-3.330855618,7.7870049476623535,-0.0032469765283167362,-0.040194179862737656,"off"],[-3.324075306,7.794807434082031,-0.0032469765283167362,-0.03996368125081062,"off"],[-3.31441239,7.802718162536621,-0.0032469765283167362,-0.03973006457090378,"off"],[-3.303180827,7.809598922729492,-0.0032469765283167362,-0.03949327394366264,"off"],[-3.290180151,7.816289901733398,-0.003070027567446232,-0.03925330564379692,"off"],[-3.283759995,7.823194980621338,-0.003070027567446232,-0.039010170847177505,"off"],[-3.274524943,7.828043460845947,-0.003070027567446232,-0.03876374661922455,"off"],[-3.264022131,7.836673736572266,-0.003070027567446232,-0.03851427882909775,"off"],[-3.240065986,7.844281196594238,-0.0024795937351882458,-0.03826170414686203,"off"],[-3.237783954,7.844281196594238,-0.0024795937351882458,-0.038005534559488297,"off"],[-3.229631767,7.856603622436523,-0.0024795937351882458,-0.03774658218026161,"off"],[-3.222287653,7.865336894989014,-0.0024795937351882458,-0.03748461604118347,"off"],[-3.21481484,7.8763885498046875,-0.0024795937351882458,-0.03721980005502701,"off"],[-3.208389945,7.885791778564453,-0.0024795937351882458,-0.03695204108953476,"off"],[-3.191597706,7.894028186798096,-0.0022694978397339582,-0.03668126463890076,"off"],[-3.184662029,7.900803089141846,-0.0022694978397339582,-0.03640738129615784,"off"],[-3.17796255,7.908878326416016,-0.0022694978397339582,-0.03613048419356346,"off"],[-3.166504738,7.917411804199219,-0.0022694978397339582,-0.03585061803460121,"off"],[-3.158274217,7.9233479499816895,-0.0022694978397339582,-0.03556760773062706,"off"],[-3.136587186,7.932933330535889,-0.0017394563183188438,-0.0352817177772522,"off"],[-3.134288697,7.932933330535889,-0.0017394563183188438,-0.034992266446352005,"off"],[-3.126901718,7.940990924835205,-0.0017394563183188438,-0.03469984233379364,"off"],[-3.116511875,7.957373142242432,-0.0017394563183188438,-0.03440507501363754,"off"],[-3.114306562,7.957373142242432,-0.0017394563183188438,-0.03410675749182701,"off"],[-3.102582969,7.963419437408447,-0.0017394563183188438,-0.03380534425377846,"off"],[-3.090243125,7.97387170791626,-0.0017001779051497579,-0.03350117802619934,"off"],[-3.083752292,7.981226444244385,-0.0017001779051497579,-0.03319403529167175,"off"],[-3.072718386,7.989104747772217,-0.0017001779051497579,-0.032883964478969574,"off"],[-3.064105887,7.996717929840088,-0.0017001779051497579,-0.03257095813751221,"off"],[-3.052970366,8.004844665527344,-0.0017001779051497579,-0.032255060970783234,"off"],[-3.034069533,8.01307201385498,-0.0014552605571225286,-0.031936295330524445,"off"],[-3.027274794,8.022147178649902,-0.0014552605571225286,-0.03161473944783211,"off"],[-3.017189273,8.030868530273438,-0.0014552605571225286,-0.03129037469625473,"off"],[-3.008987867,8.037744522094727,-0.0014552605571225286,-0.03096306137740612,"off"],[-2.991576305,8.047070503234863,-0.001274708192795515,-0.03063301369547844,"off"],[-2.984893285,8.053754806518555,-0.001274708192795515,-0.030300021171569824,"off"],[-2.978435577,8.062150955200195,-0.001274708192795515,-0.029964236542582512,"off"],[-2.965166514,8.074026107788086,-0.001274708192795515,-0.029625961557030678,"off"],[-2.957224171,8.081926345825195,-0.001274708192795515,-0.029284877702593803,"off"],[-2.943668338,8.092552185058594,-0.000949925510212779,-0.02894122526049614,"off"],[-2.927673755,8.101161003112793,-0.000949925510212779,-0.0285948459059,"off"],[-2.91945188,8.108269691467285,-0.000949925510212779,-0.02824562042951584,"off"],[-2.906273287,8.117396354675293,-0.000949925510212779,-0.027893735095858574,"off"],[-2.899871673,8.124996185302734,-0.000949925510212779,-0.027539068832993507,"off"],[-2.887150892,8.133071899414062,-0.0008965698070824146,-0.02718167193233967,"off"],[-2.880664069,8.139705657958984,-0.0008965698070824146,-0.026821427047252655,"off"],[-2.871303965,8.148792266845703,-0.0008965698070824146,-0.026458561420440674,"off"],[-2.861328028,8.158266067504883,-0.0008965698070824146,-0.026093123480677605,"off"],[-2.848715684,8.169221878051758,-0.0008965698070824146,-0.025725258514285088,"off"],[-2.836994851,8.178511619567871,-0.0008359860512427986,-0.02535482682287693,"off"],[-2.822958706,8.186997413635254,-0.0008359860512427986,-0.024981768801808357,"off"],[-2.813574488,8.196626663208008,-0.0008359860512427986,-0.02460620179772377,"off"],[-2.800303915,8.207690238952637,-0.0008359860512427986,-0.02422826737165451,"off"],[-2.788489801,8.215532302856445,-0.0007254352094605565,-0.023847680538892746,"off"],[-2.781586051,8.223408699035645,-0.0007254352094605565,-0.02346445620059967,"off"],[-2.77128527,8.231327056884766,-0.0007254352094605565,-0.023078609257936478,"off"],[-2.762380114,8.238160133361816,-0.0007254352094605565,-0.022690046578645706,"off"],[-2.749775896,8.245890617370605,-0.0007254352094605565,-0.022298861294984818,"off"],[-2.738395375,8.252198219299316,-0.0004978126962669194,-0.021904930472373962,"off"],[-2.731402355,8.259469032287598,-0.0004978126962669194,-0.0215083509683609,"off"],[-2.711711261,8.270668983459473,-0.0004978126962669194,-0.02110951580107212,"off"],[-2.699084335,8.27869701385498,-0.0004978126962669194,-0.020708128809928894,"off"],[-2.688298293,8.290575981140137,-0.00038360522012226284,-0.020304573699831963,"off"],[-2.680759804,8.29958438873291,-0.00038360522012226284,-0.019898585975170135,"off"],[-2.671912617,8.308183670043945,-0.00038360522012226284,-0.019490135833621025,"off"],[-2.662406888,8.314891815185547,-0.00038360522012226284,-0.019079046323895454,"off"],[-2.656105794,8.320237159729004,-0.00038360522012226284,-0.018665192648768425,"off"],[-2.638746784,8.32850170135498,-0.00029903239919804037,-0.018248870968818665,"off"],[-2.632046524,8.338350296020508,-0.00029903239919804037,-0.017830252647399902,"off"],[-2.624909233,8.347015380859375,-0.00029903239919804037,-0.017409229651093483,"off"],[-2.61211866,8.356840133666992,-0.00029903239919804037,-0.016985930502414703,"off"],[-2.6012259,8.365843772888184,-0.00029903239919804037,-0.016560282558202744,"off"],[-2.589526421,8.374077796936035,-0.00017554531223140657,-0.01613221876323223,"off"],[-2.58307163,8.38424301147461,-0.00017554531223140657,-0.01570194773375988,"off"],[-2.574022672,8.391280174255371,-0.00017554531223140657,-0.01526915654540062,"off"],[-2.564502776,8.399197578430176,-0.00017554531223140657,-0.014833947643637657,"off"],[-2.551919495,8.407565116882324,-0.00017554531223140657,-0.014396375976502895,"off"],[-2.538607517,8.417107582092285,1.2688426977547351e-05,-0.013956575654447079,"off"],[-2.532188038,8.425088882446289,1.2688426977547351e-05,-0.01351439394056797,"off"],[-2.523112101,8.431246757507324,1.2688426977547351e-05,-0.01306964736431837,"off"],[-2.505353455,8.436861038208008,1.2688426977547351e-05,-0.012622285634279251,"off"],[-2.503247726,8.436861038208008,1.2688426977547351e-05,-0.012171718291938305,"off"],[-2.490815018,8.443130493164062,0.00014483935956377536,-0.011718617752194405,"off"],[-2.484120487,8.451539039611816,0.00014483935956377536,-0.011263223364949226,"off"],[-2.477612415,8.460423469543457,0.00014483935956377536,-0.01080559566617012,"off"],[-2.467346738,8.467097282409668,0.00014483935956377536,-0.01034550555050373,"off"],[-2.459331061,8.477768898010254,0.00014483935956377536,-0.00988339725881815,"off"],[-2.443348718,8.487226486206055,9.22852341318503e-05,-0.009419148787856102,"off"],[-2.436945072,8.495161056518555,9.22852341318503e-05,-0.008952603675425053,"off"],[-2.428625489,8.505459785461426,9.22852341318503e-05,-0.00848403200507164,"off"],[-2.418166635,8.514161109924316,9.22852341318503e-05,-0.008013268932700157,"off"],[-2.401768146,8.522505760192871,9.22852341318503e-05,-0.0075402832590043545,"off"],[-2.390102105,8.531274795532227,0.00034358183620497584,-0.007065133191645145,"off"],[-2.383336584,8.539680480957031,0.00034358183620497584,-0.006587786599993706,"off"],[-2.373931324,8.546993255615234,0.00034358183620497584,-0.006108131725341082,"off"],[-2.363703668,8.555944442749023,0.00034358183620497584,-0.005626360420137644,"off"],[-2.353870439,8.56301498413086,0.00034358183620497584,-0.005142271053045988,"off"],[-2.337605857,8.579151153564453,0.0005530533380806446,-0.004656898323446512,"off"],[-2.334833773,8.579151153564453,0.0005530533380806446,-0.004168422892689705,"off"],[-2.324904451,8.584668159484863,0.0005530533380806446,-0.00367747712880373,"off"],[-2.314896795,8.594250679016113,0.0005530533380806446,-0.0031845339108258486,"off"],[-2.291854712,8.604205131530762,0.0007761885644868016,-0.00268964609131217,"off"],[-2.284042837,8.612598419189453,0.0007761885644868016,-0.0021926439367234707,"off"],[-2.276826588,8.620670318603516,0.0007761885644868016,-0.0016934992745518684,"off"],[-2.268792734,8.625802040100098,0.0007761885644868016,-0.0011918791569769382,"off"],[-2.258410026,8.631248474121094,0.0007761885644868016,-0.0006878265994600952,"off"],[-2.243372839,8.640414237976074,0.0009873996023088694,-0.00018178392201662064,"off"],[-2.235713516,8.648873329162598,0.0009873996023088694,0.0003263223043177277,"off"],[-2.225239037,8.662399291992188,0.0009873996023088694,0.0008358864579349756,"off"],[-2.222413673,8.662399291992188,0.0009873996023088694,0.0009873996023088694,"off"],[-2.212719715,8.670267105102539,0.0009873996023088694,0.0009873996023088694,"off"],[-2.186326851,8.677912712097168,0.0010484895901754498,0.0010484895901754498,"off"],[-2.178122841,8.684218406677246,0.0010484895901754498,0.0010484895901754498,"off"],[-2.169849664,8.694578170776367,0.0010484895901754498,0.0010484895901754498,"off"],[-2.162518623,8.70566463470459,0.0010484895901754498,0.0010484895901754498,"off"],[-2.15051331,8.713994979858398,0.0010484895901754498,0.0010484895901754498,"off"],[-2.139346332,8.722780227661133,0.0012813901994377375,0.0012813901994377375,"off"],[-2.130959561,8.73160457611084,0.0012813901994377375,0.0012813901994377375,"off"],[-2.124451436,8.740081787109375,0.0012813901994377375,0.0012813901994377375,"off"],[-2.114795239,8.74898910522461,0.0012813901994377375,0.0012813901994377375,"off"],[-2.103549093,8.756779670715332,0.0012813901994377375,0.0012813901994377375,"off"],[-2.084650865,8.767314910888672,0.0016203975537791848,0.0016203975537791848,"off"],[-2.077834719,8.775094032287598,0.0016203975537791848,0.0016203975537791848,"off"],[-2.061658157,8.793315887451172,0.0016203975537791848,0.0016203975537791848,"off"],[-2.059521959,8.793315887451172,0.0016203975537791848,0.0016203975537791848,"off"],[-2.042087376,8.803023338317871,0.0018278987845405936,0.0018278987845405936,"off"],[-2.03529946,8.810394287109375,0.0018278987845405936,0.0018278987845405936,"off"],[-2.028667377,8.818625450134277,0.0018278987845405936,0.0018278987845405936,"off"],[-2.017276075,8.826904296875,0.0018278987845405936,0.0018278987845405936,"off"],[-2.008891127,8.833720207214355,0.0018278987845405936,0.0018278987845405936,"off"],[-1.995096857,8.839568138122559,0.001883979537524283,0.0018278987845405936,"off"],[-1.97853092,8.849449157714844,0.001883979537524283,0.001883979537524283,"off"],[-1.970673264,8.857780456542969,0.001883979537524283,0.001883979537524283,"off"],[-1.957743785,8.868067741394043,0.001883979537524283,0.001883979537524283,"off"],[-1.951189046,8.876755714416504,0.001883979537524283,0.001883979537524283,"off"],[-1.938278682,8.885087013244629,0.0020777801983058453,0.0020777801983058453,"off"],[-1.931780609,8.891952514648438,0.0020777801983058453,0.0020777801983058453,"off"],[-1.922640818,8.898982048034668,0.0020777801983058453,0.0020777801983058453,"off"],[-1.91310962,8.905420303344727,0.0020777801983058453,0.0020777801983058453,"off"],[-1.899662641,8.914725303649902,0.0020777801983058453,0.0020777801983058453,"off"],[-1.888746288,8.925176620483398,0.002344152657315135,0.002344152657315135,"off"],[-1.874841132,8.934783935546875,0.002344152657315135,0.002344152657315135,"off"],[-1.865051965,8.94510269165039,0.002344152657315135,0.002344152657315135,"off"],[-1.851373008,8.954934120178223,0.002344152657315135,0.002344152657315135,"off"],[-1.844786445,8.963165283203125,0.0024625910446047783,0.002344152657315135,"off"],[-1.835084206,8.973312377929688,0.0024625910446047783,0.0024625910446047783,"off"],[-1.826722227,8.98371696472168,0.0024625910446047783,0.0024625910446047783,"off"],[-1.817488321,8.990947723388672,0.0024625910446047783,0.0024625910446047783,"off"],[-1.80838228,8.999016761779785,0.0024625910446047783,0.0024625910446047783,"off"],[-1.791520145,9.005230903625488,0.00249536638148129,0.00249536638148129,"off"],[-1.784938739,9.011253356933594,0.00249536638148129,0.00249536638148129,"off"],[-1.771809885,9.018256187438965,0.00249536638148129,0.00249536638148129,"off"],[-1.763182333,9.02580738067627,0.00249536638148129,0.00249536638148129,"off"],[-1.743848792,9.037630081176758,0.002629235154017806,0.00249536638148129,"off"],[-1.741505511,9.037630081176758,0.002629235154017806,0.002629235154017806,"off"],[-1.733173949,9.045502662658691,0.002629235154017806,0.002629235154017806,"off"],[-1.723711553,9.052020072937012,0.002629235154017806,0.002629235154017806,"off"],[-1.714477022,9.061822891235352,0.002629235154017806,0.002629235154017806,"off"],[-1.700304887,9.07012939453125,0.002629235154017806,0.002629235154017806,"off"],[-1.688604888,9.077792167663574,0.0027065707836300135,0.0027065707836300135,"off"],[-1.682085044,9.085614204406738,0.0027065707836300135,0.0027065707836300135,"off"],[-1.663772076,9.093583106994629,0.0027065707836300135,0.0027065707836300135,"off"],[-1.661665201,9.093583106994629,0.0027065707836300135,0.0027065707836300135,"off"],[-1.653338274,9.101311683654785,0.0027065707836300135,0.0027065707836300135,"off"],[-1.640646348,9.111082077026367,0.002636753721162677,0.002636753721162677,"off"],[-1.63332791,9.119714736938477,0.002636753721162677,0.002639818238094449,"off"],[-1.624039473,9.128035545349121,0.002636753721162677,0.002636753721162677,"off"],[-1.614184057,9.13719654083252,0.002636753721162677,0.0026409972924739122,"off"],[-1.604468068,9.14637565612793,0.002636753721162677,0.0026455060578882694,"off"],[-1.587891349,9.156316757202148,0.002558700041845441,0.0025754543021321297,"off"],[-1.580959371,9.16468334197998,0.002558700041845441,0.0025789355859160423,"off"],[-1.574089162,9.170830726623535,0.002558700041845441,0.002582145854830742,"off"],[-1.561032913,9.17908763885498,0.002558700041845441,0.0025881980545818806,"off"],[-1.550260674,9.186288833618164,0.002558700041845441,0.0025929969269782305,"off"],[-1.535174945,9.193648338317871,0.002473784377798438,0.002509430982172489,"off"],[-1.525905779,9.204419136047363,0.002473784377798438,0.0025102803483605385,"off"],[-1.522559685,9.204419136047363,0.002473784377798438,0.0025105890817940235,"off"],[-1.512957237,9.210702896118164,0.002473784377798438,0.0025114205200225115,"off"],[-1.500379477,9.216526985168457,0.002473784377798438,0.002512517152354121,"off"],[-1.487519165,9.225714683532715,0.0023945218417793512,0.002432047389447689,"off"],[-1.48053802,9.234618186950684,0.0023945218417793512,0.002431376138702035,"off"],[-1.47018427,9.240974426269531,0.0023945218417793512,0.0024304368998855352,"off"],[-1.45078125,9.251770973205566,0.0023945218417793512,0.0024287384003400803,"off"],[-1.443241719,9.262137413024902,0.0024410102050751448,0.0024746032431721687,"off"],[-1.432617501,9.270943641662598,0.0024410102050751448,0.0024737033527344465,"off"],[-1.425821199,9.279800415039062,0.0024410102050751448,0.0024731550365686417,"off"],[-1.41540896,9.28982925415039,0.0024410102050751448,0.0024723238311707973,"off"],[-1.406009689,9.299798965454102,0.0024410102050751448,0.002471599029377103,"off"],[-1.388487815,9.310453414916992,0.002345010871067643,0.0023730562534183264,"off"],[-1.381498909,9.31794548034668,0.002345010871067643,0.002372050192207098,"off"],[-1.372154013,9.326282501220703,0.002345010871067643,0.0023708143271505833,"off"],[-1.364306409,9.334653854370117,0.002345010871067643,0.0023698206059634686,"off"],[-1.349928754,9.343058586120605,0.002345010871067643,0.0023681034799665213,"off"],[-1.336825004,9.349608421325684,0.002313694218173623,0.0023353465367108583,"off"],[-1.328391671,9.358227729797363,0.002313694218173623,0.0023344375658780336,"off"],[-1.320779015,9.36650562286377,0.002313694218173623,0.0023336531594395638,"off"],[-1.311155474,9.371443748474121,0.002313694218173623,0.0023327351082116365,"off"],[-1.300207818,9.377461433410645,0.002313694218173623,0.002331705531105399,"off"],[-1.288367454,9.38337230682373,0.0022077762987464666,0.0022247759625315666,"off"],[-1.281672714,9.388446807861328,0.0022077762987464666,0.0022241882979869843,"off"],[-1.272234642,9.39541244506836,0.0022077762987464666,0.0022234334610402584,"off"],[-1.262715684,9.404522895812988,0.0022077762987464666,0.002222704002633691,"off"],[-1.23873058,9.412213325500488,0.002280646935105324,0.0022947238758206367,"off"],[-1.229988133,9.423111915588379,0.002280646935105324,0.002294408157467842,"off"],[-1.222181362,9.434320449829102,0.002280646935105324,0.002294141799211502,"off"],[-1.212853706,9.451937675476074,0.002280646935105324,0.0022938307374715805,"off"],[-1.210044227,9.451937675476074,0.002280646935105324,0.0022937345784157515,"off"],[-1.191447561,9.460440635681152,0.0021371664479374886,0.0021498792339116335,"off"],[-1.184149749,9.471261024475098,0.0021371664479374886,0.002149740932509303,"off"],[-1.176699437,9.480436325073242,0.0021371664479374886,0.00214961776509881,"off"],[-1.166740375,9.491095542907715,0.0021371664479374886,0.00214947247877717,"off"],[-1.157060792,9.49933910369873,0.0021371664479374886,0.00214934628456831,"off"],[-1.131150532,9.507983207702637,0.0021051582880318165,0.002122665988281369,"off"],[-1.128410376,9.507983207702637,0.0021051582880318165,0.0021232878789305687,"off"],[-1.118913032,9.520777702331543,0.0021051582880318165,0.0021251479629427195,"off"],[-1.111725637,9.52719783782959,0.0021051582880318165,0.0021265526302158833,"off"],[-1.105445116,9.532242774963379,0.0021051582880318165,0.002127773826941848,"off"],[-1.09165121,9.539066314697266,0.002056207275018096,0.0020864896941930056,"off"],[-1.084507409,9.544541358947754,0.002056207275018096,0.0020905977580696344,"off"],[-1.076944596,9.551434516906738,0.002056207275018096,0.0020946951117366552,"off"],[-1.065529649,9.559659004211426,0.002056207275018096,0.0021007321774959564,"off"],[-1.057570326,9.56647777557373,0.002056207275018096,0.0021046202164143324,"off"],[-1.042037514,9.576152801513672,0.002068936824798584,0.002123825019225478,"off"],[-1.028819077,9.586959838867188,0.002068936824798584,0.002129137748852372,"off"],[-1.01781038,9.596148490905762,0.002068936824798584,0.0021333808545023203,"off"],[-1.006138557,9.604955673217773,0.002068936824798584,0.002137752715498209,"off"],[-0.998782307,9.612268447875977,0.002068936824798584,0.0021404039580374956,"off"],[-0.985520225,9.621979713439941,0.0019488077377900481,0.002027125097811222,"off"],[-0.978543871,9.630132675170898,0.0019488077377900481,0.0020307633094489574,"off"],[-0.966583767,9.640236854553223,0.0019488077377900481,0.002036722144111991,"off"],[-0.959691007,9.648741722106934,0.0019488077377900481,0.002040088875219226,"off"],[-0.948935643,9.657269477844238,0.0019488077377900481,0.002045112196356058,"off"],[-0.93685981,9.66506290435791,0.0018986444920301437,0.002000626176595688,"off"],[-0.922847779,9.673314094543457,0.0018986444920301437,0.002007189905270934,"off"],[-0.913113821,9.680861473083496,0.0018986444920301437,0.0020114604849368334,"off"],[-0.899177988,9.688516616821289,0.0018986444920301437,0.0020175338722765446,"off"],[-0.887183509,9.696280479431152,0.0018177233869209886,0.0019426082726567984,"off"],[-0.878894291,9.703773498535156,0.0018177233869209886,0.0019467839738354087,"off"],[-0.871365489,9.714043617248535,0.0018177233869209886,0.001950466656126082,"off"],[-0.861746218,9.720457077026367,0.0018177233869209886,0.001954971347004175,"off"],[-0.850321011,9.727832794189453,0.0018177233869209886,0.001960250549018383,"off"],[-0.838593511,9.734979629516602,0.0017099581891670823,0.0018591153202578425,"off"],[-0.831558824,9.740410804748535,0.0017099581891670823,0.0018632520223036408,"off"],[-0.819161689,9.752582550048828,0.0017099581891670823,0.0018700269283726811,"off"],[-0.80967721,9.763052940368652,0.0017099581891670823,0.0018750852905213833,"off"],[-0.7911532,9.770406723022461,0.0016796451527625322,0.0018533248221501708,"off"],[-0.784057367,9.776379585266113,0.0016796451527625322,0.0018566339276731014,"off"],[-0.772712055,9.792817115783691,0.0016796451527625322,0.001861612661741674,"off"],[-0.769957159,9.792817115783691,0.0016796451527625322,0.0018628358375281096,"off"],[-0.760590024,9.802258491516113,0.0016796451527625322,0.0018667837139219046,"off"],[-0.747426535,9.81131362915039,0.001606699195690453,0.001872190972790122,"off"],[-0.735625181,9.821891784667969,0.001606699195690453,0.0018050129292532802,"off"],[-0.728537109,9.830842018127441,0.001606699195690453,0.001808521687053144,"off"],[-0.71166263,9.83940601348877,0.001606699195690453,0.0018164950888603926,"off"],[-0.704761641,9.847989082336426,0.001606699195690453,0.0018196383025497198,"off"],[-0.68545362,9.856589317321777,0.0015678014606237411,0.001788032939657569,"off"],[-0.675987527,9.871309280395508,0.0015678014606237411,0.001791593967936933,"off"],[-0.672648256,9.871309280395508,0.0015678014606237411,0.0017928138840943575,"off"],[-0.661106798,9.878275871276855,0.0015678014606237411,0.001796880504116416,"off"],[-0.654390497,9.887296676635742,0.0015678014606237411,0.0017992127686738968,"off"],[-0.636926122,9.894444465637207,0.0014876762870699167,0.0017239192966371775,"off"],[-0.62970956,9.900238037109375,0.0014876762870699167,0.00172593689057976,"off"],[-0.622217425,9.907814979553223,0.0014876762870699167,0.001727933413349092,"off"],[-0.603423623,9.917058944702148,0.0014876762870699167,0.0017327407840639353,"off"],[-0.594967895,9.928218841552734,0.0013428976526483893,0.0017348122783005238,"off"],[-0.58355128,9.936976432800293,0.0013428976526483893,0.0015934716211631894,"off"],[-0.576631593,9.945758819580078,0.0013428976526483893,0.001595608307980001,"off"],[-0.566408312,9.954561233520508,0.0013428976526483893,0.0015985977370291948,"off"],[-0.557382636,9.966401100158691,0.0013428976526483893,0.0016011545667424798,"off"],[-0.540005605,9.97461223602295,0.001342552830465138,0.001604835968464613,"off"],[-0.532857272,9.980206489562988,0.001342552830465138,0.0016065154923126101,"off"],[-0.521620293,9.998621940612793,0.001342552830465138,0.0016089777927845716,"off"],[-0.518639929,9.998621940612793,0.001342552830465138,0.0016096248291432858,"off"],[-0.500272013,10.007430076599121,0.001342552830465138,0.0016134511679410934,"off"],[-0.497368523,10.007430076599121,0.0012115883873775601,0.0016140614170581102,"off"],[-0.484097482,10.014728546142578,0.0012115883873775601,0.0014865415869280696,"off"],[-0.476344878,10.022899627685547,0.0012115883873775601,0.0014886093558743596,"off"],[-0.466688629,10.040205001831055,0.0012115883873775601,0.0014910026220604777,"off"],[-0.46413342,10.040205001831055,0.0012115883873775601,0.0014916775980964303,"off"],[-0.449672067,10.046225547790527,0.0012115883873775601,0.001495083444751799,"off"],[-0.442287015,10.054739952087402,0.0011751247802749276,0.0014601867878809571,"off"],[-0.430728786,10.062175750732422,0.0011751247802749276,0.0014627095079049468,"off"],[-0.423002536,10.070504188537598,0.0011751247802749276,0.0014643126633018255,"off"],[-0.413630818,10.077016830444336,0.0011751247802749276,0.0014662088360637426,"off"],[-0.390807486,10.086389541625977,0.001165187219157815,0.0014605282340198755,"off"],[-0.382832538,10.094257354736328,0.001165187219157815,0.0014620013535022736,"off"],[-0.373802955,10.10374927520752,0.001165187219157815,0.0014635813422501087,"off"],[-0.36423457,10.116558074951172,0.001165187219157815,0.0014652005629613996,"off"],[-0.36160181,10.116558074951172,0.001165187219157815,0.0014656656421720982,"off"],[-0.34450431,10.124984741210938,0.0011800421634688973,0.0014683930203318596,"off"],[-0.337284154,10.131991386413574,0.0011800421634688973,0.0014855185290798545,"off"],[-0.328372384,10.139951705932617,0.0011800421634688973,0.0014884024858474731,"off"],[-0.318057488,10.148043632507324,0.0011800421634688973,0.001491608447395265,"off"],[-0.308934937,10.156634330749512,0.0011800421634688973,0.0014943336136639118,"off"],[-0.287049104,10.166060447692871,0.0010856534354388714,0.0014037768123671412,"off"],[-0.279410927,10.174737930297852,0.0010856534354388714,0.0014050990575924516,"off"],[-0.270351084,10.182336807250977,0.0010856534354388714,0.0014065825380384922,"off"],[-0.261548063,10.189691543579102,0.0010856534354388714,0.001407983829267323,"off"],[-0.25489322,10.196070671081543,0.0010856534354388714,0.0014090121258050203,"off"],[-0.238314731,10.203069686889648,0.0010140164522454143,0.001342932926490903,"off"],[-0.230584054,10.211788177490234,0.0010140164522454143,0.0013455854495987296,"off"],[-0.222885617,10.220209121704102,0.0010140164522454143,0.0013480846537277102,"off"],[-0.210213169,10.233031272888184,0.0010140164522454143,0.0013520215870812535,"off"],[-0.207389211,10.233031272888184,0.0010140164522454143,0.0013529143761843443,"off"],[-0.18407916,10.243202209472656,0.0009111069957725704,0.0012581651099026203,"off"],[-0.181344056,10.243202209472656,0.0009111069957725704,0.0012592077255249023,"off"],[-0.172922442,10.251840591430664,0.0009111069957725704,0.0012620399938896298,"off"],[-0.164492077,10.260557174682617,0.0009111069957725704,0.0012648004340007901,"off"],[-0.154896921,10.266314506530762,0.0009111069957725704,0.0012678487692028284,"off"],[-0.143971714,10.275007247924805,0.0008783861994743347,0.0012712233001366258,"off"],[-0.135920828,10.284157752990723,0.0008783861994743347,0.0012384570436552167,"off"],[-0.127884006,10.291455268859863,0.0008783861994743347,0.0012384020956233144,"off"],[-0.120070673,10.300435066223145,0.0008783861994743347,0.0012383401626721025,"off"],[-0.110567288,10.308326721191406,0.0008783861994743347,0.0012382564600557089,"off"],[-0.09951208,10.317850112915039,0.0008783861994743347,0.0012381457490846515,"off"],[-0.077537081,10.327746391296387,0.000848320429213345,0.0012061273446306586,"off"],[-0.067703904,10.33608341217041,0.000848320429213345,0.0012052726233378053,"off"],[-0.060737498,10.343345642089844,0.000848320429213345,0.001204643165692687,"off"],[-0.04930979,10.351507186889648,0.000848320429213345,0.001203726977109909,"off"],[-0.037524843,10.355974197387695,0.0008376292535103858,0.0011924299178645015,"off"],[-0.026266614,10.370450019836426,0.0008376292535103858,0.0011918586678802967,"off"],[-0.022858124,10.370450019836426,0.0008376292535103858,0.0011916846269741654,"off"],[-0.013266406,10.378329277038574,0.0008376292535103858,0.001191218150779605,"off"],[-0.000321406,10.383715629577637,0.0008376292535103858,0.0011905917199328542,"off"],[0.023207134,10.399613380432129,0.0007683018920943141,0.0011220852611586452,"off"],[0.026741197,10.399613380432129,0.0007683018920943141,0.0011222186731174588,"off"],[0.038686353,10.416584968566895,0.0007683018920943141,0.0011225880589336157,"off"],[0.041458384,10.416584968566895,0.0007683018920943141,0.001122674671933055,"off"],[0.0602338,10.424744606018066,0.000771163497120142,0.0011244151974096894,"off"],[0.067198122,10.434165000915527,0.000771163497120142,0.0011239851592108607,"off"],[0.074093122,10.443611145019531,0.000771163497120142,0.0011235718848183751,"off"],[0.085880049,10.454198837280273,0.000771163497120142,0.0011228686198592186,"off"],[0.094653278,10.462425231933594,0.000771163497120142,0.0011223413748666644,"off"],[0.107663485,10.471853256225586,0.0007581986137665808,0.001108488766476512,"off"],[0.115558381,10.482011795043945,0.0007581986137665808,0.001107925083488226,"off"],[0.126221714,10.490943908691406,0.0007581986137665808,0.0011071934131905437,"off"],[0.136415828,10.496843338012695,0.0007581986137665808,0.0011065158760175109,"off"],[0.148651192,10.50334358215332,0.0007581986137665808,0.0011056974763050675,"off"],[0.160601869,10.510422706604004,0.0007571581518277526,0.001104686176404357,"off"],[0.1677239,10.519942283630371,0.0007571581518277526,0.0011046936269849539,"off"],[0.177550722,10.525673866271973,0.0007571581518277526,0.001104694907553494,"off"],[0.187418222,10.533571243286133,0.0007571581518277526,0.0011046846630051732,"off"],[0.198272336,10.541217803955078,0.0007571581518277526,0.0011046614963561296,"off"],[0.210722336,10.549391746520996,0.0007398101151920855,0.0010875484440475702,"off"],[0.217811137,10.556164741516113,0.0007398101151920855,0.001087678479962051,"off"],[0.230068064,10.565422058105469,0.0007398101151920855,0.0010878753382712603,"off"],[0.23981322,10.573201179504395,0.0007398101151920855,0.001088014803826809,"off"],[0.259360146,10.582629203796387,0.0007277733529917896,0.00107623974326998,"off"],[0.266410094,10.590556144714355,0.0007277733529917896,0.0010763260070234537,"off"],[0.276742854,10.602041244506836,0.0007277733529917896,0.001076425425708294,"off"],[0.279556812,10.602041244506836,0.0007277733529917896,0.0010764531325548887,"off"],[0.288178218,10.609926223754883,0.0007277733529917896,0.001076522166840732,"off"],[0.302526134,10.619858741760254,0.0006551926489919424,0.0010766108753159642,"off"],[0.31371478,10.63054370880127,0.0006551926489919424,0.0010051927529275417,"off"],[0.320923529,10.639633178710938,0.0006551926489919424,0.0010059368796646595,"off"],[0.337463268,10.648740768432617,0.0006551926489919424,0.0010075622703880072,"off"],[0.344303372,10.655210494995117,0.0006551926489919424,0.0010082063963636756,"off"],[0.358023893,10.663397789001465,0.0006367704481817782,0.0009910374647006392,"off"],[0.36540983,10.669785499572754,0.0006367704481817782,0.0009917308343574405,"off"],[0.374461548,10.677536010742188,0.0006367704481817782,0.0009925100021064281,"off"],[0.384553319,10.684292793273926,0.0006367704481817782,0.0009933494729921222,"off"],[0.393522329,10.691268920898438,0.0006367704481817782,0.0009940670570358634,"off"],[0.410985453,10.698457717895508,0.0005629346705973148,0.000919657526537776,"off"],[0.419780921,10.712831497192383,0.0005629346705973148,0.0009193398873321712,"off"],[0.422378682,10.712831497192383,0.0005629346705973148,0.0009192466386593878,"off"],[0.441293368,10.722553253173828,0.0005629346705973148,0.0009185867384076118,"off"],[0.443739202,10.722553253173828,0.0005629346705973148,0.000918495818041265,"off"],[0.457497899,10.72889232635498,0.0005409906152635813,0.0008952101343311369,"off"],[0.464716857,10.733986854553223,0.0005409906152635813,0.0008944681030698121,"off"],[0.471722378,10.738701820373535,0.0005409906152635813,0.000893789401743561,"off"],[0.483360138,10.751401901245117,0.0005409906152635813,0.0008926447480916977,"off"],[0.492465293,10.759773254394531,0.0005409906152635813,0.0008917624945752323,"off"],[0.503703418,10.768261909484863,0.000496625725645572,0.0008906858856789768,"off"],[0.514842115,10.777984619140625,0.000496625725645572,0.0008454126073047519,"off"],[0.526400292,10.792994499206543,0.000496625725645572,0.0008444267441518605,"off"],[0.529141229,10.792994499206543,0.000496625725645572,0.0008441995596513152,"off"],[0.546132323,10.799247741699219,0.000496625725645572,0.0008428272558376193,"off"],[0.558993155,10.805765151977539,0.0004573763580992818,0.0008034156053327024,"off"],[0.566026749,10.809895515441895,0.0004573763580992818,0.0008033181657083333,"off"],[0.575763467,10.816750526428223,0.0004573763580992818,0.0008031779434531927,"off"],[0.585330863,10.823123931884766,0.0004573763580992818,0.0008030342869460583,"off"],[0.595285862,10.828303337097168,0.0004573763580992818,0.0008028739248402417,"off"],[0.605527945,10.83541202545166,0.0004361376049928367,0.0008027007570490241,"off"],[0.617391018,10.842805862426758,0.0004361376049928367,0.0007809787639416754,"off"],[0.625750913,10.851972579956055,0.0004361376049928367,0.0007806113571859896,"off"],[0.635112475,10.860501289367676,0.0004361376049928367,0.0007802126929163933,"off"],[0.651461485,10.867669105529785,0.0004361376049928367,0.0007795080309733748,"off"],[0.663889037,10.873570442199707,0.00036963808815926313,0.0007083879318088293,"off"],[0.672736536,10.877551078796387,0.00036963808815926313,0.0007050898275338113,"off"],[0.682665025,10.883931159973145,0.00036963808815926313,0.0007015140145085752,"off"],[0.68957315,10.894062042236328,0.00036963808815926313,0.0006990841939114034,"off"],[0.706743254,10.902363777160645,0.0003448356583248824,0.000671250163577497,"off"],[0.714358045,10.909330368041992,0.0003448356583248824,0.000669867848046124,"off"],[0.724868201,10.929449081420898,0.0003448356583248824,0.0006680427468381822,"off"],[0.727638201,10.929449081420898,0.0003448356583248824,0.0006675519980490208,"off"],[0.737560336,10.937844276428223,0.0003448356583248824,0.0006658827769570053,"off"],[0.760111533,10.945216178894043,0.00033975805854424834,0.0006582504720427096,"off"],[0.769315543,10.953152656555176,0.00033975805854424834,0.0006572036654688418,"off"],[0.77707299,10.961612701416016,0.00033975805854424834,0.000656347197946161,"off"],[0.785393303,10.970930099487305,0.00033975805854424834,0.0006554300198331475,"off"],[0.792174969,10.978388786315918,0.00033975805854424834,0.0006547062075696886,"off"],[0.809478979,10.986008644104004,0.0003265798441134393,0.0006390917114913464,"off"],[0.816868926,10.996427536010742,0.0003265798441134393,0.0006380334962159395,"off"],[0.823930801,11.0045166015625,0.0003265798441134393,0.0006370648625306785,"off"],[0.834337207,11.010059356689453,0.0003265798441134393,0.0006356468074955046,"off"],[0.843863404,11.02043342590332,0.0003265798441134393,0.0006343746790662408,"off"],[0.864319862,11.02961540222168,0.00027988420333713293,0.0005863411934114993,"off"],[0.867077726,11.02961540222168,0.00027988420333713293,0.0005861453828401864,"off"],[0.875117986,11.037299156188965,0.00027988420333713293,0.0005856327479705215,"off"],[0.885177621,11.05247974395752,0.00027988420333713293,0.000584971159696579,"off"],[0.887685538,11.05247974395752,0.00027988420333713293,0.0005847992142662406,"off"],[0.901219027,11.063224792480469,0.00027988420333713293,0.0005839445511810482,"off"],[0.913095068,11.07310676574707,0.00024132616817951202,0.0005456620128825307,"off"],[0.919934339,11.081415176391602,0.00024132616817951202,0.0005458150408230722,"off"],[0.929576474,11.08901596069336,0.00024132616817951202,0.0005460081156343222,"off"],[0.93909314,11.095210075378418,0.00024132616817951202,0.0005461847176775336,"off"],[0.951018765,11.100872039794922,0.00024132616817951202,0.000546377501450479,"off"],[0.975018451,11.107574462890625,0.0002534966042730957,0.0005592423258349299,"off"],[0.97785793,11.107574462890625,0.0002534966042730957,0.0005593308014795184,"off"],[0.985595222,11.112992286682129,0.0002534966042730957,0.0005595323746092618,"off"],[0.996062929,11.120636940002441,0.0002534966042730957,0.0005597875569947064,"off"]],[[-5.995173445,8.456132888793945,-0.005459736101329327,-0.0010612092446535826,"laneChangeStarting"],[-5.984455894,8.435598373413086,-0.011575731448829174,-0.0011218524305149913,"laneChangeStarting"],[-5.976302637,8.41854476928711,-0.011575731448829174,-0.0011827414855360985,"laneChangeStarting"],[-5.957236727,8.388728141784668,-0.011575731448829174,-0.001244064187631011,"laneChangeStarting"],[-5.95512088,8.388728141784668,-0.011575731448829174,-0.0013053868897259235,"laneChangeStarting"],[-5.944616038,8.373734474182129,-0.011575731448829174,-0.0013669293839484453,"laneChangeStarting"],[-5.929461377,8.35866928100586,-0.013793042860925198,-0.001428693882189691,"laneChangeStarting"],[-5.912802742,8.343883514404297,-0.013793042860925198,-0.001490677474066615,"laneChangeStarting"],[-5.904587906,8.331594467163086,-0.013793042860925198,-0.001552844187244773,"laneChangeStarting"],[-5.897841827,8.318175315856934,-0.013793042860925198,-0.0016152114840224385,"laneChangeStarting"],[-5.885748043,8.304800987243652,-0.015654664486646652,-0.001677779946476221,"laneChangeStarting"],[-5.879353355,8.293701171875,-0.015654664486646652,-0.0017405159305781126,"laneChangeStarting"],[-5.862830171,8.28201961517334,-0.015654664486646652,-0.0018034289823845029,"laneChangeStarting"],[-5.85457371,8.270511627197266,-0.015654664486646652,-0.0018665172392502427,"laneChangeStarting"],[-5.847984702,8.258367538452148,-0.015654664486646652,-0.0019297911785542965,"laneChangeStarting"],[-5.839454682,8.247490882873535,-0.015654664486646652,-0.001993232173845172,"laneChangeStarting"],[-5.813444466,8.235864639282227,-0.018981682136654854,-0.002056852448731661,"laneChangeStarting"],[-5.803881738,8.213337898254395,-0.018981682136654854,-0.0021208219695836306,"laneChangeStarting"],[-5.801568393,8.213337898254395,-0.018981682136654854,-0.002184791723266244,"laneChangeStarting"],[-5.793461293,8.203167915344238,-0.018981682136654854,-0.0022489200346171856,"laneChangeStarting"],[-5.784092989,8.194347381591797,-0.02269803360104561,-0.0023131866473704576,"laneChangeStarting"],[-5.770822189,8.187870979309082,-0.02269803360104561,-0.002377554774284363,"laneChangeStarting"],[-5.755943379,8.183892250061035,-0.02269803360104561,-0.0024419857654720545,"laneChangeStarting"],[-5.753778509,8.183892250061035,-0.02269803360104561,-0.0025064165238291025,"laneChangeStarting"],[-5.744899235,8.17945671081543,-0.02269803360104561,-0.0025709171313792467,"laneChangeStarting"],[-5.733583199,8.174474716186523,-0.024185972288250923,-0.0026354966685175896,"laneChangeStarting"],[-5.709420064,8.165536880493164,-0.024185972288250923,-0.0027002173010259867,"laneChangeStarting"],[-5.706875724,8.165536880493164,-0.024185972288250923,-0.0027649381663650274,"laneChangeStarting"],[-5.698391184,8.150816917419434,-0.024185972288250923,-0.0028298930265009403,"laneChangeStarting"],[-5.696178751,8.150816917419434,-0.024185972288250923,-0.0028948478866368532,"laneChangeStarting"],[-5.685468076,8.147156715393066,-0.026145122945308685,-0.0029598609544336796,"laneChangeStarting"],[-5.678596288,8.143507957458496,-0.026145122945308685,-0.003024932462722063,"laneChangeStarting"],[-5.662223767,8.14057731628418,-0.026145122945308685,-0.003090050769969821,"laneChangeStarting"],[-5.654284574,8.139397621154785,-0.026145122945308685,-0.003155187936499715,"laneChangeStarting"],[-5.647861441,8.137140274047852,-0.026145122945308685,-0.003220361191779375,"laneChangeStarting"],[-5.639586799,8.131942749023438,-0.026145122945308685,-0.0032856178004294634,"laneChangeStarting"],[-5.627456756,8.128907203674316,-0.02797889895737171,-0.003350923303514719,"laneChangeStarting"],[-5.602937072,8.127093315124512,-0.02797889895737171,-0.0034162579104304314,"laneChangeStarting"],[-5.60075548,8.127093315124512,-0.02797889895737171,-0.0034815922845155,"laneChangeStarting"],[-5.594344381,8.1233549118042,-0.02797889895737171,-0.0035469871945679188,"laneChangeStarting"],[-5.58708635,8.1196928024292,-0.03198106959462166,-0.0036124407779425383,"laneChangeStarting"],[-5.580645452,8.116840362548828,-0.03198106959462166,-0.003677940461784601,"laneChangeStarting"],[-5.562504202,8.114337921142578,-0.03198106959462166,-0.00374348065815866,"laneChangeStarting"],[-5.553338606,8.108072280883789,-0.03198106959462166,-0.003809122135862708,"laneChangeStarting"],[-5.551314084,8.108072280883789,-0.03198106959462166,-0.0038747636135667562,"laneChangeStarting"],[-5.54315072,8.10651969909668,-0.03198106959462166,-0.003940430004149675,"laneChangeStarting"],[-5.533529904,8.104385375976562,-0.033202752470970154,-0.004006131552159786,"laneChangeStarting"],[-5.51718713,8.102066040039062,-0.033202752470970154,-0.004071870353072882,"laneChangeStarting"],[-5.497633701,8.101067543029785,-0.033202752470970154,-0.004137625452131033,"laneChangeStarting"],[-5.495344529,8.101067543029785,-0.033202752470970154,-0.004203380085527897,"laneChangeStarting"],[-5.488628874,8.100893020629883,-0.033202752470970154,-0.004269137978553772,"laneChangeStarting"],[-5.482078574,8.100318908691406,-0.03514854609966278,-0.004334905184805393,"laneChangeStarting"],[-5.469827042,8.098211288452148,-0.03514854609966278,-0.004400706849992275,"laneChangeStarting"],[-5.455619442,8.094117164611816,-0.03514854609966278,-0.004466574639081955,"laneChangeStarting"],[-5.45359268,8.094117164611816,-0.03514854609966278,-0.004532442893832922,"laneChangeStarting"],[-5.445200665,8.09332275390625,-0.03514854609966278,-0.004598323721438646,"laneChangeStarting"],[-5.434636434,8.093232154846191,-0.036553096026182175,-0.004664205946028233,"laneChangeStarting"],[-5.419681562,8.09188461303711,-0.036553096026182175,-0.00473011052235961,"laneChangeStarting"],[-5.407555009,8.09048843383789,-0.036553096026182175,-0.004796037450432777,"laneChangeStarting"],[-5.398174567,8.090550422668457,-0.036553096026182175,-0.004861963447183371,"laneChangeStarting"],[-5.386344078,8.091191291809082,-0.038286976516246796,-0.004927879199385643,"laneChangeStarting"],[-5.378136068,8.090079307556152,-0.038286976516246796,-0.00499381311237812,"laneChangeStarting"],[-5.36019987,8.089595794677734,-0.038286976516246796,-0.005059754941612482,"laneChangeStarting"],[-5.353010606,8.09044075012207,-0.038286976516246796,-0.0051256828010082245,"laneChangeStarting"],[-5.346933081,8.091744422912598,-0.038286976516246796,-0.005191589239984751,"laneChangeStarting"],[-5.338538669,8.093070030212402,-0.038913656026124954,-0.005257474258542061,"laneChangeStarting"],[-5.329553431,8.09288215637207,-0.038913656026124954,-0.005323362536728382,"laneChangeStarting"],[-5.310801454,8.090899467468262,-0.038913656026124954,-0.005389282945543528,"laneChangeStarting"],[-5.304297311,8.090279579162598,-0.038913656026124954,-0.0054552131332457066,"laneChangeStarting"],[-5.292190764,8.089794158935547,-0.038913656026124954,-0.005521151702851057,"laneChangeStarting"],[-5.282728061,8.090184211730957,-0.03990062698721886,-0.005587083753198385,"laneChangeStarting"],[-5.27410234,8.089866638183594,-0.03990062698721886,-0.005653020925819874,"laneChangeStarting"],[-5.256272732,8.10256290435791,-0.03990062698721886,-0.005718751810491085,"laneChangeStarting"],[-5.254124585,8.10256290435791,-0.03990062698721886,-0.005784482695162296,"laneChangeStarting"],[-5.245825927,8.106311798095703,-0.03990062698721886,-0.00585015257820487,"laneChangeStarting"],[-5.235671955,8.107065200805664,-0.0403151772916317,-0.005915810354053974,"laneChangeStarting"],[-5.21846813,8.108805656433105,-0.0403151772916317,-0.005981439724564552,"laneChangeStarting"],[-5.210650583,8.11032485961914,-0.0403151772916317,-0.0060470448806881905,"laneChangeStarting"],[-5.200303125,8.111254692077637,-0.0403151772916317,-0.0061126346699893475,"laneChangeStarting"],[-5.18804904,8.113147735595703,-0.0417521707713604,-0.0061781941913068295,"laneChangeStarting"],[-5.176539883,8.112906455993652,-0.0417521707713604,-0.00624375743791461,"laneChangeStarting"],[-5.162380317,8.114845275878906,-0.0417521707713604,-0.006309289485216141,"laneChangeStarting"],[-5.15311011,8.116162300109863,-0.0417521707713604,-0.006374800112098455,"laneChangeStarting"],[-5.146466192,8.11954116821289,-0.0417521707713604,-0.006440256256610155,"laneChangeStarting"],[-5.137805411,8.120284080505371,-0.0410095639526844,-0.006505700293928385,"laneChangeStarting"],[-5.129331916,8.123109817504883,-0.0410095639526844,-0.006571099162101746,"laneChangeStarting"],[-5.111423538,8.125972747802734,-0.0410095639526844,-0.006636451464146376,"laneChangeStarting"],[-5.103879706,8.129231452941895,-0.0410095639526844,-0.006701751612126827,"laneChangeStarting"],[-5.097506169,8.131710052490234,-0.0410095639526844,-0.006767012178897858,"laneChangeStarting"],[-5.083109146,8.135340690612793,-0.041468992829322815,-0.006832214538007975,"laneChangeStarting"],[-5.068903631,8.138138771057129,-0.041468992829322815,-0.006897371727973223,"laneChangeStarting"],[-5.061232787,8.140530586242676,-0.041468992829322815,-0.006962490733712912,"laneChangeStarting"],[-5.04877042,8.142743110656738,-0.041468992829322815,-0.007027574349194765,"laneChangeStarting"],[-5.046520216,8.142743110656738,-0.041468992829322815,-0.007092657964676619,"laneChangeStarting"],[-5.036149836,8.144569396972656,-0.041226357221603394,-0.007157712709158659,"laneChangeStarting"],[-5.023735555,8.147582054138184,-0.041226357221603394,-0.007222719024866819,"laneChangeStarting"],[-5.007285514,8.151653289794922,-0.041226357221603394,-0.007287660613656044,"laneChangeStarting"],[-5.004900484,8.151653289794922,-0.041226357221603394,-0.007352601736783981,"laneChangeStarting"],[-4.996496382,8.154679298400879,-0.041226357221603394,-0.007417495362460613,"laneChangeStarting"],[-4.976740253,8.159920692443848,-0.04301738739013672,-0.00748230516910553,"laneChangeStarting"],[-4.974510836,8.159920692443848,-0.04301738739013672,-0.007547114975750446,"laneChangeStarting"],[-4.961739438,8.165305137634277,-0.04301738739013672,-0.0076118395663797855,"laneChangeStarting"],[-4.953323722,8.171170234680176,-0.04301738739013672,-0.007676471024751663,"laneChangeStarting"],[-4.946907989,8.175562858581543,-0.04301738739013672,-0.007741033099591732,"laneChangeStarting"],[-4.93480155,8.18121337890625,-0.04306101053953171,-0.007805506233125925,"laneChangeStarting"],[-4.927902571,8.186121940612793,-0.04306101053953171,-0.007869902066886425,"laneChangeStarting"],[-4.911823724,8.185790061950684,-0.04306101053953171,-0.007934303022921085,"laneChangeStarting"],[-4.902788997,8.18779468536377,-0.04306101053953171,-0.007998672313988209,"laneChangeStarting"],[-4.896349508,8.19009780883789,-0.04306101053953171,-0.008063005283474922,"laneChangeStarting"],[-4.887819857,8.187774658203125,-0.0430772565305233,-0.00812737550586462,"laneChangeStarting"],[-4.861802931,8.191773414611816,-0.0430772565305233,-0.008191682398319244,"laneChangeStarting"],[-4.85935872,8.191773414611816,-0.0430772565305233,-0.008255989290773869,"laneChangeStarting"],[-4.85249303,8.195600509643555,-0.0430772565305233,-0.008320235647261143,"laneChangeStarting"],[-4.84567553,8.201521873474121,-0.0430772565305233,-0.00838438980281353,"laneChangeStarting"],[-4.839035417,8.206717491149902,-0.0430772565305233,-0.008448462933301926,"laneChangeStarting"],[-4.830296859,8.206305503845215,-0.04370081424713135,-0.008512542583048344,"laneChangeStarting"],[-4.817809595,8.207505226135254,-0.04370081424713135,-0.008576602675020695,"laneChangeStarting"],[-4.804071964,8.21287727355957,-0.04370081424713135,-0.008640579879283905,"laneChangeStarting"],[-4.801939499,8.21287727355957,-0.04370081424713135,-0.008704557083547115,"laneChangeStarting"],[-4.793480438,8.215035438537598,-0.04370081424713135,-0.008768499828875065,"laneChangeStarting"],[-4.782565133,8.216385841369629,-0.04489579051733017,-0.008832422085106373,"laneChangeStarting"],[-4.75753382,8.220019340515137,-0.04489579051733017,-0.00889628753066063,"laneChangeStarting"],[-4.755237512,8.220019340515137,-0.04489579051733017,-0.00896015390753746,"laneChangeStarting"],[-4.74847539,8.223125457763672,-0.04489579051733017,-0.009023970924317837,"laneChangeStarting"],[-4.741821836,8.223845481872559,-0.04489579051733017,-0.009087777696549892,"laneChangeStarting"],[-4.735231581,8.222363471984863,-0.044612109661102295,-0.009151606820523739,"laneChangeStarting"],[-4.726850821,8.224915504455566,-0.044612109661102295,-0.009215396828949451,"laneChangeStarting"],[-4.707830138,8.227818489074707,-0.044612109661102295,-0.009279141202569008,"laneChangeStarting"],[-4.701336052,8.230287551879883,-0.044612109661102295,-0.009342847391963005,"laneChangeStarting"],[-4.695040871,8.234615325927734,-0.044612109661102295,-0.009406487457454205,"laneChangeStarting"],[-4.686382437,8.236876487731934,-0.04342251271009445,-0.009470092132687569,"laneChangeStarting"],[-4.671686283,8.239480972290039,-0.04342251271009445,-0.00953365582972765,"laneChangeStarting"],[-4.651599602,8.24051570892334,-0.04342251271009445,-0.009597204625606537,"laneChangeStarting"],[-4.649390868,8.24051570892334,-0.04342251271009445,-0.00966075249016285,"laneChangeStarting"],[-4.642998787,8.240856170654297,-0.04342251271009445,-0.009724295698106289,"laneChangeStarting"],[-4.636394986,8.241705894470215,-0.044332846999168396,-0.009787825867533684,"laneChangeStarting"],[-4.627951659,8.243795394897461,-0.044332846999168396,-0.009851324371993542,"laneChangeStarting"],[-4.61134471,8.246663093566895,-0.044332846999168396,-0.009914777241647243,"laneChangeStarting"],[-4.603372855,8.249490737915039,-0.044332846999168396,-0.009978188201785088,"laneChangeStarting"],[-4.597137168,8.253406524658203,-0.044332846999168396,-0.010041537694633007,"laneChangeStarting"],[-4.58648495,8.25492000579834,-0.04549425467848778,-0.010104864835739136,"laneChangeStarting"],[-4.578223231,8.257224082946777,-0.04549425467848778,-0.010168156586587429,"laneChangeStarting"],[-4.559516117,8.2598876953125,-0.04549425467848778,-0.01023140735924244,"laneChangeStarting"],[-4.55733588,8.2598876953125,-0.04549425467848778,-0.010294657200574875,"laneChangeStarting"],[-4.546776081,8.262503623962402,-0.04549425467848778,-0.010357867926359177,"laneChangeStarting"],[-4.538546662,8.2650728225708,-0.045776087790727615,-0.010421039536595345,"laneChangeStarting"],[-4.53008036,8.266084671020508,-0.045776087790727615,-0.010484195314347744,"laneChangeStarting"],[-4.51056648,8.27019214630127,-0.045776087790727615,-0.010547288693487644,"laneChangeStarting"],[-4.503598993,8.274519920349121,-0.045776087790727615,-0.010610315948724747,"laneChangeStarting"],[-4.496140966,8.277522087097168,-0.045776087790727615,-0.010673297569155693,"laneChangeStarting"],[-4.48473361,8.280797004699707,-0.04524923488497734,-0.01073622889816761,"laneChangeStarting"],[-4.478299591,8.284318923950195,-0.04524923488497734,-0.010799107141792774,"laneChangeStarting"],[-4.462124314,8.288818359375,-0.04524923488497734,-0.010861917398869991,"laneChangeStarting"],[-4.454829714,8.294589042663574,-0.04524923488497734,-0.01092463918030262,"laneChangeStarting"],[-4.440253798,8.298491477966309,-0.04524923488497734,-0.010987303219735622,"laneChangeStarting"],[-4.429520778,8.302915573120117,-0.04507872089743614,-0.011049900203943253,"laneChangeStarting"],[-4.419111695,8.30705738067627,-0.04507872089743614,-0.011112434789538383,"laneChangeStarting"],[-4.406638288,8.310928344726562,-0.04507872089743614,-0.011174910701811314,"laneChangeStarting"],[-4.40049476,8.311898231506348,-0.04507872089743614,-0.01123737171292305,"laneChangeStarting"],[-4.391401373,8.315104484558105,-0.04507872089743614,-0.01129978522658348,"laneChangeStarting"],[-4.382766486,8.315858840942383,-0.04518866166472435,-0.011362187564373016,"laneChangeStarting"],[-4.370354762,8.316999435424805,-0.04518866166472435,-0.011424572207033634,"laneChangeStarting"],[-4.356542789,8.320785522460938,-0.04518866166472435,-0.011486900970339775,"laneChangeStarting"],[-4.349831148,8.322114944458008,-0.04518866166472435,-0.011549209244549274,"laneChangeStarting"],[-4.338030203,8.323060989379883,-0.045302290469408035,-0.011611503548920155,"laneChangeStarting"],[-4.326847956,8.32291316986084,-0.045302290469408035,-0.011673799715936184,"laneChangeStarting"],[-4.315971775,8.324040412902832,-0.045302290469408035,-0.01173607911914587,"laneChangeStarting"],[-4.301511305,8.326062202453613,-0.045302290469408035,-0.011798328720033169,"laneChangeStarting"],[-4.299207235,8.326062202453613,-0.045302290469408035,-0.011860577389597893,"laneChangeStarting"],[-4.287038749,8.325164794921875,-0.045733049511909485,-0.011922840029001236,"laneChangeStarting"],[-4.278868459,8.32603645324707,-0.045733049511909485,-0.011985089629888535,"laneChangeStarting"],[-4.262077245,8.326298713684082,-0.045733049511909485,-0.012047335505485535,"laneChangeStarting"],[-4.25373165,8.328272819519043,-0.045733049511909485,-0.012109551578760147,"laneChangeStarting"],[-4.247352021,8.331835746765137,-0.045733049511909485,-0.012171714566648006,"laneChangeStarting"],[-4.232505516,8.331572532653809,-0.044853515923023224,-0.012233882211148739,"laneChangeStarting"],[-4.216136387,8.32918643951416,-0.044853515923023224,-0.012296084314584732,"laneChangeStarting"],[-4.213894936,8.32918643951416,-0.044853515923023224,-0.0123582873493433,"laneChangeStarting"],[-4.202773329,8.327560424804688,-0.044853515923023224,-0.012420513667166233,"laneChangeStarting"],[-4.193412323,8.327473640441895,-0.044853515923023224,-0.012482741847634315,"laneChangeStarting"],[-4.184779103,8.32731819152832,-0.04437187314033508,-0.012544972822070122,"laneChangeStarting"],[-4.175980686,8.325972557067871,-0.04437187314033508,-0.012607223354279995,"laneChangeStarting"],[-4.156562247,8.324673652648926,-0.04437187314033508,-0.012669493444263935,"laneChangeStarting"],[-4.154251091,8.324673652648926,-0.04437187314033508,-0.012731763534247875,"laneChangeStarting"],[-4.147121793,8.323434829711914,-0.04437187314033508,-0.012794052250683308,"laneChangeStarting"],[-4.140528828,8.32264518737793,-0.04437187314033508,-0.012856352142989635,"laneChangeStarting"],[-4.130651337,8.318127632141113,-0.04369904100894928,-0.01291872002184391,"laneChangeStarting"],[-4.117802836,8.315850257873535,-0.04369904100894928,-0.012981122359633446,"laneChangeStarting"],[-4.105600331,8.314167022705078,-0.04369904100894928,-0.013043549843132496,"laneChangeStarting"],[-4.099368708,8.31116008758545,-0.04369904100894928,-0.013106022961437702,"laneChangeStarting"],[-4.091095579,8.308075904846191,-0.04369904100894928,-0.013168541714549065,"laneChangeStarting"],[-4.082623547,8.303054809570312,-0.045277584344148636,-0.013231136836111546,"laneChangeStarting"],[-4.067545213,8.300045013427734,-0.045277584344148636,-0.013293776661157608,"laneChangeStarting"],[-4.053319646,8.300890922546387,-0.045277584344148636,-0.0133564043790102,"laneChangeStarting"],[-4.051259334,8.300890922546387,-0.045277584344148636,-0.013419031165540218,"laneChangeStarting"],[-4.042642005,8.299988746643066,-0.045277584344148636,-0.013481671921908855,"laneChangeStarting"],[-4.024829804,8.301166534423828,-0.04615038260817528,-0.01354429591447115,"laneChangeStarting"],[-4.022468114,8.301166534423828,-0.04615038260817528,-0.013606918975710869,"laneChangeStarting"],[-4.004969326,8.30354118347168,-0.04615038260817528,-0.013669505715370178,"laneChangeStarting"],[-4.002731157,8.30354118347168,-0.04615038260817528,-0.013732093386352062,"laneChangeStarting"],[-3.994624165,8.30434513092041,-0.04615038260817528,-0.013794668950140476,"laneChangeStarting"],[-3.985704939,8.307032585144043,-0.045576173812150955,-0.013857203535735607,"laneChangeStarting"],[-3.97524517,8.310685157775879,-0.045576173812150955,-0.013919683173298836,"laneChangeStarting"],[-3.960676449,8.312556266784668,-0.045576173812150955,-0.013982134871184826,"laneChangeStarting"],[-3.954240818,8.313883781433105,-0.045576173812150955,-0.014044567011296749,"laneChangeStarting"],[-3.945558475,8.31242847442627,-0.045576173812150955,-0.014107020571827888,"laneChangeStarting"],[-3.933577438,8.312556266784668,-0.045698054134845734,-0.014169472269713879,"laneChangeStarting"],[-3.905844256,8.312271118164062,-0.045698054134845734,-0.014231927692890167,"laneChangeStarting"],[-3.898387847,8.311983108520508,-0.045698054134845734,-0.014294388704001904,"laneChangeStarting"],[-3.891497674,8.31472110748291,-0.045698054134845734,-0.014356807805597782,"laneChangeStarting"],[-3.88494368,8.317229270935059,-0.04694944620132446,-0.014419188722968102,"laneChangeStarting"],[-3.873414006,8.322145462036133,-0.04694944620132446,-0.0144814969971776,"laneChangeStarting"],[-3.86251199,8.326617240905762,-0.04694944620132446,-0.014543738216161728,"laneChangeStarting"],[-3.851427375,8.327240943908691,-0.04694944620132446,-0.014605969190597534,"laneChangeStarting"],[-3.844911099,8.32768726348877,-0.04694944620132446,-0.014668194577097893,"laneChangeStarting"],[-3.83644006,8.326087951660156,-0.04740491881966591,-0.014730443246662617,"laneChangeStarting"],[-3.821845968,8.327523231506348,-0.04740491881966591,-0.014792670495808125,"laneChangeStarting"],[-3.802327563,8.329133987426758,-0.04740491881966591,-0.014854874461889267,"laneChangeStarting"],[-3.800086218,8.329133987426758,-0.04740491881966591,-0.014917077496647835,"laneChangeStarting"],[-3.79325085,8.327880859375,-0.04740491881966591,-0.014979299157857895,"laneChangeStarting"],[-3.78638683,8.329648971557617,-0.04544461518526077,-0.01504149567335844,"laneChangeStarting"],[-3.778063328,8.3304443359375,-0.04544461518526077,-0.015103679150342941,"laneChangeStarting"],[-3.759950485,8.329957962036133,-0.04544461518526077,-0.015165870077908039,"laneChangeStarting"],[-3.752793944,8.333579063415527,-0.04544461518526077,-0.01522800698876381,"laneChangeStarting"],[-3.744959836,8.337596893310547,-0.04544461518526077,-0.015290084294974804,"laneChangeStarting"],[-3.738262941,8.341577529907227,-0.04544461518526077,-0.015352101996541023,"laneChangeStarting"],[-3.729826285,8.33907413482666,-0.0464947335422039,-0.015414156951010227,"laneChangeStarting"],[-3.714663456,8.341906547546387,-0.0464947335422039,-0.015476169995963573,"laneChangeStarting"],[-3.695318481,8.358911514282227,-0.0464947335422039,-0.015537931583821774,"laneChangeStarting"],[-3.693011285,8.358911514282227,-0.0464947335422039,-0.015599692240357399,"laneChangeStarting"],[-3.683031741,8.363520622253418,-0.04639854654669762,-0.015661384910345078,"laneChangeStarting"],[-3.669839408,8.364494323730469,-0.04639854654669762,-0.01572306454181671,"laneChangeStarting"],[-3.656286204,8.368898391723633,-0.04639854654669762,-0.015784677118062973,"laneChangeStarting"],[-3.647947124,8.373080253601074,-0.04639854654669762,-0.01584623008966446,"laneChangeStarting"],[-3.64129696,8.377798080444336,-0.04639854654669762,-0.015907712280750275,"laneChangeStarting"],[-3.632729178,8.385641098022461,-0.04713290557265282,-0.015969080850481987,"laneChangeStarting"],[-3.619234217,8.392568588256836,-0.04713290557265282,-0.01603034697473049,"laneChangeStarting"],[-3.603845133,8.397100448608398,-0.04713290557265282,-0.016091547906398773,"laneChangeStarting"],[-3.590531468,8.397504806518555,-0.04713290557265282,-0.016152741387486458,"laneChangeStarting"],[-3.581884812,8.401656150817871,-0.04588466137647629,-0.016213875263929367,"laneChangeStarting"],[-3.570351855,8.40929126739502,-0.04588466137647629,-0.01627489924430847,"laneChangeStarting"],[-3.559309127,8.423152923583984,-0.04588466137647629,-0.01633572205901146,"laneChangeStarting"],[-3.547729335,8.432138442993164,-0.04588466137647629,-0.016396414488554,"laneChangeStarting"],[-3.541316471,8.429366111755371,-0.04588466137647629,-0.016457147896289825,"laneChangeStarting"],[-3.532844858,8.431589126586914,-0.047259386628866196,-0.0165180042386055,"preLaneChange"],[-3.520391201,8.437732696533203,-0.047259386628866196,-0.016579078510403633,"preLaneChange"],[-3.505803569,8.44637680053711,-0.047259386628866196,-0.016640475019812584,"preLaneChange"],[-3.499095734,8.458818435668945,-0.047259386628866196,-0.01670226827263832,"preLaneChange"],[-3.489296808,8.472078323364258,-0.047259386628866196,-0.016764571890234947,"preLaneChange"],[-3.479834374,8.482623100280762,-0.046704646199941635,-0.016824545338749886,"laneChangeStarting"],[-3.462176906,8.489465713500977,-0.046704646199941635,-0.016884420067071915,"laneChangeStarting"],[-3.454367388,8.493234634399414,-0.046704646199941635,-0.016944242641329765,"laneChangeStarting"],[-3.447756087,8.49719524383545,-0.046704646199941635,-0.01700401119887829,"laneChangeStarting"],[-3.439511876,8.504758834838867,-0.046704646199941635,-0.017063671723008156,"laneChangeStarting"],[-3.429797555,8.509998321533203,-0.04693152755498886,-0.017123259603977203,"laneChangeStarting"],[-3.415989963,8.515728950500488,-0.04693152755498886,-0.017182765528559685,"laneChangeStarting"],[-3.403868733,8.523811340332031,-0.04693152755498886,-0.01724216155707836,"laneChangeStarting"],[-3.397583661,8.531420707702637,-0.04693152755498886,-0.017301449552178383,"laneChangeStarting"],[-3.380533013,8.53215503692627,-0.043906163424253464,-0.017360728234052658,"laneChangeStarting"],[-3.363369316,8.528057098388672,-0.043906163424253464,-0.01742006279528141,"laneChangeStarting"],[-3.354998093,8.52900218963623,-0.043906163424253464,-0.017479384317994118,"laneChangeStarting"],[-3.347807429,8.529396057128906,-0.043906163424253464,-0.017538702115416527,"laneChangeStarting"],[-3.341339602,8.529319763183594,-0.043906163424253464,-0.017598019912838936,"laneChangeStarting"],[-3.332851839,8.533380508422852,-0.04389306157827377,-0.01765727996826172,"laneChangeStarting"],[-3.321486582,8.530715942382812,-0.04389306157827377,-0.017716579139232635,"laneChangeStarting"],[-3.305125222,8.532776832580566,-0.04389306157827377,-0.017775848507881165,"laneChangeStarting"],[-3.302892368,8.532776832580566,-0.04389306157827377,-0.017835117876529694,"laneChangeStarting"],[-3.293951314,8.534509658813477,-0.04389306157827377,-0.017894364893436432,"laneChangeStarting"],[-3.278266271,8.541501998901367,-0.04334402084350586,-0.01795351319015026,"laneChangeStarting"],[-3.261893502,8.540129661560059,-0.04334402084350586,-0.018012680113315582,"laneChangeStarting"],[-3.250249268,8.550081253051758,-0.04334402084350586,-0.01807171106338501,"laneChangeStarting"],[-3.247941551,8.550081253051758,-0.04334402084350586,-0.018130740150809288,"laneChangeStarting"],[-3.239578195,8.550613403320312,-0.04334402084350586,-0.01818976365029812,"laneChangeStarting"],[-3.230089139,8.552693367004395,-0.044680483639240265,-0.01824875734746456,"laneChangeStarting"],[-3.21167919,8.534693717956543,-0.044680483639240265,-0.018308000639081,"laneChangeStarting"],[-3.204194597,8.522753715515137,-0.044680483639240265,-0.018367409706115723,"laneChangeStarting"],[-3.197753079,8.525651931762695,-0.044680483639240265,-0.01842677779495716,"laneChangeStarting"],[-3.184507973,8.538935661315918,-0.04133283346891403,-0.018485963344573975,"laneChangeStarting"],[-3.166490623,8.53493881225586,-0.04133283346891403,-0.018545202910900116,"laneChangeStarting"],[-3.163644021,8.53493881225586,-0.04133283346891403,-0.018604442477226257,"laneChangeStarting"],[-3.148845138,8.512775421142578,-0.04133283346891403,-0.01866399124264717,"laneChangeStarting"],[-3.146635571,8.512775421142578,-0.04133283346891403,-0.018723538145422935,"laneChangeStarting"],[-3.13822189,8.500889778137207,-0.04133283346891403,-0.01878325454890728,"laneChangeStarting"],[-3.127807184,8.48969841003418,-0.04243046045303345,-0.01884312741458416,"laneChangeStarting"],[-3.118130633,8.481507301330566,-0.04243046045303345,-0.018903115764260292,"laneChangeStarting"],[-3.102935921,8.467665672302246,-0.04243046045303345,-0.018963299691677094,"laneChangeStarting"],[-3.10062164,8.467665672302246,-0.04243046045303345,-0.019023485481739044,"laneChangeStarting"],[-3.091869436,8.46034049987793,-0.04243046045303345,-0.019083773717284203,"laneChangeStarting"],[-3.081763037,8.453250885009766,-0.04083746299147606,-0.01914416439831257,"laneChangeStarting"],[-3.056551636,8.44375991821289,-0.04083746299147606,-0.01920468918979168,"laneChangeStarting"],[-3.054030637,8.44375991821289,-0.04083746299147606,-0.01926521584391594,"laneChangeStarting"],[-3.045827738,8.435480117797852,-0.04083746299147606,-0.0193258598446846,"laneChangeStarting"],[-3.039017429,8.42306900024414,-0.04083746299147606,-0.019386684522032738,"laneChangeStarting"],[-3.03182093,8.409900665283203,-0.041491035372018814,-0.019447699189186096,"laneChangeStarting"],[-3.025400927,8.400630950927734,-0.041491035372018814,-0.0195088479667902,"laneChangeStarting"],[-3.005319985,8.375279426574707,-0.041491035372018814,-0.019570367410779,"laneChangeStarting"],[-3.00306796,8.375279426574707,-0.041491035372018814,-0.0196318868547678,"laneChangeStarting"],[-2.994951645,8.364765167236328,-0.041491035372018814,-0.019693560898303986,"laneChangeStarting"],[-2.986076288,8.356609344482422,-0.039992086589336395,-0.019755356013774872,"laneChangeStarting"],[-2.972412436,8.345735549926758,-0.039992086589336395,-0.01981731317937374,"laneChangeStarting"],[-2.949672291,8.328203201293945,-0.039992086589336395,-0.019879529252648354,"laneChangeStarting"],[-2.947464861,8.328203201293945,-0.039992086589336395,-0.019941747188568115,"laneChangeStarting"],[-2.939801528,8.312868118286133,-0.039992086589336395,-0.020004194229841232,"laneChangeStarting"],[-2.932532149,8.303038597106934,-0.040694233030080795,-0.02006678842008114,"laneChangeStarting"],[-2.925881362,8.282470703125,-0.040694233030080795,-0.020129695534706116,"laneChangeStarting"],[-2.914784405,8.273160934448242,-0.040694233030080795,-0.020192744210362434,"laneChangeStarting"],[-2.901923046,8.25334358215332,-0.040694233030080795,-0.02025609463453293,"laneChangeStarting"],[-2.899748281,8.25334358215332,-0.040694233030080795,-0.020319445058703423,"laneChangeStarting"],[-2.888833974,8.255122184753418,-0.040694233030080795,-0.020382769405841827,"laneChangeStarting"],[-2.880326106,8.24455451965332,-0.038863588124513626,-0.020446255803108215,"laneChangeStarting"],[-2.864906916,8.24856185913086,-0.038863588124513626,-0.02050967887043953,"laneChangeStarting"],[-2.844516529,8.232389450073242,-0.038863588124513626,-0.02057335339486599,"laneChangeStarting"],[-2.842214596,8.232389450073242,-0.038863588124513626,-0.02063702791929245,"laneChangeStarting"],[-2.83415929,8.22016716003418,-0.038216181099414825,-0.020700890570878983,"laneChangeStarting"],[-2.827552993,8.212788581848145,-0.038216181099414825,-0.020764868706464767,"laneChangeStarting"],[-2.816404149,8.21821117401123,-0.038216181099414825,-0.020828763023018837,"laneChangeStarting"],[-2.802773482,8.229805946350098,-0.038216181099414825,-0.02089247666299343,"laneChangeStarting"],[-2.800623931,8.229805946350098,-0.038216181099414825,-0.020956190302968025,"laneChangeStarting"],[-2.790724671,8.233421325683594,-0.038216181099414825,-0.021019848063588142,"laneChangeStarting"],[-2.781272034,8.235372543334961,-0.03709350526332855,-0.021083476021885872,"laneChangeStarting"],[-2.766707018,8.240997314453125,-0.03709350526332855,-0.021147018298506737,"laneChangeStarting"],[-2.755099256,8.252969741821289,-0.03709350526332855,-0.02121037431061268,"laneChangeStarting"],[-2.741937978,8.263904571533203,-0.03709350526332855,-0.02127356454730034,"laneChangeStarting"],[-2.73320922,8.274503707885742,-0.03758149594068527,-0.021336590871214867,"laneChangeStarting"],[-2.724972931,8.2843017578125,-0.03758149594068527,-0.021399470046162605,"laneChangeStarting"],[-2.708283891,8.293262481689453,-0.03758149594068527,-0.0214622113853693,"laneChangeStarting"],[-2.700613264,8.301368713378906,-0.03758149594068527,-0.021524831652641296,"laneChangeStarting"],[-2.694281985,8.311264991760254,-0.03758149594068527,-0.021587302908301353,"laneChangeStarting"],[-2.685289307,8.322757720947266,-0.03474832698702812,-0.02164960280060768,"laneChangeStarting"],[-2.675488041,8.331501007080078,-0.03474832698702812,-0.021711770445108414,"laneChangeStarting"],[-2.658365403,8.34179401397705,-0.03474832698702812,-0.02177378535270691,"laneChangeStarting"],[-2.651740977,8.350830078125,-0.03474832698702812,-0.02183566614985466,"laneChangeStarting"],[-2.640212349,8.361679077148438,-0.03474832698702812,-0.021897384896874428,"laneChangeStarting"],[-2.629219689,8.367741584777832,-0.03429945558309555,-0.02195901609957218,"laneChangeStarting"],[-2.618309498,8.37501335144043,-0.03429945558309555,-0.02202053926885128,"laneChangeStarting"],[-2.604624286,8.379999160766602,-0.03429945558309555,-0.02208198979496956,"laneChangeStarting"],[-2.597861284,8.378713607788086,-0.03429945558309555,-0.02214345894753933,"laneChangeStarting"],[-2.589594361,8.384881973266602,-0.03429945558309555,-0.022204838693141937,"laneChangeStarting"],[-2.581026165,8.387784957885742,-0.03277565538883209,-0.022266175597906113,"laneChangeStarting"],[-2.568364597,8.390317916870117,-0.03277565538883209,-0.022327475249767303,"laneChangeStarting"],[-2.554761749,8.388754844665527,-0.03277565538883209,-0.022388797253370285,"laneChangeStarting"],[-2.548057199,8.388731002807617,-0.03277565538883209,-0.022450119256973267,"laneChangeStarting"],[-2.535799117,8.384140014648438,-0.032785769551992416,-0.02251151017844677,"laneChangeStarting"],[-2.524116336,8.385204315185547,-0.032785769551992416,-0.02257288433611393,"laneChangeStarting"],[-2.509409201,8.383243560791016,-0.032785769551992416,-0.022634286433458328,"laneChangeStarting"],[-2.500528166,8.385692596435547,-0.032785769551992416,-0.02269565314054489,"laneChangeStarting"],[-2.492168928,8.388092994689941,-0.032785769551992416,-0.02275698632001877,"laneChangeStarting"],[-2.485461825,8.380249977111816,-0.030667036771774292,-0.02281843312084675,"laneChangeStarting"],[-2.477290031,8.373546600341797,-0.030667036771774292,-0.022879978641867638,"laneChangeStarting"],[-2.459654086,8.370253562927246,-0.030667036771774292,-0.022941570729017258,"laneChangeStarting"],[-2.451913338,8.371310234069824,-0.030667036771774292,-0.023003149777650833,"laneChangeStarting"],[-2.445680469,8.371875762939453,-0.030667036771774292,-0.023064719513058662,"laneChangeStarting"],[-2.430748381,8.37160587310791,-0.03047393262386322,-0.02312629297375679,"laneChangeStarting"],[-2.417545166,8.371697425842285,-0.03047393262386322,-0.023187866434454918,"laneChangeStarting"],[-2.408723937,8.371750831604004,-0.03047393262386322,-0.023249438032507896,"laneChangeStarting"],[-2.398235727,8.372147560119629,-0.03047393262386322,-0.023311004042625427,"laneChangeStarting"],[-2.391998378,8.372485160827637,-0.03047393262386322,-0.02337256446480751,"laneChangeStarting"],[-2.381510741,8.371258735656738,-0.02903890237212181,-0.023434143513441086,"laneChangeStarting"],[-2.370131161,8.370099067687988,-0.02903890237212181,-0.023495739325881004,"laneChangeStarting"],[-2.357180613,8.369394302368164,-0.02903890237212181,-0.02355734445154667,"laneChangeStarting"],[-2.350595416,8.368741989135742,-0.02903890237212181,-0.02361896075308323,"laneChangeStarting"],[-2.341672079,8.365123748779297,-0.02903890237212181,-0.023680631071329117,"laneChangeStarting"],[-2.32379076,8.364821434020996,-0.02926427684724331,-0.023742303252220154,"laneChangeStarting"],[-2.321413131,8.364821434020996,-0.02926427684724331,-0.02380397729575634,"laneChangeStarting"],[-2.304881683,8.365131378173828,-0.02926427684724331,-0.023865647614002228,"laneChangeStarting"],[-2.302406321,8.365131378173828,-0.02926427684724331,-0.023927316069602966,"laneChangeStarting"],[-2.294315067,8.36796760559082,-0.02926427684724331,-0.023988943547010422,"laneChangeStarting"],[-2.285726762,8.370232582092285,-0.027269668877124786,-0.02405053749680519,"laneChangeStarting"],[-2.277456454,8.374964714050293,-0.027269668877124786,-0.02411206252872944,"laneChangeStarting"],[-2.259580345,8.379304885864258,-0.027269668877124786,-0.024173522368073463,"laneChangeStarting"],[-2.252394059,8.383243560791016,-0.027269668877124786,-0.02423492632806301,"laneChangeStarting"],[-2.246080961,8.384511947631836,-0.027269668877124786,-0.024296309798955917,"laneChangeStarting"],[-2.233745359,8.390847206115723,-0.026787927374243736,-0.02435760200023651,"laneChangeStarting"],[-2.215805849,8.397696495056152,-0.026787927374243736,-0.024418793618679047,"laneChangeStarting"],[-2.212997122,8.397696495056152,-0.026787927374243736,-0.024479985237121582,"laneChangeStarting"],[-2.200568841,8.397796630859375,-0.026787927374243736,-0.024541174992918968,"laneChangeStarting"],[-2.190820087,8.410253524780273,-0.026787927374243736,-0.02460218407213688,"laneChangeStarting"],[-2.18827476,8.410253524780273,-0.026787927374243736,-0.02466319315135479,"laneChangeStarting"],[-2.178561953,8.412099838256836,-0.025493761524558067,-0.02472417615354061,"laneChangeStarting"],[-2.166033699,8.418447494506836,-0.025493761524558067,-0.02478506602346897,"laneChangeStarting"],[-2.151820751,8.425830841064453,-0.025493761524558067,-0.024845851585268974,"laneChangeStarting"],[-2.149613581,8.425830841064453,-0.025493761524558067,-0.024906635284423828,"laneChangeStarting"],[-2.140823975,8.42650032043457,-0.025493761524558067,-0.024967407807707787,"laneChangeStarting"],[-2.130028501,8.430214881896973,-0.025378908962011337,-0.025028130039572716,"laneChangeStarting"],[-2.105210698,8.428825378417969,-0.025378908962011337,-0.025088869035243988,"laneChangeStarting"],[-2.103026816,8.428825378417969,-0.025378908962011337,-0.02514960989356041,"laneChangeStarting"],[-2.095430843,8.427257537841797,-0.025378908962011337,-0.025210373103618622,"laneChangeStarting"],[-2.088109575,8.426321983337402,-0.023153821006417274,-0.025271151214838028,"laneChangeStarting"],[-2.080971948,8.426762580871582,-0.023153821006417274,-0.02520533837378025,"laneChangeStarting"],[-2.074537361,8.430765151977539,-0.023153821006417274,-0.02513495460152626,"laneChangeStarting"],[-2.054757283,8.438791275024414,-0.023153821006417274,-0.025060465559363365,"laneChangeStarting"],[-2.05233605,8.438791275024414,-0.023153821006417274,-0.02498212642967701,"laneChangeStarting"],[-2.043699972,8.438638687133789,-0.023153821006417274,-0.024900313466787338,"laneChangeStarting"],[-2.034477705,8.438275337219238,-0.02280731312930584,-0.0248153917491436,"laneChangeStarting"],[-2.020461994,8.443028450012207,-0.02280731312930584,-0.024726975709199905,"laneChangeStarting"],[-2.000658994,8.448362350463867,-0.02280731312930584,-0.024635469540953636,"laneChangeStarting"],[-1.998528927,8.448362350463867,-0.02280731312930584,-0.024541141465306282,"laneChangeStarting"],[-1.990623242,8.453072547912598,-0.02280731312930584,-0.0244444627314806,"laneChangeStarting"],[-1.982990598,8.454155921936035,-0.0216695424169302,-0.024342931807041168,"laneChangeStarting"],[-1.976416864,8.455657005310059,-0.0216695424169302,-0.024237047880887985,"laneChangeStarting"],[-1.955835604,8.459799766540527,-0.0216695424169302,-0.02412734553217888,"laneChangeStarting"],[-1.953762844,8.459799766540527,-0.0216695424169302,-0.024014180526137352,"laneChangeStarting"],[-1.94532234,8.464690208435059,-0.0216695424169302,-0.023898128420114517,"laneChangeStarting"],[-1.936889493,8.467460632324219,-0.020979566499590874,-0.023779558017849922,"laneChangeStarting"],[-1.927983351,8.470520973205566,-0.020979566499590874,-0.023657215759158134,"laneChangeStarting"],[-1.912179488,8.475736618041992,-0.020979566499590874,-0.02353164367377758,"laneChangeStarting"],[-1.896368071,8.476133346557617,-0.020979566499590874,-0.02340317703783512,"laneChangeStarting"],[-1.889682174,8.476222038269043,-0.020979566499590874,-0.023272261023521423,"laneChangeStarting"],[-1.880363324,8.479827880859375,-0.019504103809595108,-0.023135840892791748,"laneChangeStarting"],[-1.867049617,8.483298301696777,-0.019504103809595108,-0.022994523867964745,"laneChangeStarting"],[-1.85534595,8.48512077331543,-0.019504103809595108,-0.022848842665553093,"laneChangeStarting"],[-1.848857863,8.490327835083008,-0.019504103809595108,-0.02269948273897171,"laneChangeStarting"],[-1.840580524,8.49223804473877,-0.019504103809595108,-0.022546881809830666,"laneChangeStarting"],[-1.831615878,8.492598533630371,-0.018918082118034363,-0.022390091791749,"laneChangeStarting"],[-1.818484092,8.494182586669922,-0.018918082118034363,-0.022229740396142006,"laneChangeStarting"],[-1.802628028,8.497294425964355,-0.018918082118034363,-0.022066455334424973,"laneChangeStarting"],[-1.784171416,8.499932289123535,-0.017425987869501114,-0.021900760009884834,"laneChangeStarting"],[-1.777500887,8.507792472839355,-0.017425987869501114,-0.021729739382863045,"laneChangeStarting"],[-1.761255142,8.511397361755371,-0.017425987869501114,-0.021553920581936836,"laneChangeStarting"],[-1.75293894,8.514432907104492,-0.017425987869501114,-0.021373948082327843,"laneChangeStarting"],[-1.74618365,8.517693519592285,-0.017425987869501114,-0.021190477535128593,"laneChangeStarting"],[-1.73776362,8.520784378051758,-0.016432875767350197,-0.021004123613238335,"laneChangeStarting"],[-1.729168493,8.524849891662598,-0.016432875767350197,-0.02081310749053955,"laneChangeStarting"],[-1.715636086,8.524147033691406,-0.016432875767350197,-0.02061792090535164,"laneChangeStarting"],[-1.70331544,8.519798278808594,-0.016432875767350197,-0.020419081673026085,"laneChangeStarting"],[-1.696766817,8.521190643310547,-0.016432875767350197,-0.02021750621497631,"laneChangeStarting"],[-1.683002893,8.524537086486816,-0.01570872962474823,-0.02001214027404785,"laneChangeStarting"],[-1.670279387,8.522892951965332,-0.01570872962474823,-0.019803447648882866,"laneChangeStarting"],[-1.662331246,8.521170616149902,-0.01570872962474823,-0.01959208957850933,"laneChangeStarting"],[-1.650466496,8.524321556091309,-0.01570872962474823,-0.019378945231437683,"laneChangeStarting"],[-1.64406093,8.527085304260254,-0.01570872962474823,-0.01916460506618023,"laneChangeStarting"],[-1.633649618,8.526830673217773,-0.014513835310935974,-0.01894950494170189,"laneChangeStarting"],[-1.625671522,8.526430130004883,-0.014513835310935974,-0.018731286749243736,"laneChangeStarting"],[-1.609268862,8.525157928466797,-0.014513835310935974,-0.018510593101382256,"laneChangeStarting"],[-1.601205588,8.526878356933594,-0.014513835310935974,-0.018288226798176765,"laneChangeStarting"],[-1.594555898,8.528356552124023,-0.014513835310935974,-0.018064802512526512,"laneChangeStarting"],[-1.579300662,8.532629013061523,-0.01382231991738081,-0.017839374020695686,"laneChangeStarting"],[-1.562636895,8.540326118469238,-0.01382231991738081,-0.017612768337130547,"laneChangeStarting"],[-1.560109282,8.540326118469238,-0.01382231991738081,-0.017385205253958702,"laneChangeStarting"],[-1.550041442,8.545138359069824,-0.01382231991738081,-0.0171575378626585,"laneChangeStarting"],[-1.541939929,8.552786827087402,-0.01382231991738081,-0.016934284940361977,"laneChangeStarting"],[-1.532915945,8.561888694763184,-0.012809663079679012,-0.01670956239104271,"laneChangeStarting"],[-1.519214172,8.567748069763184,-0.012809663079679012,-0.016483837738633156,"laneChangeStarting"],[-1.506984539,8.574338912963867,-0.012809663079679012,-0.0162577535957098,"laneChangeStarting"],[-1.497626773,8.578181266784668,-0.012809663079679012,-0.016031738370656967,"laneChangeStarting"],[-1.491149626,8.586640357971191,-0.012809663079679012,-0.015814607962965965,"laneChangeStarting"],[-1.478618877,8.595577239990234,-0.011956259608268738,-0.015596358105540276,"laneChangeStarting"],[-1.459849894,8.598489761352539,-0.011956259608268738,-0.015377293340861797,"laneChangeStarting"],[-1.452521231,8.602231979370117,-0.011956259608268738,-0.015158029273152351,"laneChangeStarting"],[-1.445898006,8.606377601623535,-0.011956259608268738,-0.01494218036532402,"laneChangeStarting"],[-1.43751835,8.610146522521973,-0.011466197669506073,-0.014737174846231937,"laneChangeStarting"],[-1.427388724,8.616215705871582,-0.011466197669506073,-0.014531753025949001,"laneChangeStarting"],[-1.415092773,8.62138843536377,-0.011466197669506073,-0.014326399192214012,"laneChangeStarting"],[-1.400733022,8.626657485961914,-0.011466197669506073,-0.014127902686595917,"laneChangeStarting"],[-1.398313404,8.626657485961914,-0.011466197669506073,-0.013939330354332924,"laneChangeStarting"],[-1.387402749,8.627434730529785,-0.009780202992260456,-0.013760197907686234,"laneChangeStarting"],[-1.369474289,8.62661361694336,-0.009780202992260456,-0.013577356934547424,"laneChangeStarting"],[-1.367322864,8.62661361694336,-0.009780202992260456,-0.013391468673944473,"laneChangeStarting"],[-1.353594313,8.630646705627441,-0.009780202992260456,-0.013203301467001438,"laneChangeStarting"],[-1.351187459,8.630646705627441,-0.009780202992260456,-0.013013259507715702,"laneChangeStarting"],[-1.342804365,8.6300630569458,-0.009780202992260456,-0.01282187458127737,"laneChangeStarting"],[-1.33353782,8.630731582641602,-0.008331254124641418,-0.01262619812041521,"laneChangeStarting"],[-1.32081796,8.630699157714844,-0.008331254124641418,-0.012426886707544327,"laneChangeStarting"],[-1.305445716,8.635422706604004,-0.008331254124641418,-0.012224814854562283,"laneChangeStarting"],[-1.303338416,8.635422706604004,-0.008331254124641418,-0.012020403519272804,"laneChangeStarting"],[-1.29420232,8.63885498046875,-0.008331254124641418,-0.011814421974122524,"laneChangeStarting"],[-1.28338872,8.642923355102539,-0.007879149168729782,-0.011606378480792046,"laneChangeStarting"],[-1.255378564,8.642657279968262,-0.007879149168729782,-0.011396684683859348,"laneChangeStarting"],[-1.2486674,8.642162322998047,-0.007879149168729782,-0.011185921728610992,"laneChangeStarting"],[-1.240650752,8.641104698181152,-0.007879149168729782,-0.010974627919495106,"laneChangeStarting"],[-1.233825653,8.644842147827148,-0.007425522897392511,-0.010764610953629017,"laneChangeStarting"],[-1.22539562,8.648493766784668,-0.007425522897392511,-0.010554171167314053,"laneChangeStarting"],[-1.205146995,8.650038719177246,-0.007425522897392511,-0.010343746282160282,"laneChangeStarting"],[-1.203054751,8.650038719177246,-0.007425522897392511,-0.010142660699784756,"laneChangeStarting"],[-1.19469906,8.651565551757812,-0.007425522897392511,-0.009951651096343994,"laneChangeStarting"],[-1.18628028,8.654681205749512,-0.006828428246080875,-0.009770232252776623,"laneChangeStarting"],[-1.172208101,8.656620025634766,-0.006828428246080875,-0.009587879292666912,"laneChangeStarting"],[-1.151382769,8.656707763671875,-0.006828428246080875,-0.00940500758588314,"laneChangeStarting"],[-1.144749437,8.661134719848633,-0.006828428246080875,-0.00922226533293724,"laneChangeStarting"],[-1.137963046,8.664664268493652,-0.006828428246080875,-0.009047594852745533,"laneChangeStarting"],[-1.129476228,8.665833473205566,-0.005932111758738756,-0.008871209807693958,"laneChangeStarting"],[-1.117179078,8.66707992553711,-0.005932111758738756,-0.008693628013134003,"laneChangeStarting"],[-1.101328797,8.672690391540527,-0.005932111758738756,-0.008515516296029091,"laneChangeStarting"],[-1.099131579,8.672690391540527,-0.005932111758738756,-0.008337114006280899,"laneChangeStarting"],[-1.089975635,8.675450325012207,-0.005932111758738756,-0.008162028156220913,"laneChangeStarting"],[-1.080587705,8.678543090820312,-0.005136608146131039,-0.007985549047589302,"laneChangeStarting"],[-1.066288542,8.683448791503906,-0.005136608146131039,-0.007808250840753317,"laneChangeStarting"],[-1.046715764,8.690016746520996,-0.005136608146131039,-0.007630674168467522,"laneChangeStarting"],[-1.04451839,8.690016746520996,-0.005136608146131039,-0.0074530113488435745,"laneChangeStarting"],[-1.035551243,8.699445724487305,-0.004674247000366449,-0.007282670587301254,"laneChangeStarting"],[-1.033204977,8.699445724487305,-0.004674247000366449,-0.0071116178296506405,"laneChangeStarting"],[-1.02035175,8.705824851989746,-0.004674247000366449,-0.006940547376871109,"laneChangeStarting"],[-1.007321917,8.711013793945312,-0.004674247000366449,-0.006772863212972879,"laneChangeStarting"],[-1.001238306,8.714323043823242,-0.004674247000366449,-0.006613606587052345,"laneChangeStarting"],[-0.993212177,8.721551895141602,-0.004674247000366449,-0.00646240683272481,"laneChangeStarting"],[-0.984873629,8.727862358093262,-0.0033356263302266598,-0.006318849045783281,"laneChangeStarting"],[-0.970421048,8.73102855682373,-0.0033356263302266598,-0.006172478199005127,"laneChangeStarting"],[-0.958374226,8.73769760131836,-0.0033356263302266598,-0.006023906171321869,"laneChangeStarting"],[-0.946431858,8.743070602416992,-0.0033356263302266598,-0.005873566493391991,"laneChangeStarting"],[-0.937141613,8.74496078491211,-0.0028988972771912813,-0.005721802357584238,"laneChangeStarting"],[-0.927778537,8.750053405761719,-0.0028988972771912813,-0.0055680968798696995,"laneChangeStarting"],[-0.913629626,8.753966331481934,-0.0028988972771912813,-0.00541288498789072,"laneChangeStarting"],[-0.901148372,8.756794929504395,-0.0028988972771912813,-0.00525658531114459,"laneChangeStarting"],[-0.89881263,8.756794929504395,-0.0028988972771912813,-0.005099536385387182,"laneChangeStarting"],[-0.888489366,8.755200386047363,-0.0028988972771912813,-0.004942102357745171,"laneChangeStarting"],[-0.877724584,8.754884719848633,-0.0023963353596627712,-0.004783503711223602,"laneChangeStarting"],[-0.860023689,8.754664421081543,-0.0023963353596627712,-0.004624187480658293,"laneChangeStarting"],[-0.853423389,8.756451606750488,-0.0023963353596627712,-0.004464647267013788,"laneChangeStarting"],[-0.840709473,8.75784969329834,-0.0023963353596627712,-0.004307469353079796,"laneChangeStarting"],[-0.830431376,8.75889778137207,-0.0022838013246655464,-0.004152536857873201,"laneChangeStarting"],[-0.814659923,8.758641242980957,-0.0022838013246655464,-0.004005347844213247,"laneChangeStarting"],[-0.812467811,8.758641242980957,-0.0022838013246655464,-0.0038655181415379047,"laneChangeStarting"],[-0.800491736,8.761248588562012,-0.0022838013246655464,-0.0037327136378735304,"laneChangeStarting"],[-0.791821074,8.765741348266602,-0.0022838013246655464,-0.0036066067405045033,"laneChangeStarting"],[-0.783338218,8.769320487976074,-0.0016532049048691988,-0.003479369916021824,"laneChangeStarting"],[-0.774832492,8.77241039276123,-0.0016532049048691988,-0.0033513575326651335,"laneChangeStarting"],[-0.756278311,8.77579402923584,-0.0016532049048691988,-0.003222930710762739,"laneChangeStarting"],[-0.749750945,8.778684616088867,-0.0016532049048691988,-0.0030944107566028833,"laneChangeStarting"],[-0.734120361,8.780357360839844,-0.0018559148302301764,-0.0029688761569559574,"laneChangeStarting"],[-0.719946339,8.788431167602539,-0.0018559148302301764,-0.002859856467694044,"laneChangeStarting"],[-0.717677385,8.788431167602539,-0.0018559148302301764,-0.002756287809461355,"laneChangeStarting"],[-0.701838259,8.79088306427002,-0.0018559148302301764,-0.0026579287368804216,"laneChangeStarting"],[-0.695644519,8.792126655578613,-0.0018559148302301764,-0.0025645035784691572,"laneChangeStarting"],[-0.684948296,8.794149398803711,-0.0017480537062510848,-0.0024757750798016787,"laneChangeStarting"],[-0.676433975,8.79992389678955,-0.0017480537062510848,-0.0023871143348515034,"laneChangeStarting"],[-0.659301972,8.80462646484375,-0.0017480537062510848,-0.002301995176821947,"laneChangeStarting"],[-0.649579952,8.816682815551758,-0.0017480537062510848,-0.002221283968538046,"laneChangeStarting"],[-0.647440614,8.816682815551758,-0.0017480537062510848,-0.002144608413800597,"laneChangeStarting"],[-0.629972954,8.816195487976074,-0.0018096480052918196,-0.002074840245768428,"laneChangeStarting"],[-0.616251442,8.819939613342285,-0.0018096480052918196,-0.0020086076110601425,"laneChangeStarting"],[-0.60796921,8.82421875,-0.0018096480052918196,-0.0019457403104752302,"laneChangeStarting"],[-0.597517892,8.826720237731934,-0.0018096480052918196,-0.0018860477721318603,"laneChangeStarting"],[-0.590676178,8.832483291625977,-0.0018096480052918196,-0.0018294121837243438,"laneChangeStarting"],[-0.580771091,8.838994979858398,-0.0015603265492245555,-0.00177238998003304,"laneChangeStarting"],[-0.569472738,8.846171379089355,-0.0015603265492245555,-0.0017151521751657128,"laneChangeStarting"],[-0.558594803,8.852799415588379,-0.0015603265492245555,-0.0016578463837504387,"laneChangeStarting"],[-0.548381305,8.858137130737305,-0.0015603265492245555,-0.0016006029909476638,"laneChangeStarting"],[-0.54171354,8.862641334533691,-0.0015603265492245555,-0.0015603265492245555,"laneChangeStarting"],[-0.528889857,8.869388580322266,-0.0015476001426577568,-0.0015476001426577568,"laneChangeStarting"],[-0.50574489,8.87291431427002,-0.0015476001426577568,-0.0015476001426577568,"laneChangeStarting"],[-0.503635976,8.87291431427002,-0.0015476001426577568,-0.0015476001426577568,"laneChangeStarting"],[-0.495684191,8.87571907043457,-0.0015476001426577568,-0.0015476001426577568,"laneChangeStarting"],[-0.488295933,8.883155822753906,-0.0015476001426577568,-0.0015476001426577568,"laneChangeStarting"],[-0.481355912,8.887700080871582,-0.001590547151863575,-0.001590547151863575,"laneChangeStarting"],[-0.474772803,8.89106273651123,-0.001590547151863575,-0.001590547151863575,"laneChangeStarting"],[-0.459598844,8.89598274230957,-0.001590547151863575,-0.001590547151863575,"laneChangeStarting"],[-0.452461274,8.901592254638672,-0.001590547151863575,-0.001590547151863575,"laneChangeStarting"],[-0.446313378,8.906702041625977,-0.001590547151863575,-0.001590547151863575,"laneChangeStarting"],[-0.43319806,8.907564163208008,-0.0013480840716511011,-0.0015356907388195395,"laneChangeStarting"],[-0.414441327,8.910930633544922,-0.0013480840716511011,-0.0014805631944909692,"laneChangeStarting"],[-0.412197432,8.910930633544922,-0.0013480840716511011,-0.0014252725522965193,"laneChangeStarting"],[-0.39967075,8.912286758422852,-0.0013480840716511011,-0.0013699784176424146,"laneChangeStarting"],[-0.397478534,8.912286758422852,-0.0013480840716511011,-0.0013480840716511011,"laneChangeStarting"],[-0.38717392,8.915579795837402,-0.0010584749979898334,-0.0013480840716511011,"laneChangeStarting"],[-0.378804582,8.91789436340332,-0.0010584749979898334,-0.0012932386016473174,"laneChangeStarting"],[-0.365487026,8.921575546264648,-0.0010584749979898334,-0.0012380164116621017,"laneChangeStarting"],[-0.353254533,8.920105934143066,-0.0010584749979898334,-0.001182509120553732,"laneChangeStarting"],[-0.346537328,8.919178009033203,-0.0010584749979898334,-0.0011268719099462032,"laneChangeStarting"],[-0.33800769,8.919170379638672,-0.0010584749979898334,-0.0010717064142227173,"laneChangeStarting"],[-0.32629017,8.915513038635254,-0.0008449727902188897,-0.0010161095997318625,"laneChangeStarting"],[-0.301304011,8.917206764221191,-0.0008449727902188897,-0.0009603023063391447,"laneChangeStarting"],[-0.298868609,8.917206764221191,-0.0008449727902188897,-0.0009044106118381023,"laneChangeStarting"],[-0.292090712,8.92124080657959,-0.0008449727902188897,-0.000849718286190182,"laneChangeStarting"],[-0.284698547,8.923652648925781,-0.0006465460755862296,-0.0008449727902188897,"laneChangeStarting"],[-0.278147165,8.921907424926758,-0.0006465460755862296,-0.0007903985097073019,"laneChangeStarting"],[-0.267997798,8.921238899230957,-0.0006465460755862296,-0.0007356048445217311,"laneChangeStarting"],[-0.255335718,8.921595573425293,-0.0006465460755862296,-0.0006807483150623739,"laneChangeStarting"],[-0.246996336,8.922540664672852,-0.0006465460755862296,-0.0006465460755862296,"laneChangeStarting"],[-0.240281893,8.924415588378906,-0.0006465460755862296,-0.0006465460755862296,"laneChangeStarting"],[-0.231773667,8.926018714904785,-0.0007882386562414467,-0.0007007084786891937,"laneChangeStarting"],[-0.217792662,8.929253578186035,-0.0007882386562414467,-0.0007548316498287022,"laneChangeStarting"],[-0.195568602,8.933612823486328,-0.0007882386562414467,-0.0007882386562414467,"laneChangeStarting"],[-0.193352995,8.933612823486328,-0.0007882386562414467,-0.0007882386562414467,"laneChangeStarting"],[-0.186513051,8.934080123901367,-0.000905486405827105,-0.0007882386562414467,"laneChangeStarting"],[-0.179642423,8.93358039855957,-0.000905486405827105,-0.0008423093822784722,"laneChangeStarting"],[-0.164694666,8.935224533081055,-0.000905486405827105,-0.0008963602595031261,"laneChangeStarting"],[-0.156970334,8.932820320129395,-0.000905486405827105,-0.000905486405827105,"laneChangeStarting"],[-0.147031958,8.930838584899902,-0.000905486405827105,-0.000905486405827105,"laneChangeStarting"],[-0.140296884,8.930416107177734,-0.000905486405827105,-0.000905486405827105,"laneChangeStarting"],[-0.130208001,8.932591438293457,-0.0007016733870841563,-0.000851028598845005,"laneChangeStarting"],[-0.113283603,8.93777084350586,-0.0007016733870841563,-0.0007964099058881402,"laneChangeStarting"],[-0.110966668,8.93777084350586,-0.0007016733870841563,-0.0007417112938128412,"laneChangeStarting"],[-0.097348932,8.939506530761719,-0.0007016733870841563,-0.0007016733870841563,"laneChangeStarting"],[-0.086479957,8.942127227783203,-0.0008888556621968746,-0.0007016733870841563,"laneChangeStarting"],[-0.078270087,8.94631290435791,-0.0008888556621968746,-0.0007555903284810483,"laneChangeStarting"],[-0.065998157,8.948528289794922,-0.0008888556621968746,-0.000809480610769242,"laneChangeStarting"],[-0.052732696,8.953812599182129,-0.0008888556621968746,-0.0008633072720840573,"laneChangeStarting"],[-0.046553596,8.957008361816406,-0.0008888556621968746,-0.0008888556621968746,"laneChangeStarting"],[-0.038361438,8.957493782043457,-0.0008888556621968746,-0.0008888556621968746,"laneChangeStarting"],[-0.029112714,8.959246635437012,-0.0006447710329666734,-0.0008346209651790559,"laneChangeStarting"],[-0.015388545,8.962183952331543,-0.0006447710329666734,-0.000780103902798146,"laneChangeStarting"],[-0.003173243,8.966590881347656,-0.0006447710329666734,-0.0007254708325490355,"laneChangeStarting"],[0.014928374,8.968188285827637,-0.0005988081102259457,-0.0006708291475661099,"laneChangeStarting"],[0.017454995,8.968188285827637,-0.0005988081102259457,-0.0006161819328553975,"laneChangeStarting"],[0.024461541,8.972087860107422,-0.0005988081102259457,-0.0005988081102259457,"laneChangeStarting"],[0.031208281,8.975468635559082,-0.0005988081102259457,-0.0005988081102259457,"laneChangeStarting"],[0.045052162,8.979111671447754,-0.0005988081102259457,-0.0005988081102259457,"laneChangeStarting"],[0.052637394,8.978455543518066,-0.0005988081102259457,-0.0005988081102259457,"laneChangeStarting"],[0.059662173,8.980249404907227,-0.0005988081102259457,-0.0005988081102259457,"laneChangeStarting"],[0.068319235,8.982062339782715,-0.0004219472757540643,-0.0005450101452879608,"laneChangeStarting"],[0.080234978,8.978232383728027,-0.0004219472757540643,-0.0004910032148472965,"laneChangeStarting"],[0.091403241,8.974494934082031,-0.0004219472757540643,-0.0004369278613012284,"laneChangeStarting"],[0.101142918,8.974297523498535,-0.0004219472757540643,-0.0004219472757540643,"laneChangeStarting"],[0.113531644,8.976276397705078,-0.0006127918022684753,-0.0004219472757540643,"laneChangeStarting"],[0.123130817,8.976118087768555,-0.0006127918022684753,-0.0004755067639052868,"laneChangeStarting"],[0.139684335,8.977002143859863,-0.0006127918022684753,-0.0005290557164698839,"laneChangeStarting"],[0.148230328,8.97848129272461,-0.0006127918022684753,-0.0005825869739055634,"laneChangeStarting"],[0.154945341,8.978241920471191,-0.0006127918022684753,-0.0006127918022684753,"laneChangeStarting"],[0.165178892,8.979059219360352,-0.0007321580196730793,-0.0006127918022684753,"laneChangeStarting"],[0.173559948,8.983506202697754,-0.0007321580196730793,-0.0006662632222287357,"laneChangeStarting"],[0.187316776,8.98563003540039,-0.0007321580196730793,-0.0007197093800641596,"laneChangeStarting"],[0.198653467,8.986715316772461,-0.0007321580196730793,-0.0007321580196730793,"laneChangeStarting"],[0.205328419,8.984946250915527,-0.0007321580196730793,-0.0007321580196730793,"laneChangeStarting"],[0.219592562,8.98621654510498,-0.0007290634675882757,-0.0007290634675882757,"laneChangeStarting"],[0.235931911,8.9910888671875,-0.0007290634675882757,-0.0007290634675882757,"laneChangeStarting"],[0.238230247,8.9910888671875,-0.0007290634675882757,-0.0007290634675882757,"laneChangeStarting"],[0.248520166,8.996482849121094,-0.0007290634675882757,-0.0007290634675882757,"laneChangeStarting"],[0.257274805,9.001331329345703,-0.0007290634675882757,-0.0007290634675882757,"laneChangeStarting"],[0.265958592,9.005638122558594,-0.0007966487901285291,-0.0007290634675882757,"laneChangeStarting"],[0.278902144,9.010930061340332,-0.0007966487901285291,-0.0007822099141776562,"laneChangeStarting"],[0.295186584,9.010947227478027,-0.0007966487901285291,-0.0007966487901285291,"laneChangeStarting"],[0.297545925,9.010947227478027,-0.0007966487901285291,-0.0007966487901285291,"laneChangeStarting"],[0.305809034,9.012892723083496,-0.0007966487901285291,-0.0007966487901285291,"laneChangeStarting"],[0.32571985,9.014490127563477,-0.0007198401144705713,-0.0007434789440594614,"laneChangeStarting"],[0.328095914,9.014490127563477,-0.0007198401144705713,-0.0007198401144705713,"laneChangeStarting"],[0.339319868,9.016523361206055,-0.0007198401144705713,-0.0007198401144705713,"laneChangeStarting"],[0.350552315,9.020852088928223,-0.0007198401144705713,-0.0007198401144705713,"laneChangeStarting"],[0.358159271,9.02804946899414,-0.0007198401144705713,-0.0007198401144705713,"laneChangeStarting"],[0.365239428,9.028425216674805,-0.0005539362318813801,-0.0007198401144705713,"laneChangeStarting"],[0.376298498,9.032292366027832,-0.0005539362318813801,-0.0006666621193289757,"laneChangeStarting"],[0.391156747,9.034109115600586,-0.0005539362318813801,-0.0006133663700893521,"laneChangeStarting"],[0.398790793,9.037800788879395,-0.0005539362318813801,-0.0005601116572506726,"laneChangeStarting"],[0.405623701,9.039449691772461,-0.0005539362318813801,-0.0005539362318813801,"laneChangeStarting"],[0.418391167,9.040339469909668,-0.0005180884036235511,-0.0005180884036235511,"laneChangeStarting"],[0.440895758,9.047338485717773,-0.0005180884036235511,-0.0005180884036235511,"laneChangeStarting"],[0.448209567,9.045626640319824,-0.0005180884036235511,-0.0005180884036235511,"laneChangeStarting"],[0.455063366,9.04718017578125,-0.0005180884036235511,-0.0005180884036235511,"laneChangeStarting"],[0.463938451,9.05064868927002,-0.0005948347970843315,-0.0005180884036235511,"laneChangeStarting"],[0.472372177,9.055134773254395,-0.0005948347970843315,-0.0005707172094844282,"laneChangeStarting"],[0.490916088,9.058281898498535,-0.0005948347970843315,-0.0005948347970843315,"laneChangeStarting"],[0.498478866,9.0624418258667,-0.0005948347970843315,-0.0005948347970843315,"laneChangeStarting"],[0.507758376,9.066397666931152,-0.0005948347970843315,-0.0005948347970843315,"laneChangeStarting"],[0.514377481,9.06864070892334,-0.0006811006460338831,-0.0005948347970843315,"laneChangeStarting"],[0.527996566,9.071561813354492,-0.0006811006460338831,-0.0006472732056863606,"laneChangeStarting"],[0.54964886,9.073980331420898,-0.0006811006460338831,-0.0006811006460338831,"laneChangeStarting"],[0.551940423,9.073980331420898,-0.0006811006460338831,-0.0006811006460338831,"laneChangeStarting"],[0.560754556,9.076334953308105,-0.0006811006460338831,-0.0006811006460338831,"laneChangeStarting"],[0.563260756,9.076334953308105,-0.0004927921108901501,-0.0006811006460338831,"laneChangeStarting"],[0.57188406,9.075308799743652,-0.0004927921108901501,-0.000628368288744241,"laneChangeStarting"],[0.58576941,9.072990417480469,-0.0004927921108901501,-0.0005754167214035988,"laneChangeStarting"],[0.599950094,9.072417259216309,-0.0004927921108901501,-0.0005224047927185893,"laneChangeStarting"],[0.602078283,9.072417259216309,-0.0004927921108901501,-0.0004927921108901501,"laneChangeStarting"],[0.613073587,9.073593139648438,-0.0004927921108901501,-0.0004927921108901501,"laneChangeStarting"],[0.621847501,9.075299263000488,-0.0004679835692513734,-0.0004679835692513734,"laneChangeStarting"],[0.63737373,9.076740264892578,-0.0004679835692513734,-0.0004679835692513734,"laneChangeStarting"],[0.65629706,9.0839204788208,-0.0004679835692513734,-0.0004679835692513734,"laneChangeStarting"],[0.658524284,9.0839204788208,-0.0004679835692513734,-0.0004679835692513734,"laneChangeStarting"],[0.668606182,9.088407516479492,-0.0005813679890707135,-0.0005202277679927647,"laneChangeStarting"],[0.681315092,9.089725494384766,-0.0005813679890707135,-0.0005724567454308271,"laneChangeStarting"],[0.696387145,9.093010902404785,-0.0005813679890707135,-0.0005813679890707135,"laneChangeStarting"],[0.703954872,9.096226692199707,-0.0005813679890707135,-0.0005813679890707135,"laneChangeStarting"],[0.710724743,9.100500106811523,-0.0005813679890707135,-0.0005813679890707135,"laneChangeStarting"],[0.719297358,9.10384750366211,-0.0007795732817612588,-0.0006334350910037756,"laneChangeStarting"],[0.731740263,9.10783576965332,-0.0007795732817612588,-0.0006854566163383424,"laneChangeStarting"],[0.747930409,9.110138893127441,-0.0007795732817612588,-0.0007374518318101764,"laneChangeStarting"],[0.760627388,9.115785598754883,-0.0007795732817612588,-0.0007795732817612588,"laneChangeStarting"],[0.769156502,9.11545181274414,-0.0005560028948821127,-0.0007272153743542731,"laneChangeStarting"],[0.780111535,9.118267059326172,-0.0005560028948821127,-0.0006746148574166,"laneChangeStarting"],[0.79421793,9.12023639678955,-0.0005560028948821127,-0.0006219038041308522,"laneChangeStarting"],[0.800450167,9.120290756225586,-0.0005560028948821127,-0.0005692291888408363,"laneChangeStarting"],[0.810825524,9.120853424072266,-0.0005560028948821127,-0.0005560028948821127,"laneChangeStarting"],[0.819532963,9.121529579162598,-0.0004877437895629555,-0.0005040928954258561,"laneChangeStarting"],[0.838591587,9.122488975524902,-0.0004877437895629555,-0.0004877437895629555,"laneChangeStarting"],[0.840916648,9.122488975524902,-0.0004877437895629555,-0.0004877437895629555,"laneChangeStarting"],[0.851111648,9.123215675354004,-0.0004877437895629555,-0.0004877437895629555,"laneChangeStarting"],[0.869060983,9.122210502624512,-0.0003517493896652013,-0.00043567639659158885,"laneChangeStarting"],[0.871520349,9.122210502624512,-0.0003517493896652013,-0.00038353647687472403,"laneChangeStarting"],[0.880238624,9.123772621154785,-0.0003517493896652013,-0.0003517493896652013,"laneChangeStarting"],[0.895970426,9.1242036819458,-0.0003517493896652013,-0.0003517493896652013,"laneChangeStarting"],[0.898266105,9.1242036819458,-0.0003517493896652013,-0.0003517493896652013,"laneChangeStarting"],[0.907886637,9.12293815612793,-0.0003517493896652013,-0.0003517493896652013,"laneChangeStarting"],[0.91660434,9.122795104980469,-0.0002550627395976335,-0.0003517493896652013,"laneChangeStarting"],[0.930543141,9.122952461242676,-0.0002550627395976335,-0.00029978627571836114,"laneChangeStarting"],[0.945965542,9.123730659484863,-0.0002550627395976335,-0.0002550627395976335,"laneChangeStarting"],[0.948206624,9.123730659484863,-0.0002550627395976335,-0.0002550627395976335,"laneChangeStarting"],[0.956383253,9.12132453918457,-0.0002550627395976335,-0.0002550627395976335,"laneChangeStarting"],[0.973544527,9.123196601867676,-0.00014484094572253525,-0.00020306938677094877,"laneChangeStarting"],[0.988551721,9.123373031616211,-0.00014484094572253525,-0.00015106514911167324,"laneChangeStarting"],[0.996202073,9.125749588012695,-0.00014484094572253525,-0.00014484094572253525,"laneChangeStarting"]]]} diff --git a/openpilot/starpilot/lateral/tests/test_auto_lane_change.py b/openpilot/starpilot/lateral/tests/test_auto_lane_change.py new file mode 100644 index 0000000000..f00c306ee4 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_auto_lane_change.py @@ -0,0 +1,214 @@ +"""Live Auto evidence and optional typed status contract.""" + +from types import SimpleNamespace +import time +import unittest + +from openpilot.cereal import log +import openpilot.cereal.messaging as messaging +from openpilot.starpilot.lateral.auto_lane_change import ClockEpochGuard, auto_evidence, session_policy +from openpilot.starpilot.lateral.lane_change_preferences import LaneChangePolicy, decode as decode_policy +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.starpilot.lateral.lane_change_status_wire import ( + Direction, LaneChangeStatus, Phase, alert_wording, decode, encode, encode_optional, fresh_for_model, +) + + +def line(y): + return SimpleNamespace(x=ModelConstants.X_IDXS, y=[y] * ModelConstants.IDX_N) + + +def model(): + return SimpleNamespace(frameId=77, frameAge=0, timestampEof=10_000_000_000, + laneLines=[line(-5.5), line(-1.8), line(1.8), line(5.5)], + laneLineProbs=[0.95] * 4, laneLineStds=[0.1] * 4, + roadEdges=[line(-6.2), line(6.2)], roadEdgeStds=[0.1] * 2, + meta=SimpleNamespace(laneChangeDirection="left")) + + +def submaster(now): + services = ("carState", "carControl", "extrinsicsCalibration") + class FakeSubMaster(SimpleNamespace): + def __getitem__(self, key): + return self.values[key] + return FakeSubMaster(seen=dict.fromkeys(services, True), alive=dict.fromkeys(services, True), + valid=dict.fromkeys(services, True), logMonoTime=dict.fromkeys(services, now - 10_000_000), + recv_time=dict.fromkeys(services, (now - 10_000_000) / 1e9), + values={"carState": SimpleNamespace(canValid=True, canTimeout=False), + "carControl": SimpleNamespace(enabled=True, latActive=True), + "extrinsicsCalibration": SimpleNamespace(calStatus=log.ExtrinsicsCalibration.Status.calibrated, + rpyCalib=[0., 0., 0.])}) + + +class TestAutoLaneChange(unittest.TestCase): + def test_four_startup_modes(self): + old = decode_policy(b'{"version":1,"enabled":true,"minimumSpeedMps":8.9408,"onePerSignal":false}') + self.assertIsNotNone(old) + assert old is not None + self.assertFalse(session_policy(old, True).auto_lane_change) + requested = LaneChangePolicy(auto_lane_change=True) + self.assertFalse(session_policy(requested, False).auto_lane_change) + self.assertTrue(session_policy(requested, True).auto_lane_change) + self.assertFalse(session_policy(LaneChangePolicy(enabled=False, auto_lane_change=True), True).enabled) + + def test_clock_domains_and_current_calibration(self): + now_mono = 1_000_000_000 + now_boot = 10_050_000_000 # includes a long suspend offset from MONOTONIC + sm, frame = submaster(now_mono), model() + self.assertTrue(auto_evidence(sm, frame, -1, 3.0, now_mono_ns=now_mono, now_boot_ns=now_boot, model_valid=True, vehicle_capable=True)) + self.assertFalse(auto_evidence(sm, frame, -1, 3.0, now_mono_ns=now_mono, now_boot_ns=now_boot + 200_000_000, model_valid=True, vehicle_capable=True)) + sm.values["extrinsicsCalibration"].calStatus = log.ExtrinsicsCalibration.Status.uncalibrated + self.assertFalse(auto_evidence(sm, frame, -1, 3.0, now_mono_ns=now_mono, now_boot_ns=now_boot, model_valid=True, vehicle_capable=True)) + sm.values["extrinsicsCalibration"].calStatus = log.ExtrinsicsCalibration.Status.calibrated + sm.logMonoTime["extrinsicsCalibration"] = now_mono - 800_000_000 + self.assertFalse(auto_evidence(sm, frame, -1, 3.0, now_mono_ns=now_mono, now_boot_ns=now_boot, model_valid=True, vehicle_capable=True)) + + def test_suspend_offset_requires_new_calibration_and_controls(self): + before_mono, before_boot = 1_000_000_000, 10_000_000_000 + guard = ClockEpochGuard(before_mono, before_boot) + sm = submaster(before_mono) + self.assertTrue(guard.ready(sm, before_mono, before_boot)) + # MONOTONIC advanced a short interval; BOOTTIME includes a long suspend. + resumed_mono, resumed_boot = 1_050_000_000, 15_050_000_000 + self.assertFalse(guard.ready(sm, resumed_mono, resumed_boot)) + self.assertFalse(guard.ready(sm, resumed_mono + 10_000_000, resumed_boot + 10_000_000)) + for name in ("carState", "carControl", "extrinsicsCalibration"): + sm.logMonoTime[name] = resumed_mono + 11_000_000 + sm.recv_time[name] = (resumed_mono + 11_000_000) / 1e9 + self.assertTrue(guard.ready(sm, resumed_mono + 12_000_000, resumed_boot + 12_000_000)) + + short_guard = ClockEpochGuard(1_000_000_000, 10_000_000_000) + short_sm = submaster(1_020_000_000) + for name in ("carState", "carControl", "extrinsicsCalibration"): + short_sm.logMonoTime[name] = 1_000_000_000 # apparently only 20 ms old on MONOTONIC + short_sm.recv_time[name] = 1.0 + fresh_camera = model() + fresh_camera.timestampEof = 10_100_000_000 + self.assertTrue(auto_evidence(short_sm, fresh_camera, -1, 3.0, now_mono_ns=1_020_000_000, + now_boot_ns=10_110_000_000, model_valid=True, vehicle_capable=True)) + self.assertFalse(short_guard.ready(short_sm, 1_020_000_000, 10_110_000_000)) + self.assertFalse(short_guard.ready(short_sm, 1_021_000_000, 10_111_000_000)) + self.assertFalse(short_guard.ready(short_sm, 1_021_000_000, 10_111_000_000, sample_skew_ns=2_000_000)) + + def test_same_frame_geometry_and_engagement(self): + now = 1_000_000_000 + sm, frame = submaster(now), model() + self.assertTrue(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True)) + frame.laneLineProbs[3] = 0.2 + self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True)) + frame.laneLineProbs[3] = 0.95 + sm.values["carControl"].enabled = False + self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True)) + sm.values["carControl"].enabled = True + sm.values["carState"].canTimeout = True + self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True)) + sm.values["carState"].canTimeout = False + frame.laneLineProbs[3] = 1.2 + self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True)) + frame.laneLineProbs[3] = 0.95 + frame.frameAge = 2 + self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=True)) + frame.frameAge = 0 + self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=False, vehicle_capable=True)) + self.assertFalse(auto_evidence(sm, frame, 1, 3.0, now_mono_ns=now, now_boot_ns=10_050_000_000, model_valid=True, vehicle_capable=False)) + + def test_typed_wire_arrival_order_and_neutral_fallback(self): + frame = model() + status = LaneChangeStatus("session", 2, 77, 10_000_000_000, 1_000_000_000, 1_150_000_000, + Phase.WAITING_FOR_DELAY, Direction.LEFT, True, True) + raw = encode(status) + self.assertEqual(decode(raw), status) + zero_eof = LaneChangeStatus(status.producer_session_id, status.sequence, status.model_frame_id, 0, + status.observed_mono_time_ns, status.valid_until_mono_time_ns, + status.phase, status.direction, status.auto_configured, status.engaged) + self.assertIsNone(encode_optional(zero_eof)) + self.assertTrue(fresh_for_model(decode(raw), frame, 1_050_000_000, 1_020_000_000)) + # Status-first and model-first delivery are neutral until both identify the same frame. + self.assertFalse(fresh_for_model(decode(raw), replace_status_model(frame, 76), 1_050_000_000, 1_020_000_000)) + self.assertFalse(fresh_for_model(None, frame, 1_050_000_000, 1_020_000_000)) + self.assertFalse(fresh_for_model(decode(raw), frame, 1_400_000_000, 1_020_000_000)) + self.assertFalse(fresh_for_model(decode(raw), frame, 1_050_000_000, 1_020_000_000, 3, "session")) + self.assertIsNone(decode(raw[:-8])) + self.assertEqual(alert_wording(None, frame, 1_050_000_000, 1_020_000_000), + ("Lane Change Pending", "Check surroundings")) + self.assertEqual(alert_wording(status, frame, 1_050_000_000, 1_020_000_000), + ("Automatic Lane Change Pending", "Check surroundings; steer to confirm now")) + self.assertIsNone(alert_wording(replace_status_phase(status, Phase.MANUAL_REQUIRED), + frame, 1_050_000_000, 1_020_000_000)) + aol_manual = LaneChangeStatus(status.producer_session_id, status.sequence, status.model_frame_id, + status.model_timestamp_eof_ns, status.observed_mono_time_ns, + status.valid_until_mono_time_ns, Phase.MANUAL_REQUIRED, status.direction, True, False) + self.assertIsNone(alert_wording(aol_manual, frame, 1_050_000_000, 1_020_000_000)) + aol_auto = replace_status_phase(aol_manual, Phase.WAITING_FOR_DELAY) + self.assertEqual(alert_wording(aol_auto, frame, 1_050_000_000, 1_020_000_000), + ("Lane Change Pending", "Check surroundings")) + self.assertEqual(alert_wording(replace_status_phase(status, Phase.LANE_UNAVAILABLE), + frame, 1_050_000_000, 1_020_000_000), + ("Automatic Change Unavailable", "Steer to confirm when safe")) + self.assertEqual(alert_wording(status, frame, 1_050_000_000, 1_020_000_000, + message_mono_ns=800_000_000), ("Lane Change Pending", "Check surroundings")) + after_suspend = replace_status_model(frame, 78) + after_suspend.timestampEof = 15_000_000_000 + self.assertEqual(alert_wording(status, after_suspend, 1_050_000_000, 1_020_000_000), + ("Lane Change Pending", "Check surroundings")) + + def test_real_optional_service_roundtrip(self): + publisher = messaging.PubMaster(["laneChangeAssistWire", "modelV2"]) + subscriber = messaging.SubMaster(["laneChangeAssistWire", "modelV2"], ignore_alive=["laneChangeAssistWire"], + ignore_valid=["laneChangeAssistWire"]) + self.assertTrue(subscriber.all_checks(["laneChangeAssistWire"])) # optional status cannot hold stock engagement + time.sleep(0.05) + now = time.monotonic_ns() + status = LaneChangeStatus("ipc-session", 1, 77, 10_000_000_000, now, now + 100_000_000, + Phase.MANUAL_REQUIRED, Direction.LEFT, False, True) + msg = messaging.new_message("laneChangeAssistWire", 0) + msg.valid = True + msg.laneChangeAssistWire = encode(status) + publisher.send("laneChangeAssistWire", msg) + subscriber.update(100) + self.assertTrue(subscriber.seen["laneChangeAssistWire"] and subscriber.valid["laneChangeAssistWire"]) + self.assertEqual(decode(subscriber["laneChangeAssistWire"]), status) + self.assertEqual(alert_wording(status, subscriber["modelV2"], time.monotonic_ns(), + int(subscriber.recv_time["laneChangeAssistWire"] * 1e9)), + ("Lane Change Pending", "Check surroundings")) # status arrived first + + def send_model(frame_id): + message = messaging.new_message("modelV2") + message.valid = True + message.modelV2.frameId = frame_id + message.modelV2.timestampEof = 10_000_000_000 + message.modelV2.meta.laneChangeDirection = log.LaneChangeDirection.left + publisher.send("modelV2", message) + subscriber.update(100) + + send_model(77) + self.assertIsNone(alert_wording(decode(subscriber["laneChangeAssistWire"]), subscriber["modelV2"], + time.monotonic_ns(), int(subscriber.recv_time["laneChangeAssistWire"] * 1e9))) + send_model(78) # model arrived first; old wire must not describe it + self.assertEqual(alert_wording(decode(subscriber["laneChangeAssistWire"]), subscriber["modelV2"], + time.monotonic_ns(), int(subscriber.recv_time["laneChangeAssistWire"] * 1e9)), + ("Lane Change Pending", "Check surroundings")) + next_status = LaneChangeStatus("ipc-session", 2, 78, 10_000_000_000, time.monotonic_ns(), + time.monotonic_ns() + 100_000_000, Phase.MANUAL_REQUIRED, Direction.LEFT, False, True) + next_msg = messaging.new_message("laneChangeAssistWire", 0) + next_msg.valid = True + next_msg.laneChangeAssistWire = encode(next_status) + publisher.send("laneChangeAssistWire", next_msg) + subscriber.update(100) + self.assertIsNone(alert_wording(decode(subscriber["laneChangeAssistWire"]), subscriber["modelV2"], + time.monotonic_ns(), int(subscriber.recv_time["laneChangeAssistWire"] * 1e9))) + + +def replace_status_model(frame, frame_id): + return SimpleNamespace(frameId=frame_id, timestampEof=frame.timestampEof, meta=frame.meta) + + +def replace_status_phase(status, phase): + return LaneChangeStatus(status.producer_session_id, status.sequence, status.model_frame_id, + status.model_timestamp_eof_ns, status.observed_mono_time_ns, + status.valid_until_mono_time_ns, phase, status.direction, + status.auto_configured, status.engaged) + + +if __name__ == "__main__": + unittest.main() diff --git a/openpilot/starpilot/lateral/tests/test_bolt_torque_admission.py b/openpilot/starpilot/lateral/tests/test_bolt_torque_admission.py new file mode 100644 index 0000000000..017a5946ad --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_bolt_torque_admission.py @@ -0,0 +1,95 @@ +import ast +from pathlib import Path +import tempfile +from types import SimpleNamespace +import unittest +from unittest.mock import patch + +from opendbc.car import structs +from openpilot.starpilot.lateral.bolt_policy import BOLT_GENERATIONS +from openpilot.starpilot.lateral.torque_runtime import TorqueHost, runtime_enabled +from openpilot.starpilot.lateral.torque_settings import DOCUMENT_KEY, FieldChoice, PlatformProfile, serialize_document + + +class TestBoltTorqueAdmission(unittest.TestCase): + def setUp(self): + temporary = tempfile.TemporaryDirectory() + self.addCleanup(temporary.cleanup) + self.root = Path(temporary.name) + self.params = SimpleNamespace(get_param_path=lambda key: str(self.root / key)) + + def cp(self, identity): + cp = structs.CarParams.new_message() + cp.brand, cp.carFingerprint = 'gm', identity + cp.steerControlType = structs.CarParams.SteerControlType.torque + tune = cp.lateralTuning.init('torque') + tune.latAccelFactor, tune.latAccelOffset, tune.friction = 2.0, 0.0, 0.1 + return cp + + def document(self, cp, friction=None): + if friction is None: + friction = FieldChoice('custom', .14) + tune = cp.lateralTuning.torque + basis = (float(tune.latAccelFactor), float(tune.latAccelOffset), float(tune.friction)) + raw = serialize_document({str(cp.carFingerprint): PlatformProfile(basis, FieldChoice(), friction)}) + (self.root / DOCUMENT_KEY).write_bytes(raw) + + def test_defaults_and_canonical_opt_in(self): + for identity in BOLT_GENERATIONS: + with self.subTest(identity=identity): + cp = self.cp(identity) + for file in self.root.iterdir(): + file.unlink() + self.assertFalse(runtime_enabled(cp, self.params)) + self.document(cp) + self.assertTrue(runtime_enabled(cp, self.params)) + (self.root / 'AdvancedLateralTune').write_bytes(b'0') + self.assertTrue(runtime_enabled(cp, self.params)) + (self.root / 'AdvancedLateralTune').write_bytes(b'1') + self.assertTrue(runtime_enabled(cp, self.params)) + + def test_retired_flag_is_irrelevant_and_document_read_failure_denies(self): + cp = self.cp(next(iter(BOLT_GENERATIONS))) + self.document(cp) + for raw in (b'true', b'2', b'', b'1\n'): + (self.root / 'AdvancedLateralTune').write_bytes(raw) + self.assertTrue(runtime_enabled(cp, self.params)) + with patch('openpilot.starpilot.lateral.torque_runtime._raw', side_effect=PermissionError): + self.assertFalse(runtime_enabled(cp, self.params)) + + def test_opt_in_requires_valid_vehicle_friction_document(self): + cp = self.cp(next(iter(BOLT_GENERATIONS))) + (self.root / 'AdvancedLateralTune').write_bytes(b'1') + self.assertFalse(runtime_enabled(cp, self.params)) + self.document(cp, FieldChoice()) + self.assertFalse(runtime_enabled(cp, self.params)) + self.document(self.cp(list(BOLT_GENERATIONS)[1])) + self.assertFalse(runtime_enabled(cp, self.params)) + (self.root / DOCUMENT_KEY).write_bytes(b'{bad') + self.assertFalse(runtime_enabled(cp, self.params)) + self.document(cp) + (self.root / 'ForceAutoTuneOff').write_bytes(b'bad') + self.assertFalse(runtime_enabled(cp, self.params)) + + def test_actual_controls_host_assignment_defaults_and_saved_choice(self): + source = Path(__file__).resolve().parents[3] / 'selfdrive/controls/controlsd.py' + tree = ast.parse(source.read_text()) + controls = next(node for node in tree.body if isinstance(node, ast.ClassDef) and node.name == 'Controls') + init = next(node for node in controls.body if isinstance(node, ast.FunctionDef) and node.name == '__init__') + assignment = next(node for node in init.body if isinstance(node, ast.Assign) and + any(isinstance(target, ast.Attribute) and target.attr == 'torque_host' for target in node.targets)) + code = compile(ast.Module(body=[assignment], type_ignores=[]), str(source), 'exec') + cp = self.cp('CHEVROLET_BOLT_CC_2018_2021') + current = SimpleNamespace(CP=cp, params=self.params, torque_learning_allowed=False, + lateral_controller_selection=SimpleNamespace(policy=None)) + environment = {'self': current, 'TorqueHost': TorqueHost, 'torque_runtime_enabled': runtime_enabled} + exec(code, environment) + self.assertIsNone(current.torque_host) + self.document(cp) + (self.root / 'AdvancedLateralTune').write_bytes(b'1') + exec(code, environment) + self.assertIsInstance(current.torque_host, TorqueHost) + + +if __name__ == '__main__': + unittest.main() diff --git a/openpilot/starpilot/lateral/tests/test_bolt_torque_policy.py b/openpilot/starpilot/lateral/tests/test_bolt_torque_policy.py new file mode 100644 index 0000000000..62a830bd21 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_bolt_torque_policy.py @@ -0,0 +1,116 @@ +import math +from types import SimpleNamespace +import unittest + +import numpy as np + +from opendbc.car.gm.interface import CarInterface +from opendbc.car.gm.tests.test_bolt_cc import params +from opendbc.car.gm.values import CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.torque_extension import selected_policy +from openpilot.starpilot.lateral.bolt_policy import BOLT_GENERATIONS +from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection, replace_mode, selection_from_bytes + + +class TestBoltTorquePolicy(unittest.TestCase): + def controller(self, identity, mode=None): + cp = params(identity, alpha=True).as_reader() + return cp, LatControlTorque(cp, CarInterface(cp), 0.01, controller_mode=mode) + + def state(self): + return SimpleNamespace(vEgo=20.0, steeringAngleDeg=0.0, steeringPressed=False, steeringRateDeg=0.0, standstill=False) + + def test_exact_default_selection_and_saved_standard(self): + for identity in BOLT_GENERATIONS: + cp, controller = self.controller(identity) + self.assertEqual(default_selection(cp).mode, ControllerMode.STARPILOT) + self.assertIsNotNone(selected_policy(controller)) + choice = selection_from_bytes(cp, replace_mode(None, cp, ControllerMode.STANDARD)) + self.assertEqual(choice.mode, ControllerMode.STANDARD) + standard = LatControlTorque(cp, CarInterface(cp), 0.01, controller_mode=choice.mode) + self.assertIsNone(selected_policy(standard)) + self.assertEqual(standard.pid._k_i[1], [0.15]) + self.assertAlmostEqual(standard.torque_from_lateral_accel(1.0, standard.torque_params), 1.0 / cp.lateralTuning.torque.latAccelFactor) + cp, controller = self.controller(CAR.CHEVROLET_BOLT_EUV) + self.assertEqual(default_selection(cp).mode, ControllerMode.STANDARD) + self.assertIsNone(selected_policy(controller)) + valid, _ = self.controller(CAR.CHEVROLET_BOLT_CC_2017) + for field, value in (('brand', 'toyota'), ('notCar', True), ('dashcamOnly', True), ('passive', True)): + bad = valid.as_builder() + setattr(bad, field, value) + self.assertIsNone(default_selection(bad).policy) + + def test_generation_gains_and_nonlinear_limits_survive_parameter_updates(self): + coefficients = { + 2017: ((2.15, 1.0, 0.129), (2.15, 1.0, 0.145)), + 2018: ((1.8, 1.1, 0.27), (2.0, 1.0, 0.205)), + 2022: ((2.6531724862969748, 1.1, 0.1919764879840985), (2.7031724862969748, 1.0, 0.1469764879840985)), + } + for identity, generation in BOLT_GENERATIONS.items(): + cp, controller = self.controller(identity) + policy = selected_policy(controller) + self.assertEqual(policy.ff_positive, float(np.float32(1.03))) + self.assertEqual(policy.ff_negative, float(np.float32(float(np.float32(1.07)) * (0.9 if generation == 2017 else 1.07)))) + multiplier = float(np.float32(float(np.float32(0.93)) * (0.9 if generation == 2017 else 0.93))) + self.assertEqual(controller.pid._k_i[1], [0.35 * multiplier]) + values = np.arange(-5.0, 5.0, 0.01) + torques = [] + for value in values: + a, b, c = coefficients[generation][0 if value >= 0 else 1] + x = a * value + torques.append(float(np.sign(x) * (1 / (1 + math.exp(-abs(x))) - 0.5) * b + value * c)) + expected_limits = (np.interp(1.0, torques, values), np.interp(-1.0, torques, values)) + for value in (-2.0, -0.05, 0.0, 0.05, 2.0): + self.assertAlmostEqual(controller.torque_from_lateral_accel(value, controller.torque_params), np.interp(value, values, torques)) + self.assertEqual((controller.pid.pos_limit, controller.pid.neg_limit), expected_limits) + controller.update_torque_parameters(1.1 * cp.lateralTuning.torque.latAccelFactor, 0.01, 0.05) + self.assertEqual((controller.pid.pos_limit, controller.pid.neg_limit), expected_limits) + self.assertAlmostEqual(controller.torque_params.friction, 0.05) + + def test_reset_inactive_priming_driver_release_and_saturation(self): + for identity in BOLT_GENERATIONS: + cp, controller = self.controller(identity) + policy = selected_policy(controller) + vm, cs = VehicleModel(cp), self.state() + live = SimpleNamespace(angleOffsetDeg=0.0, roll=0.0) + controller.pid.i = 0.2 + controller.sat_time = 0.3 + policy.previous_output = 0.1 + policy.curvature_buffer[-1] = 0.001 + controller.reset() + self.assertEqual(controller.sat_time, 0.0) + self.assertEqual(controller.pid.i, 0.2) + self.assertEqual(policy.previous_output, 0.1) + self.assertEqual(policy.curvature_buffer[-1], 0.001) + output = controller.update(False, cs, vm, live, False, 0.002, False, 0.2) + self.assertEqual(output[0], 0.0) + self.assertEqual(controller.pid.i, 0.0) + self.assertEqual(policy.previous_output, 0.0) + self.assertEqual(policy.curvature_buffer[-1], 0.002) + self.assertEqual(policy.jerk_filter.x, 0.0) + controller.pid.i = 0.2 + policy.previous_pressed = True + controller.update(True, cs, vm, live, True, 0.002, False, 0.2) + self.assertAlmostEqual(controller.pid.i, 0.16) + cs.vEgo = 35.0 + for _ in range(80): + _, _, logged = controller.update(True, cs, vm, live, False, 1.0, False, 0.2) + self.assertTrue(logged.saturated) + _, _, logged = controller.update(True, cs, vm, live, True, 1.0, False, 0.2) + self.assertFalse(logged.saturated) + + def test_default_kp_survives_parameter_updates_reset_and_inactive_dispatch(self): + for identity in BOLT_GENERATIONS: + cp, controller = self.controller(identity) + self.assertEqual(controller.pid._k_p, ([0], [0.6])) + self.assertEqual(controller.pid._k_p[1][0], 0.6) + controller.update_torque_parameters(3.1, 0.01, 0.09) + controller.reset() + state = self.state() + controller.update(False, state, VehicleModel(cp), SimpleNamespace(angleOffsetDeg=0.0, roll=0.0), False, 0.001, False, 0.2) + self.assertEqual(controller.pid._k_p, ([0], [0.6])) + standard = LatControlTorque(cp, CarInterface(cp), 0.01, controller_mode=ControllerMode.STANDARD) + self.assertEqual(standard.pid._k_p[0], [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]) + self.assertEqual(standard.pid._k_p[1][-1], 0.8) diff --git a/openpilot/starpilot/lateral/tests/test_controller_selection.py b/openpilot/starpilot/lateral/tests/test_controller_selection.py new file mode 100644 index 0000000000..722fab37a2 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_controller_selection.py @@ -0,0 +1,171 @@ +"""The optional policy choice is latched at startup and separate from torque sources.""" + +from dataclasses import replace +import json +import os +import struct +from types import SimpleNamespace +import unittest +from unittest import mock + +from opendbc.car import DT_CTRL, structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR as HYUNDAI +from opendbc.car.toyota.values import CAR as TOYOTA +from opendbc.car.vehicle_model import VehicleModel +from openpilot.common.params import Params +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.selfdrive.controls.controlsd import Controls +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.torque_extension import selected_policy +from openpilot.starpilot.lateral.controller_selection import ( + DOCUMENT_KEY, ControllerMode, default_selection, policy_for, read_selection, selection_from_bytes, +) +from openpilot.starpilot.lateral.torque_tuning import TorqueSource + + +def cp_for(vehicle): + return interfaces[vehicle].get_non_essential_params(vehicle) + + +def document(**choices): + brands = {'HYUNDAI_IONIQ_6': 'hyundai', 'GENESIS_G70_2020': 'hyundai', 'TOYOTA_COROLLA_TSS2': 'toyota'} + return json.dumps({'version': 1, 'vehicles': {key: {'brand': brands[key], 'mode': value} + for key, value in choices.items()}}, sort_keys=True).encode() + + +class TestControllerSelection(unittest.TestCase): + def test_missing_choice_exact_policy_and_untuned_matrix(self): + expected = ((HYUNDAI.HYUNDAI_IONIQ_6, 'ioniq6'), + (HYUNDAI.GENESIS_G70_2020, 'genesis_g70_2020'), + (TOYOTA.TOYOTA_COROLLA_TSS2, 'corolla_tss2'), + (HYUNDAI.KIA_EV6, None), + (HYUNDAI.HYUNDAI_SONATA, None), + (TOYOTA.TOYOTA_RAV4_TSS2, None)) + for vehicle, policy in expected: + with self.subTest(vehicle=vehicle): + cp = cp_for(vehicle) + self.assertEqual(policy_for(cp), policy) + selected = selection_from_bytes(cp, None) + self.assertEqual(selected.mode, ControllerMode.STARPILOT if policy else ControllerMode.STANDARD) + lateral = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL) + self.assertEqual(lateral.controller_mode, selected.mode) + self.assertEqual(lateral.controller_policy, policy) + self.assertEqual(bool(selected_policy(lateral)), bool(policy)) + + def test_saved_standard_is_per_vehicle_and_invalid_cannot_enable_policy(self): + ioniq = cp_for(HYUNDAI.HYUNDAI_IONIQ_6) + g70 = cp_for(HYUNDAI.GENESIS_G70_2020) + ev6 = cp_for(HYUNDAI.KIA_EV6) + raw = document(HYUNDAI_IONIQ_6='standard', GENESIS_G70_2020='starpilot') + self.assertEqual(selection_from_bytes(ioniq, raw).mode, ControllerMode.STANDARD) + self.assertEqual(selection_from_bytes(g70, raw).mode, ControllerMode.STARPILOT) + self.assertEqual(selection_from_bytes(ev6, raw), default_selection(ev6)) + for invalid in (b'{', b'{"version":true,"vehicles":{}}', b'{"version":2,"vehicles":{}}', + b'{"version":1,"vehicles":{"HYUNDAI_IONIQ_6":{"brand":"toyota","mode":"standard"}}}', + b'{"version":1,"vehicles":{},"vehicles":{}}', b'x' * 4097): + with self.subTest(raw=invalid[:50]): + selected = selection_from_bytes(ioniq, invalid) + self.assertEqual(selected.mode, ControllerMode.STARPILOT) + self.assertEqual(selected.source, 'invalid') + wrong = ioniq.as_reader().as_builder() + wrong.brand = 'toyota' + self.assertIsNone(policy_for(wrong)) + wrong = ioniq.as_reader().as_builder() + wrong.lateralTuning.init('pid') + self.assertIsNone(policy_for(wrong)) + wrong = ioniq.as_reader().as_builder() + wrong.passive = True + self.assertIsNone(policy_for(wrong)) + wrong = ioniq.as_reader().as_builder() + wrong.notCar = True + self.assertIsNone(policy_for(wrong)) + with self.assertRaises(ValueError): + LatControlTorque(ev6.as_reader(), interfaces[HYUNDAI.KIA_EV6](ev6), DT_CTRL, + controller_mode=ControllerMode.STARPILOT) + + def test_standard_skips_policy_constructor_and_ioniq_factor_multiplier(self): + for vehicle in (HYUNDAI.HYUNDAI_IONIQ_6, HYUNDAI.GENESIS_G70_2020, TOYOTA.TOYOTA_COROLLA_TSS2): + with self.subTest(vehicle=vehicle): + cp = cp_for(vehicle) + standard = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL, + controller_mode=ControllerMode.STANDARD) + policy = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL, + controller_mode=ControllerMode.STARPILOT) + self.assertIsNone(selected_policy(standard)) + self.assertIsNotNone(selected_policy(policy)) + self.assertEqual(standard.pid._k_i[1], [0.15]) + self.assertAlmostEqual(standard.torque_params.latAccelFactor, cp.lateralTuning.torque.latAccelFactor) + if vehicle == HYUNDAI.HYUNDAI_IONIQ_6: + self.assertEqual(standard.pid.pos_limit, cp.lateralTuning.torque.latAccelFactor) + self.assertEqual(policy.pid.pos_limit, cp.lateralTuning.torque.latAccelFactor) + self.assertEqual(policy.torque_params.latAccelFactor, + struct.unpack('f', struct.pack('f', cp.lateralTuning.torque.latAccelFactor * 1.22))[0]) + for controller, multiplier in ((standard, 1.0), (policy, 1.22)): + controller.update_torque_parameters(3.3, 0.0, 0.12) + expected = struct.unpack('f', struct.pack('f', 3.3 * multiplier))[0] + self.assertEqual(controller.torque_params.latAccelFactor, expected) + + def test_explicit_policy_preserves_legacy_output_through_reset(self): + sequence = ((False, 2.0, 4.0, -0.0005, False), + (True, 8.0, 2.0, 0.0003, False), + (True, 18.0, -1.0, -0.0004, True), + (False, 0.0, 3.0, 0.0, False), + (True, 5.0, 1.0, 0.0002, False)) + for vehicle in (HYUNDAI.HYUNDAI_IONIQ_6, HYUNDAI.GENESIS_G70_2020, TOYOTA.TOYOTA_COROLLA_TSS2): + with self.subTest(vehicle=vehicle): + cp = cp_for(vehicle) + legacy = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL) + selected = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL, + controller_mode=ControllerMode.STARPILOT) + vm = VehicleModel(cp) + cs = structs.CarState.new_message() + params = SimpleNamespace(angleOffsetDeg=0.0, roll=0.02) + for active, speed, angle, curvature, pressed in sequence: + cs.vEgo = speed + cs.steeringAngleDeg = angle + cs.steeringPressed = pressed + before = legacy.update(active, cs, vm, params, False, curvature, False, 0.1) + after = selected.update(active, cs, vm, params, False, curvature, False, 0.1) + self.assertEqual(after[0], before[0]) + self.assertEqual(after[1], before[1]) + self.assertEqual(after[2].to_dict(), before[2].to_dict()) + self.assertEqual(selected.pid.i, legacy.pid.i) + if not active: + self.assertEqual(after[0], 0.0) + + def test_controlsd_latches_choice_while_manual_source_remains_independent(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '0', 'REPLAY': '1', + 'AOL_REPLAY_RUNTIME': '0', 'LANE_CENTERING_REPLAY_RUNTIME': '0'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params = Params() + cp = cp_for(HYUNDAI.HYUNDAI_IONIQ_6) + params.put('CarParams', cp.to_bytes(), block=True) + params.put_bool('AdvancedLateralTune', True, block=True) + legacy = Controls() + self.assertEqual(legacy.lateral_controller_selection, default_selection(cp)) + self.assertIsNotNone(selected_policy(legacy.LaC)) + self.assertEqual(legacy.LaC.pid.pos_limit, cp.lateralTuning.torque.latAccelFactor) + params.put(DOCUMENT_KEY, json.loads(document(HYUNDAI_IONIQ_6='standard')), block=True) + self.assertEqual(read_selection(params, cp).mode, ControllerMode.STANDARD) + first = Controls() + self.assertEqual(first.lateral_controller_selection.mode, ControllerMode.STANDARD) + self.assertIsNone(selected_policy(first.LaC)) + self.assertIsNotNone(first.torque_host) + source = replace(first.torque_host.vehicle, source=TorqueSource.USER, lat_accel_factor=3.3, friction=0.12) + self.assertTrue(first.torque_host.apply(first.LaC, source)) + self.assertEqual(first.LaC.torque_params.latAccelFactor, struct.unpack('f', struct.pack('f', 3.3))[0]) + params.put(DOCUMENT_KEY, json.loads(document(HYUNDAI_IONIQ_6='starpilot')), block=True) + self.assertEqual(first.LaC.controller_mode, ControllerMode.STANDARD) + second = Controls() + self.assertEqual(second.lateral_controller_selection.mode, ControllerMode.STARPILOT) + self.assertIsNotNone(selected_policy(second.LaC)) + self.assertIsNotNone(second.torque_host) + self.assertTrue(second.torque_host.apply(second.LaC, source)) + self.assertEqual(second.LaC.torque_params.latAccelFactor, + struct.unpack('f', struct.pack('f', 3.3 * 1.22))[0]) + self.assertEqual(first.torque_host.vehicle, second.torque_host.vehicle) + + +if __name__ == '__main__': + unittest.main() diff --git a/openpilot/starpilot/lateral/tests/test_corolla_tss2_policy.py b/openpilot/starpilot/lateral/tests/test_corolla_tss2_policy.py new file mode 100644 index 0000000000..bd3d62d532 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_corolla_tss2_policy.py @@ -0,0 +1,74 @@ +"""Compare runtime output against frozen default original StarPilot traces.""" + +import json +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from openpilot.cereal import log +from opendbc.car import structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.toyota.values import CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.torque_extension import selected_policy +from openpilot.starpilot.lateral.corolla_tss2_policy import supported_cp + +FIXTURE = Path(__file__).with_name('fixtures') / 'corolla_tss2_ba901b5f.json' + + +def controller_for(car=CAR.TOYOTA_COROLLA_TSS2): + cp = interfaces[car].get_non_essential_params(car) + return cp, LatControlTorque(cp.as_reader(), interfaces[car](cp), 0.01), VehicleModel(cp) + + +@pytest.mark.parametrize('case_index', range(5)) +def test_frozen_original_numeric_traces(case_index): + fixture = json.loads(FIXTURE.read_text()) + cp, controller, vm = controller_for() + assert selected_policy(controller) is not None + params = log.VehicleParameters.new_message(angleOffsetDeg=0.3, roll=0.006) + for index, frame in enumerate(fixture['cases'][case_index]['frames']): + active, speed, angle, pressed, limited, curve, curve_limited, delay = frame['input'] + cs = SimpleNamespace(vEgo=speed, steeringAngleDeg=angle, steeringPressed=pressed) + torque, desired_angle, state = controller.update(active, cs, vm, params, limited, curve, curve_limited, delay) + assert desired_angle == 0.0 + assert torque == pytest.approx(frame['torque'], abs=1e-12), (case_index, index) + assert [getattr(state, field) for field in fixture['log_fields']] == pytest.approx(frame['log'], abs=1e-12) + assert state.active == frame['active'] + assert state.saturated == frame['saturated'] + assert abs(torque) <= 1.0 + if not active: + assert torque == 0.0 + assert controller.pid.i == 0.0 + + +def test_exact_vehicle_admission_and_upstream_gains(): + cp, controller, _ = controller_for() + assert supported_cp(cp) + assert controller.pid._k_i[1] == [0.35] + for field, value in [('brand', 'hyundai'), ('dashcamOnly', True), ('passive', True), ('steerControlType', structs.CarParams.SteerControlType.angle)]: + changed = cp.as_reader().as_builder() + setattr(changed, field, value) + assert not supported_cp(changed) + native = LatControlTorque(changed.as_reader(), interfaces[CAR.TOYOTA_COROLLA_TSS2](changed), 0.01) + assert selected_policy(native) is None + assert native.pid._k_i[1] == [0.15] + other_cp, other, _ = controller_for(CAR.TOYOTA_RAV4_TSS2) + assert not supported_cp(other_cp) + assert selected_policy(other) is None + assert other.pid._k_i[1] == [0.15] + + +def test_live_source_updates_keep_raw_calibration_and_limits(): + cp, controller, _ = controller_for() + original = cp.lateralTuning.torque.latAccelFactor + assert controller.torque_params.latAccelFactor == original + for _ in range(3): + controller.update_torque_parameters(original * 1.1, 0.025, 0.08) + assert controller.torque_params.latAccelFactor == pytest.approx(original * 1.1) + assert controller.torque_params.latAccelOffset == pytest.approx(0.025) + assert controller.torque_params.friction == pytest.approx(0.08) + assert controller.pid.pos_limit == controller.lateral_accel_from_torque(1.0, controller.torque_params) + assert controller.pid.neg_limit == controller.lateral_accel_from_torque(-1.0, controller.torque_params) diff --git a/openpilot/starpilot/lateral/tests/test_ev6_default.py b/openpilot/starpilot/lateral/tests/test_ev6_default.py new file mode 100644 index 0000000000..8cd81934e9 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_ev6_default.py @@ -0,0 +1,134 @@ +"""Untuned EV6 lateral selection and steering command boundaries.""" + +import unittest + +from opendbc.can import CANParser +from opendbc.car import Bus, DT_CTRL, gen_empty_fingerprint, structs +from opendbc.car.hyundai.carcontroller import CarController, MAX_ANGLE_FRAMES +from opendbc.car.hyundai.carstate import CarState +from opendbc.car.hyundai.hyundaicanfd import CanBus +from opendbc.car.hyundai.interface import CarInterface +from opendbc.car.hyundai.values import CAR, DBC, HyundaiFlags, CarControllerParams +from opendbc.car.lateral import apply_driver_steer_torque_limits +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.torque_extension import selected_policy +from openpilot.starpilot.lateral.torque_runtime import runtime_enabled + + +def ev6(lka_steering: bool): + fingerprint = gen_empty_fingerprint() + if lka_steering: + fingerprint[CanBus(None, fingerprint).CAM][0x50] = 16 + return CarInterface.get_params(CAR.KIA_EV6, fingerprint, [], False, False, False) + + +class NoTorqueSettings: + def get_param_path(self, _key): + raise AssertionError("untuned EV6 must not read saved torque settings") + + +class TestEV6DefaultLateral(unittest.TestCase): + def test_hda_topologies_keep_vehicle_tune_and_generic_controller(self): + for lka_steering in (False, True): + with self.subTest(lka_steering=lka_steering): + cp = ev6(lka_steering) + self.assertEqual(bool(cp.flags & HyundaiFlags.CANFD_LKA_STEER_MSG), lka_steering) + self.assertEqual(cp.lateralTuning.which(), "torque") + self.assertAlmostEqual(cp.lateralTuning.torque.latAccelFactor, 3.2) + self.assertAlmostEqual(cp.lateralTuning.torque.friction, 0.005) + self.assertFalse(runtime_enabled(cp, NoTorqueSettings())) + lateral = LatControlTorque(cp.as_reader(), CarInterface(cp), DT_CTRL) + self.assertIsNone(selected_policy(lateral)) + self.assertAlmostEqual(lateral.torque_params.latAccelFactor, cp.lateralTuning.torque.latAccelFactor) + controller = CarController(DBC[cp.carFingerprint], cp) + self.assertIsNone(controller.ioniq6_longitudinal) + self.assertEqual(controller.params.STEER_MAX, 409) + self.assertEqual(controller.params.STEER_DRIVER_ALLOWANCE, 250) + + def test_opposing_driver_torque_limits_can_command_and_angle_request(self): + for lka_steering in (False, True): + with self.subTest(lka_steering=lka_steering): + cp = ev6(lka_steering) + state = CarState(cp) + state.out = state.update(state.get_can_parsers(cp)) + control = structs.CarControl() + control.enabled = control.latActive = True + control.actuators.torque = 1.0 + controller = CarController(DBC[cp.carFingerprint], cp) + can = CanBus(cp) + name = "LKAS" if lka_steering else "LFA" + bus = can.ACAN if lka_steering else can.ECAN + parser = CANParser(DBC[cp.carFingerprint][Bus.pt], [(name, 0)], bus) + + state.out.steeringTorque = 0 + state.out.vEgoRaw = 15.0 + controller.apply_torque_last = 60 + _, sent = controller.update(control.as_reader(), state, 1_000_000_000) + self.assertLessEqual(controller.apply_torque_last, 62) + self.assertEqual(parser.update((1_000_000_000, sent)), {next(msg[0] for msg in sent if msg[2] == bus)}) + self.assertEqual(parser.vl[name]["ActToiSta"], 1) + + state.out.steeringTorque = -500 + for frame in range(30): + previous = controller.apply_torque_last + _, sent = controller.update(control.as_reader(), state, 1_010_000_000 + frame * 10_000_000) + self.assertLessEqual(controller.apply_torque_last, previous) + self.assertEqual(controller.apply_torque_last, 0) + parser.update((1_310_000_000, sent)) + self.assertEqual(parser.vl[name]["StrTqReqVal"], 0) + + state.out.steeringAngleDeg = 90.0 + state.out.steeringTorque = 0 + requests = [] + for frame in range(MAX_ANGLE_FRAMES + 2): + _, sent = controller.update(control.as_reader(), state, 1_320_000_000 + frame * 10_000_000) + parser.update((1_320_000_000 + frame * 10_000_000, sent)) + requests.append(parser.vl[name]["ActToiSta"]) + self.assertIn(0, requests[-2:]) + + + def test_torque_speed_transition_unwind_and_zero_crossing(self): + cp = ev6(True) + for speed, up, down in ((0., 10, 8), (14.999, 10, 8), (15., 2, 3), (30., 2, 3), + (-1., 2, 3), (float('nan'), 2, 3), (float('inf'), 2, 3)): + with self.subTest(speed=speed): + limits = CarControllerParams(cp, speed) + self.assertEqual((limits.STEER_MAX, limits.STEER_DELTA_UP, limits.STEER_DELTA_DOWN), (409, up, down)) + for sign in (-1, 1): + first = sign * (min(up, down) if sign > 0 else up) + self.assertEqual(apply_driver_steer_torque_limits(sign * 409, 0, 0, limits), first) + self.assertEqual(apply_driver_steer_torque_limits(sign * 409, sign, 0, limits), sign * (1 + up)) + self.assertEqual(apply_driver_steer_torque_limits(0, sign * 409, 0, limits), sign * (409 - down)) + self.assertEqual(apply_driver_steer_torque_limits(-sign * 409, sign, 0, limits), sign * (1 - down)) + state = CarState(cp) + state.out = state.update(state.get_can_parsers(cp)) + control = structs.CarControl() + control.enabled = control.latActive = True + control.actuators.torque = 1.0 + controller = CarController(DBC[cp.carFingerprint], cp) + for tick, (speed, previous, expected) in enumerate(((14., 399, 409), (15., 409, 409), (15., 399, 401))): + state.out.vEgoRaw = speed + controller.apply_torque_last = previous + result, _ = controller.update(control.as_reader(), state, 1_000_000_000 + tick * 10_000_000) + self.assertEqual(result.torqueOutputCan, expected) + + def test_angle_carnival_and_classic_limits_are_unchanged(self): + cp = ev6(True).as_reader().as_builder() + cp.flags |= int(HyundaiFlags.CANFD_ANGLE_STEERING) + for speed in (0., 15.): + limits = CarControllerParams(cp, speed) + self.assertEqual((limits.STEER_MAX, limits.STEER_DELTA_UP, limits.STEER_DELTA_DOWN, limits.STEER_THRESHOLD), (270, 2, 3, 175)) + cp.flags &= ~int(HyundaiFlags.CANFD_ANGLE_STEERING) + cp.carFingerprint = CAR.KIA_CARNIVAL_HEV_4TH_GEN + for speed in (0., 15.): + limits = CarControllerParams(cp, speed) + self.assertEqual((limits.STEER_MAX, limits.STEER_DELTA_UP, limits.STEER_DELTA_DOWN), (409, 2, 3)) + for vehicle, flags, maximum in ((CAR.HYUNDAI_ELANTRA, 0, 255), (CAR.GENESIS_G70_2020, HyundaiFlags.ALT_LIMITS, 270), + (CAR.GENESIS_G70_2020, HyundaiFlags.ALT_LIMITS_2, 170)): + cp.carFingerprint, cp.flags = vehicle, int(flags) + for speed in (0., 15.): + self.assertEqual(CarControllerParams(cp, speed).STEER_MAX, maximum) + + +if __name__ == "__main__": + unittest.main() diff --git a/openpilot/starpilot/lateral/tests/test_genesis_g70_policy.py b/openpilot/starpilot/lateral/tests/test_genesis_g70_policy.py new file mode 100644 index 0000000000..bcae05b2ac --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_genesis_g70_policy.py @@ -0,0 +1,211 @@ +"""Compare runtime output against frozen default original StarPilot traces.""" + +import json +from collections import deque +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import patch + +import pytest + +from openpilot.cereal import log +from opendbc.car import structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.controller_selection import ControllerMode +from openpilot.starpilot.lateral.torque_extension import selected_policy +from openpilot.starpilot.lateral.genesis_g70_policy import supported_cp +from openpilot.starpilot.lateral import genesis_g70_policy + +FIXTURE = Path(__file__).with_name('fixtures') / 'genesis_g70_ba901b5f.json' + + +def controller_for(car=CAR.GENESIS_G70_2020): + cp = interfaces[car].get_non_essential_params(car) + return cp, LatControlTorque(cp.as_reader(), interfaces[car](cp), 0.01), VehicleModel(cp) + + +@pytest.mark.parametrize('case_index', range(5)) +@patch.object(genesis_g70_policy, 'get_genesis_g70_center_measurement_damping_gain', return_value=0.0) +@patch.object(genesis_g70_policy, 'get_genesis_g70_traffic_friction_increment', return_value=0.0) +def test_frozen_base_tuning_numeric_traces(_traffic, _damping, case_index): + # This ba901b5f fixture intentionally tests historical base math, as the + # existing damping isolation does. The new traffic stage is exercised by + # test_genesis_dom_leaves.py against actual friction/current driver state. + fixture = json.loads(FIXTURE.read_text()) + cp, controller, vm = controller_for() + assert selected_policy(controller) is not None + params = log.VehicleParameters.new_message(angleOffsetDeg=0.3, roll=0.006) + for index, frame in enumerate(fixture['cases'][case_index]['frames']): + active, speed, angle, pressed, limited, curve, curve_limited, delay = frame['input'] + cs = SimpleNamespace(vEgo=speed, steeringAngleDeg=angle, steeringPressed=pressed) + torque, desired_angle, state = controller.update(active, cs, vm, params, limited, curve, curve_limited, delay) + assert desired_angle == 0.0 + assert torque == pytest.approx(frame['torque'], abs=1e-12, rel=0), (case_index, index) + assert [getattr(state, field) for field in fixture['log_fields']] == pytest.approx(frame['log'], abs=1e-12, rel=0) + assert state.active == frame['active'] + assert state.saturated == frame['saturated'] + assert abs(torque) <= 1.0 + if not active: + assert torque == 0.0 + assert controller.pid.i == 0.0 + assert selected_policy(controller).prev_output_torque == 0.0 + + +def test_exact_vehicle_admission_and_upstream_gains(): + cp, controller, _ = controller_for() + assert supported_cp(cp) + assert controller.pid._k_i[1] == [0.35] + for field, value in [('brand', 'toyota'), ('dashcamOnly', True), ('passive', True), ('steerControlType', structs.CarParams.SteerControlType.angle)]: + changed = cp.as_reader().as_builder() + setattr(changed, field, value) + assert not supported_cp(changed) + native = LatControlTorque(changed.as_reader(), interfaces[CAR.GENESIS_G70_2020](changed), 0.01) + assert selected_policy(native) is None + assert native.pid._k_i[1] == [0.15] + other_cp, other, _ = controller_for(CAR.GENESIS_G70) + assert not supported_cp(other_cp) + assert selected_policy(other) is None + assert other.pid._k_i[1] == [0.15] + non_torque = cp.as_reader().as_builder() + non_torque.lateralTuning.init('pid') + assert not supported_cp(non_torque) + + +def test_live_source_updates_keep_raw_calibration_and_limits(): + cp, controller, _ = controller_for() + original = cp.lateralTuning.torque.latAccelFactor + assert controller.torque_params.latAccelFactor == original + for _ in range(3): + controller.update_torque_parameters(original * 1.1, 0.025, 0.08) + assert controller.torque_params.latAccelFactor == pytest.approx(original * 1.1) + assert controller.torque_params.latAccelOffset == pytest.approx(0.025) + assert controller.torque_params.friction == pytest.approx(0.08) + assert controller.pid.pos_limit == controller.lateral_accel_from_torque(1.0, controller.torque_params) + assert controller.pid.neg_limit == controller.lateral_accel_from_torque(-1.0, controller.torque_params) + + +def test_driver_override_and_inactive_state_reset(): + _, controller, vm = controller_for() + params = log.VehicleParameters.new_message(angleOffsetDeg=0.3, roll=0.006) + cs = SimpleNamespace(vEgo=4.0, steeringAngleDeg=20.3, steeringPressed=False) + with patch.object(genesis_g70_policy, 'get_genesis_g70_low_speed_angle_damping', + wraps=genesis_g70_policy.get_genesis_g70_low_speed_angle_damping) as damping, \ + patch.object(genesis_g70_policy, 'get_genesis_g70_stabilized_output', + wraps=genesis_g70_policy.get_genesis_g70_stabilized_output) as smoothing, \ + patch.object(controller.pid, 'update', wraps=controller.pid.update) as pid_update: + controller.update(True, cs, vm, params, False, .002, False, .2) + damping.assert_called_once() + smoothing.assert_called_once() + cs.steeringPressed = True + controller.update(True, cs, vm, params, False, .002, False, .2) + assert damping.call_count == 1 + assert smoothing.call_count == 1 + assert pid_update.call_args.kwargs['freeze_integrator'] is True + cs.steeringPressed = False + controller.update(True, cs, vm, params, True, .002, False, .2) + assert pid_update.call_args.kwargs['freeze_integrator'] is True + torque, _, state = controller.update(False, cs, vm, params, False, .002, False, .2) + policy = selected_policy(controller) + assert torque == 0.0 and not state.active and controller.pid.i == 0.0 + assert policy.prev_output_torque == 0.0 + assert policy.jerk_filter.x == 0.0 + assert policy.measurement_rate_filter.x == 0.0 + assert policy.curvature_request_buffer[-1] == .002 + + +def test_preconfigured_manual_torque_values_preserved(): + cp = interfaces[CAR.GENESIS_G70_2020].get_non_essential_params(CAR.GENESIS_G70_2020) + cp.lateralTuning.torque.latAccelFactor = 2.45 + cp.lateralTuning.torque.latAccelOffset = -.031 + cp.lateralTuning.torque.friction = .072 + controller = LatControlTorque(cp.as_reader(), interfaces[CAR.GENESIS_G70_2020](cp), .01) + assert controller.torque_params.latAccelFactor == cp.lateralTuning.torque.latAccelFactor + assert controller.torque_params.latAccelOffset == cp.lateralTuning.torque.latAccelOffset + assert controller.torque_params.friction == cp.lateralTuning.torque.friction + assert controller.pid.pos_limit == controller.lateral_accel_from_torque(1.0, controller.torque_params) + + +@pytest.mark.parametrize('angle', [-85., 0., 85.]) +@pytest.mark.parametrize('pid_output', [-.125, 0., .125]) +def test_angle_taper_uses_actual_sent_torque_direction(angle, pid_output): + outputs = [] + for taper in (lambda *_: 1., genesis_g70_policy.get_genesis_g70_angle_output_scale): + _, controller, vm = controller_for() + cs = SimpleNamespace(vEgo=15., steeringAngleDeg=angle, steeringPressed=True) + params = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=0.) + with patch.object(controller.pid, 'update', return_value=pid_output), \ + patch.object(controller, 'torque_from_lateral_accel', side_effect=lambda value, *_: value), \ + patch.object(genesis_g70_policy, 'get_genesis_g70_angle_output_scale', taper): + output, _, state = controller.update(True, cs, vm, params, False, 0., False, .2) + assert state.active + outputs.append(output) + baseline, tapered = outputs + if pid_output == 0: + assert baseline == tapered == 0 + else: + assert baseline * pid_output < 0 + if baseline * angle > 0: + assert 0 < abs(tapered) < abs(baseline) + else: + assert tapered == pytest.approx(baseline) + + +@pytest.mark.parametrize('mph,desired,measured,jerk,expected', [ + (65., 0., .10, 0., .09), (50., 0., .10, 0., 0.), (55., 0., .10, 0., .045), + (65., .25, .10, 0., .045), (65., 0., .25, 0., .045), + (65., .35, .10, 0., 0.), (65., 0., .35, 0., 0.), + (65., 0., .10, .35, .045), (65., 0., .10, .50, 0.), +]) +def test_center_measurement_damping_gates(mph, desired, measured, jerk, expected): + for direction in (-1., 1.): + gain = genesis_g70_policy.get_genesis_g70_center_measurement_damping_gain( + mph * .44704, desired * direction, measured * direction, jerk * direction) + assert gain == pytest.approx(expected) + + +@pytest.mark.parametrize('direction', (-1., 1.)) +def test_center_measurement_damping_update_and_recovery(direction): + _, controller, vm = controller_for() + cs = SimpleNamespace(vEgo=65. * .44704, steeringAngleDeg=0., steeringPressed=False) + params = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=0.) + controller.update(True, cs, vm, params, False, 0., False, .2) + cs.steeringAngleDeg = -direction * .5 + output, _, moving = controller.update(True, cs, vm, params, False, 0., False, .2) + assert moving.d * direction < 0. + assert abs(moving.d) <= .225 + assert abs(output) <= controller.steer_max + for _ in range(150): + _, _, steady = controller.update(True, cs, vm, params, False, 0., False, .2) + assert steady.d == pytest.approx(0., abs=1e-6) + cs.steeringPressed = True + cs.steeringAngleDeg = 0. + _, _, driver = controller.update(True, cs, vm, params, False, 0., False, .2) + assert driver.d == 0. + cs.steeringPressed = False + controller.update(False, cs, vm, params, False, 0., False, .2) + _, _, resumed = controller.update(True, cs, vm, params, False, 0., False, .2) + assert resumed.d == 0. + cs.vEgo = 40. * .44704 + cs.steeringAngleDeg = -direction * .5 + _, _, low_speed = controller.update(True, cs, vm, params, False, 0., False, .2) + assert low_speed.d == 0. + cs.vEgo = 65. * .44704 + curvature = direction * .8 / cs.vEgo**2 + policy = selected_policy(controller) + policy.curvature_request_buffer = deque([curvature] * policy.request_buffer_len, maxlen=policy.request_buffer_len) + _, _, curve = controller.update(True, cs, vm, params, False, curvature, False, .2) + assert curve.d == 0. + + +@pytest.mark.parametrize('car', (CAR.GENESIS_G70_2020, CAR.GENESIS_G70, CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, CAR.KIA_EV6, CAR.HYUNDAI_IONIQ_6)) +def test_center_damping_isolated_to_selected_g70(car): + cp, controller, vm = controller_for(car) + if car == CAR.GENESIS_G70_2020: + controller = LatControlTorque(cp.as_reader(), interfaces[car](cp), .01, controller_mode=ControllerMode.STANDARD) + cs = SimpleNamespace(vEgo=65. * .44704, steeringAngleDeg=0., steeringPressed=False) + params = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=0.) + with patch.object(genesis_g70_policy, 'get_genesis_g70_center_measurement_damping_gain', side_effect=AssertionError('Wrong vehicle policy')): + controller.update(True, cs, vm, params, False, 0., False, .2) diff --git a/openpilot/starpilot/lateral/tests/test_genesis_gv70_policy.py b/openpilot/starpilot/lateral/tests/test_genesis_gv70_policy.py new file mode 100644 index 0000000000..e9ff649df7 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_genesis_gv70_policy.py @@ -0,0 +1,146 @@ +"""Exact GV70 feedforward tuning through the production torque extension.""" +from collections import deque +import json +import math +import struct +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import patch + +import pytest +from openpilot.cereal import log +from opendbc.car import structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.controller_selection import ControllerMode, policy_for, selection_from_bytes +from openpilot.starpilot.lateral.torque_extension import selected_policy +from openpilot.starpilot.lateral import genesis_gv70_policy as policy +from openpilot.starpilot.lateral import gv70_shaping + + +def controller_for(vehicle=CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, mode=None): + cp=interfaces[vehicle].get_non_essential_params(vehicle) + controller=LatControlTorque(cp.as_reader(),interfaces[vehicle](cp),.01,controller_mode=mode) + cs=SimpleNamespace(vEgo=25.,steeringAngleDeg=0.,steeringPressed=False) + params=log.VehicleParameters.new_message(angleOffsetDeg=0.,roll=0.) + return cp,controller,VehicleModel(cp),cs,params + + +def update(controller,vm,cs,params,curvature=.0002,active=True): + return controller.update(active,cs,vm,params,False,curvature,False,.2) + + +def test_original_derived_default_formula_vectors(): + data=json.loads((Path(__file__).with_name('fixtures')/'gv70_385428f6.json').read_text()) + assert data['source_commit']=='385428f6ea1c183de4ddcd8604f294abec02a9e8' + for row in data['vectors']: + value=getattr(gv70_shaping,row['function'])(*row['input']) + assert value==pytest.approx(row['expected'],abs=1e-14,rel=0),row + + +def test_exact_admission_and_saved_standard_keeps_upstream_controller(): + cp,controller,*_=controller_for() + assert policy.supported_cp(cp) + assert policy_for(cp)=='genesis_gv70_electrified' + assert isinstance(selected_policy(controller),policy.GenesisGV70TorquePolicy) + # Dom retains the full speed-interpolated schedule, ending at0.6, not flat0.6. + assert controller.pid._k_p==[[1,1.5,2.,3.,5,7.5,10,15,30],[250,120,65,30,11.5,5.5,3.5,2.,.6]] + assert controller.pid._k_i==([0],[.35]) + choice=json.dumps({'version':1,'vehicles':{'GENESIS_GV70_ELECTRIFIED_1ST_GEN':{'brand':'hyundai','mode':'standard'}}}).encode() + assert selection_from_bytes(cp,choice).mode==ControllerMode.STANDARD + _,standard,*_=controller_for(mode=ControllerMode.STANDARD) + assert selected_policy(standard) is None + assert standard.pid._k_i==([0],[.15]) + assert standard.pid._k_d==([0],[0.]) + for field,value in (('brand','toyota'),('passive',True),('dashcamOnly',True),('notCar',True), + ('steerControlType',structs.CarParams.SteerControlType.angle)): + changed=cp.as_reader().as_builder();setattr(changed,field,value) + assert not policy.supported_cp(changed) + assert policy_for(changed) is None + changed=cp.as_reader().as_builder();changed.lateralTuning.init('pid') + assert not policy.supported_cp(changed) + for vehicle in (CAR.GENESIS_GV70_1ST_GEN,CAR.GENESIS_GV70_ELECTRIFIED_2ND_GEN): + sibling=interfaces[vehicle].get_non_essential_params(vehicle) + assert not policy.supported_cp(sibling) + assert policy_for(sibling) is None + + +def test_stabilization_only_changes_feedforward_feedback_remains_immediate(): + cp,controller,vm,cs,params=controller_for() + calls=[] + def stable(*values):calls.append(values);return .123 + with patch.object(policy,'get_genesis_gv70_stabilized_output',side_effect=stable): + update(controller,vm,cs,params) + assert not calls # first frame has no previous feedforward + output,_,state=update(controller,vm,cs,params) + assert state.f==pytest.approx(.123) + assert output==pytest.approx(-controller.torque_from_lateral_accel(controller.pid.control,controller.torque_params)) + prior_p=state.p + cs.steeringAngleDeg=.5 + _,_,changed=update(controller,vm,cs,params) + assert changed.f==pytest.approx(.123) + assert changed.p!=pytest.approx(prior_p) + assert calls[-1][1]==pytest.approx(.123) + previous=len(calls) + cs.steeringPressed=True + _,_,pressed=update(controller,vm,cs,params) + assert pressed.d==0 + assert len(calls)==previous + cs.steeringPressed=False + update(controller,vm,cs,params) + assert len(calls)==previous # release tick also bypasses smoothing + update(controller,vm,cs,params,active=False) + assert selected_policy(controller).previous_feedforward is None + update(controller,vm,cs,params,curvature=-.0002) + assert len(calls)==previous + + +def test_measurement_damping_matches_original_schedule_bound_and_has_no_bias(): + _,controller,vm,cs,params=controller_for() + cs.vEgo=30. + update(controller,vm,cs,params,curvature=0) + cs.steeringAngleDeg=-5. + output,_,moving=update(controller,vm,cs,params,curvature=0) + # Assert double-precision PID arithmetic and the exact Float32 telemetry separately. + assert controller.pid.d==pytest.approx(-.3,abs=1e-12), controller.pid.d + assert moving.d==struct.unpack("f",struct.pack("f",-.3))[0], moving.d + assert abs(output)<=controller.steer_max + for _ in range(150):_,_,steady=update(controller,vm,cs,params,curvature=0) + assert steady.d==pytest.approx(0,abs=1e-6) + cs.vEgo=3.;cs.steeringAngleDeg=5. + _,_,low=update(controller,vm,cs,params,curvature=0) + assert low.d==0 + + +@pytest.mark.parametrize('direction',(-1.,1.)) +def test_overshoot_feedback_bypasses_previous_turn_feedforward(direction): + _,controller,vm,cs,params=controller_for() + cs.vEgo=22. + cs.steeringAngleDeg=math.degrees(vm.get_steer_from_curvature(-direction*2.4/cs.vEgo**2,cs.vEgo,0)) + selected=selected_policy(controller) + selected.curvature_request_buffer=deque([direction*.8/cs.vEgo**2]*selected.request_buffer_len,maxlen=selected.request_buffer_len) + selected.previous_feedforward=direction*.8 + with patch.object(policy,'get_genesis_gv70_stabilized_output',return_value=direction*.2): + output,_,state=update(controller,vm,cs,params,curvature=direction*.8/cs.vEgo**2) + assert state.p*direction<0 + assert output*direction>0 + assert abs(output)<=controller.steer_max + + +@pytest.mark.parametrize('vehicle',(CAR.GENESIS_G70_2020,CAR.GENESIS_GV70_1ST_GEN,CAR.KIA_EV6,CAR.HYUNDAI_IONIQ_6)) +def test_measurement_damping_does_not_touch_other_vehicle_policies(vehicle): + _,controller,*_=controller_for(vehicle) + assert not isinstance(selected_policy(controller),policy.GenesisGV70TorquePolicy) + assert max(controller.pid._k_d[1])==0 + + +def test_learned_calibration_updates_stay_raw_without_new_multipliers(): + cp,controller,*_=controller_for() + factor=cp.lateralTuning.torque.latAccelFactor + assert controller.torque_params.latAccelFactor==factor + controller.update_torque_parameters(factor*1.1,.025,.08) + assert controller.torque_params.latAccelFactor==pytest.approx(factor*1.1,rel=1e-6) + assert controller.torque_params.latAccelOffset==pytest.approx(.025) + assert controller.torque_params.friction==pytest.approx(.08) diff --git a/openpilot/starpilot/lateral/tests/test_genesis_torque_context.py b/openpilot/starpilot/lateral/tests/test_genesis_torque_context.py new file mode 100644 index 0000000000..9bf6f56711 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_genesis_torque_context.py @@ -0,0 +1,74 @@ +"""Actual Genesis controller proof for exact mapped Dom leaves.""" + +from collections import deque +from types import SimpleNamespace +import math +import pytest +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.cereal import log +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.controller_selection import ControllerMode +from openpilot.starpilot.lateral import genesis_gv70_policy as gv70 +from openpilot.starpilot.lateral import genesis_g70_policy as g70 + + +@pytest.mark.parametrize('direction', [-1.0, 1.0]) +def test_gv70_actual_undertracking_curve_matches_undamped_policy(monkeypatch, direction): + cp = interfaces[CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN].get_non_essential_params(CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN) + + def run(): + controller = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), 0.01, controller_mode=ControllerMode.STARPILOT) + vm = VehicleModel(cp) + cs = SimpleNamespace(vEgo=30.0, steeringAngleDeg=math.degrees(vm.get_steer_from_curvature(-direction * 0.6 / 900.0, 30.0, 0.0)), steeringPressed=False) + params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0) + request = direction * 0.8 / 900.0 + policy = controller.starpilot_extension.policy + policy.curvature_request_buffer = deque([request] * policy.request_buffer_len, maxlen=policy.request_buffer_len) + rows = [] + for _ in range(100): + out, _, state = controller.update(True, cs, vm, params, False, request, False, 0.2) + assert math.isfinite(out) and abs(out) <= controller.steer_max + 1e-6 + rows.append((out, state.desiredLateralAccel, state.p, state.i, state.d, state.f)) + return rows, controller.torque_params.to_dict(), (controller.pid.pos_limit, controller.pid.neg_limit) + + actual = run() + for name in ('get_genesis_gv70_unwind_ff_scale', 'get_genesis_gv70_high_speed_error_scale', 'get_genesis_gv70_reversal_output_scale'): + monkeypatch.setattr(gv70, name, lambda *_: 1.0) + assert actual == run() + + +@pytest.mark.parametrize('direction', [-1.0, 1.0]) +@pytest.mark.parametrize('pressed', [False, True]) +def test_g70_actual_friction_receives_traffic_increment_only_without_driver_override(monkeypatch, direction, pressed): + cp = interfaces[CAR.GENESIS_G70_2020].get_non_essential_params(CAR.GENESIS_G70_2020) + actual = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), 0.01, controller_mode=ControllerMode.STARPILOT) + actual.update_torque_parameters(3.0, 0.02, 0.12) + tune = actual.torque_params.to_dict() + limits = (actual.pid.pos_limit, actual.pid.neg_limit, actual.steer_max) + cs = SimpleNamespace(vEgo=5.0, steeringAngleDeg=0.0, steeringPressed=pressed) + params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0) + vm = VehicleModel(cp) + calls = [] + original = g70.get_friction + + def capture(error, deadzone, threshold, tune): + calls.append(threshold) + return original(error, deadzone, threshold, tune) + + monkeypatch.setattr(g70, 'get_friction', capture) + for _ in range(100): + out, _, state = actual.update(True, cs, vm, params, False, direction * 0.0001, False, 0.2) + assert math.isfinite(out) and abs(out) <= actual.steer_max + 1e-6 + base = g70.get_genesis_g70_friction_threshold(cs.vEgo, state.desiredLateralAccel, state.desiredLateralJerk) + inc = ( + 0.0 if pressed else g70.get_genesis_g70_traffic_friction_increment(cs.vEgo, state.desiredLateralAccel, state.desiredLateralJerk, direction * 0.0001, 0.0) + ) + assert calls[-1] == pytest.approx(base + inc) + assert (inc == 0.0) is pressed + assert actual.torque_params.to_dict() == tune + assert (actual.pid.pos_limit, actual.pid.neg_limit, actual.steer_max) == limits + calls.clear() + out, _, state = actual.update(False, cs, vm, params, False, direction * 0.0001, False, 0.2) + assert out == 0.0 and not state.active and calls == [] diff --git a/openpilot/starpilot/lateral/tests/test_hkg_centre_context.py b/openpilot/starpilot/lateral/tests/test_hkg_centre_context.py new file mode 100644 index 0000000000..098f4f3d70 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_hkg_centre_context.py @@ -0,0 +1,111 @@ +"""Dom center leaves qualify only through actual per-vehicle selected controllers.""" + +from types import SimpleNamespace +import math +import pytest +from openpilot.cereal import log +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection +from openpilot.starpilot.lateral import hkg_vehicle_shaping as hkg +from openpilot.starpilot.lateral import ioniq6_policy as iq6 + + +@pytest.mark.parametrize('vehicle,speed', [(CAR.KIA_NIRO_EV, 65.0 * 0.44704), (CAR.KIA_CARNIVAL_HEV_4TH_GEN, 10.0), (CAR.HYUNDAI_IONIQ_6, 10.0)]) +@pytest.mark.parametrize('direction', [-1.0, 1.0]) +@pytest.mark.parametrize('pressed', [False, True]) +def test_actual_same_frame_threshold_error_output_and_override(monkeypatch, vehicle, speed, direction, pressed): + cp = interfaces[vehicle].get_non_essential_params(vehicle) + + def run(neutral=False): + controller = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), 0.01, controller_mode=ControllerMode.STARPILOT) + controller.update_torque_parameters(3.0, 0.02, 0.12) + _policy = controller.starpilot_extension.policy + cs = SimpleNamespace(vEgo=speed, steeringAngleDeg=0.0, steeringPressed=pressed) + params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0) + calls = [] + bound = controller.friction + + def capture(error, deadzone, threshold, car, request): + calls.append((error, deadzone, threshold)) + return bound(error, deadzone, threshold, car, request) + + monkeypatch.setattr(controller, 'friction', capture) + if neutral: + if vehicle == CAR.KIA_NIRO_EV: + monkeypatch.setattr(hkg, 'KIA_NIRO_EV_CENTER_FRICTION_THRESHOLD_GAIN', 0.0) + elif vehicle == CAR.KIA_CARNIVAL_HEV_4TH_GEN: + monkeypatch.setattr(hkg, 'KIA_CARNIVAL_HEV_CENTER_FRICTION_INCREMENT', 0.0) + monkeypatch.setattr(hkg, 'KIA_CARNIVAL_HEV_CENTER_ERROR_REDUCTION', 0.0) + else: + monkeypatch.setattr(iq6, 'get_ioniq_6_2023_center_friction_increment', lambda *_: 0.0) + for _ in range(150): + out, _, state = controller.update(True, cs, VehicleModel(cp), params, False, direction * 0.02 / speed**2, False, 0.2) + assert math.isfinite(out) and abs(out) <= controller.steer_max + 1e-6 + live = (out, state.to_dict(), calls[-1], controller.torque_params.to_dict(), (controller.pid.pos_limit, controller.pid.neg_limit, controller.steer_max)) + calls.clear() + off, _, offstate = controller.update(False, cs, VehicleModel(cp), params, False, 0.0, False, 0.2) + assert off == 0.0 and not offstate.active and not calls + return live + + actual = run() + baseline = run(True) + assert actual[3:] == baseline[3:] + if pressed: + assert actual == baseline + else: + assert actual[2][0:2] == baseline[2][0:2] # Friction error/deadzone retain original stage. + assert actual[2][2] > baseline[2][2] + assert abs(actual[1]['f']) < abs(baseline[1]['f']) + if vehicle == CAR.KIA_CARNIVAL_HEV_4TH_GEN: + assert abs(actual[1]['error']) < abs(baseline[1]['error']) + assert actual[1]['error'] == pytest.approx(0.75 * baseline[1]['error']) + else: + assert actual[1]['error'] == baseline[1]['error'] + + +def test_niro_saved_standard_and_other_factory_have_no_new_profile(): + cp = interfaces[CAR.KIA_NIRO_EV].get_non_essential_params(CAR.KIA_NIRO_EV) + assert default_selection(cp).mode == ControllerMode.STANDARD + standard = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), 0.01) + assert standard.starpilot_extension is None + for field in ('passive', 'dashcamOnly', 'notCar'): + other = cp.as_reader().as_builder() + setattr(other, field, True) + from openpilot.starpilot.lateral.hkg_vehicle_policy import policy_for + + assert policy_for(other) is None + + +@pytest.mark.parametrize('future,error,jerk,speed', [(0.5, 0.02, 0.0, 10.0), (0.02, 0.4, 0.0, 10.0), (0.02, 0.02, 1.0, 10.0), (0.02, 0.02, 0.0, 25.0)]) +def test_hev_center_leaf_releases_for_turn_error_jerk_and_high_speed(future, error, jerk, speed): + assert hkg.get_kia_carnival_hev_center_weight(speed, 0.02, 0.02 - error, jerk, future) == 0.0 + + +@pytest.mark.parametrize('vehicle', [CAR.KIA_CARNIVAL_HEV_4TH_GEN, CAR.HYUNDAI_IONIQ_6]) +def test_actual_previous_driver_release_does_not_immediately_apply_center_delta(monkeypatch, vehicle): + cp = interfaces[vehicle].get_non_essential_params(vehicle) + controller = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), 0.01, controller_mode=ControllerMode.STARPILOT) + cs = SimpleNamespace(vEgo=10.0, steeringAngleDeg=0.0, steeringPressed=True) + params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0) + vm = VehicleModel(cp) + for _ in range(150): + controller.update(True, cs, vm, params, False, 0.0002, False, 0.2) + calls = [] + original = controller.friction + + def capture(error, deadzone, threshold, car, request): + calls.append(threshold) + return original(error, deadzone, threshold, car, request) + + monkeypatch.setattr(controller, 'friction', capture) + cs.steeringPressed = False + _, _, state = controller.update(True, cs, vm, params, False, 0.0002, False, 0.2) + if vehicle == CAR.KIA_CARNIVAL_HEV_4TH_GEN: + assert controller.starpilot_extension.policy.hev_center_weight == 0.0 + assert calls[-1] == pytest.approx(hkg.get_kia_carnival_friction_threshold(10.0, state.desiredLateralAccel, state.desiredLateralJerk)) + else: + taper = iq6.get_ioniq_6_center_taper_scale(state.desiredLateralAccel, 10.0) + assert calls[-1] == pytest.approx(iq6.get_ioniq_6_friction_threshold(10.0, state.desiredLateralAccel, state.desiredLateralJerk) / max(taper, 1e-3)) diff --git a/openpilot/starpilot/lateral/tests/test_hkg_highway_friction.py b/openpilot/starpilot/lateral/tests/test_hkg_highway_friction.py new file mode 100644 index 0000000000..9ced0e2e08 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_hkg_highway_friction.py @@ -0,0 +1,156 @@ +import math +from types import SimpleNamespace +import unittest +from unittest import mock + +from opendbc.car import structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR, HyundaiFlags +from opendbc.car.lateral import get_friction +from opendbc.car.vehicle_model import VehicleModel +from openpilot.common.constants import CV +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.selfdrive.controls.lib import latcontrol_torque +from openpilot.starpilot.lateral.controller_selection import ControllerMode + + +def controller_for(vehicle=CAR.KIA_EV6, mode=None, **changes): + cp = interfaces[vehicle].get_non_essential_params(vehicle) + ci = interfaces[vehicle](cp) + for key, value in changes.items(): + setattr(cp, key, value) + return cp, LatControlTorque(cp.as_reader(), ci, .01, controller_mode=mode) + + +class HKGHighwayFrictionTests(unittest.TestCase): + def test_registry_and_wrong_brand_flag_collision(self): + admitted = 0 + for vehicle in CAR: + cp = interfaces[vehicle].get_non_essential_params(vehicle) + if cp.lateralTuning.which() != 'torque': + continue + _, controller = controller_for(vehicle) + expected = bool(cp.flags & HyundaiFlags.CANFD and not cp.flags & HyundaiFlags.CANFD_ANGLE_STEERING and + vehicle != CAR.HYUNDAI_IONIQ_6 and + cp.steerControlType == structs.CarParams.SteerControlType.torque and + not (cp.passive or cp.dashcamOnly or cp.notCar)) + self.assertEqual(controller.hkg_canfd_torque, expected, str(vehicle)) + admitted += expected + self.assertGreater(admitted, 20) + for changes in ( + {'brand': 'toyota'}, {'flags': 0}, {'passive': True}, {'dashcamOnly': True}, {'notCar': True}, + {'steerControlType': structs.CarParams.SteerControlType.angle}, + {'flags': int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_ANGLE_STEERING)}, + ): + with self.subTest(changes=changes): + _, controller = controller_for(**changes) + self.assertFalse(controller.hkg_canfd_torque) + cs = SimpleNamespace(vEgo=75 * CV.MPH_TO_MS, steeringPressed=False) + self.assertEqual(controller.friction(.1, 0., .2, cs, .1), get_friction(.1, 0., .2, controller.torque_params)) + + def test_threshold_floor_soft_deadband_and_existing_larger_threshold(self): + _, controller = controller_for() + for mph, threshold in ((0, .39), (50, .39), (60, .39 + (.65 - .39) * 2 / 3), + (65, .65), (75, .78), (100, .78)): + with self.subTest(mph=mph): + cs = SimpleNamespace(vEgo=mph * CV.MPH_TO_MS, steeringPressed=False) + with mock.patch.object(latcontrol_torque, 'get_friction', wraps=get_friction) as friction: + controller.friction(.1, .01, .39, cs, 0.) + error, deadzone, actual_threshold, params = friction.call_args.args + weight = min(max((mph - 50) / 15, 0.), 1.) + self.assertAlmostEqual(error, .1 - .04 * weight) + self.assertEqual(deadzone, .01) + self.assertAlmostEqual(actual_threshold, threshold) + self.assertIs(params, controller.torque_params) + cs = SimpleNamespace(vEgo=75 * CV.MPH_TO_MS, steeringPressed=False) + with mock.patch.object(latcontrol_torque, 'get_friction', wraps=get_friction) as friction: + controller.friction(.1, 0., .9, cs, 0.) + self.assertEqual(friction.call_args.args[2], .9) + + def test_curve_relief_driver_handoff_zero_and_peak_compensation(self): + _, controller = controller_for() + for mph, setpoint, pressed in ((50, 0., False), (75, .65, False), (75, -.65, False), + (75, 2., False), (75, 0., True)): + with self.subTest(mph=mph, setpoint=setpoint, pressed=pressed): + cs = SimpleNamespace(vEgo=mph * CV.MPH_TO_MS, steeringPressed=pressed) + for error in (-1., -.1, 0., .1, 1.): + self.assertEqual(controller.friction(error, 0., .39, cs, setpoint), + get_friction(error, 0., .39, controller.torque_params)) + cs = SimpleNamespace(vEgo=75 * CV.MPH_TO_MS, steeringPressed=False) + self.assertAlmostEqual(controller.friction(0., 0., .39, cs, 0.), 0.) + self.assertAlmostEqual(controller.friction(.02, 0., .39, cs, 0.), 0.) + for sign in (-1, 1): + self.assertEqual(controller.friction(sign * 2., 0., .39, cs, 0.), + get_friction(sign * 2., 0., .39, controller.torque_params)) + + def test_symmetry_continuity_and_friction_bound(self): + _, controller = controller_for() + cap = controller.torque_params.latAccelFactor * controller.torque_params.friction + epsilon = 1e-7 + for mph in (30., 50., 60., 65., 75., 100.): + cs = SimpleNamespace(vEgo=mph * CV.MPH_TO_MS, steeringPressed=False) + for accel in (0., .25, .45, .65, 1.): + for error in (-2., -.1, -.04, 0., .04, .1, 2.): + value = controller.friction(error, 0., .39, cs, accel) + negative = controller.friction(-error, 0., .39, cs, -accel) + self.assertTrue(math.isfinite(value)) + self.assertAlmostEqual(value, -negative) + self.assertLessEqual(abs(value), cap) + self.assertLessEqual(abs(value), abs(get_friction(error, 0., .39, controller.torque_params)) + 1e-15) + left = controller.friction(error - epsilon, 0., .39, cs, accel - epsilon) + right = controller.friction(error + epsilon, 0., .39, cs, accel + epsilon) + self.assertLess(abs(left - right), 1e-5) + left = controller.friction(.1, 0., .39, SimpleNamespace(vEgo=cs.vEgo - epsilon, steeringPressed=False), accel) + right = controller.friction(.1, 0., .39, SimpleNamespace(vEgo=cs.vEgo + epsilon, steeringPressed=False), accel) + self.assertLess(abs(left - right), 1e-5) + + def test_standard_and_custom_paths_only_change_friction(self): + for vehicle in (CAR.KIA_EV6, CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_6, CAR.KIA_CARNIVAL_HEV_4TH_GEN, + CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN, CAR.HYUNDAI_TUCSON_4TH_GEN): + for mode in ControllerMode: + for speed, accel, quiet in ((20., .1, False), (32., .1, True), (32., .8, False)): + with self.subTest(vehicle=str(vehicle), mode=mode, speed=speed, accel=accel): + cp, controller = controller_for(vehicle, mode) + _, baseline = controller_for(vehicle, mode) + baseline.hkg_canfd_torque = False + vm = VehicleModel(cp) + cs = structs.CarState.new_message(vEgo=speed, steeringAngleDeg=0., steeringPressed=False) + cs.gearShifter = structs.CarState.GearShifter.drive + params = SimpleNamespace(angleOffsetDeg=0., roll=0.) + traces = [] + for candidate in (controller, baseline): + values = [] + for _ in range(100): + candidate.update(False, cs, vm, params, False, accel / speed ** 2, False, .3) + for _ in range(100): + output, _, state = candidate.update(True, cs, vm, params, False, accel / speed ** 2, False, .3) + values.append((output, state.desiredLateralAccel, state.desiredLateralJerk, state.p, state.i, state.f)) + traces.append(values) + self.assertEqual(controller.torque_params.to_dict(), baseline.torque_params.to_dict()) + self.assertEqual((controller.pid.pos_limit, controller.pid.neg_limit, controller.steer_max), + (baseline.pid.pos_limit, baseline.pid.neg_limit, baseline.steer_max)) + for changed, original in zip(*traces, strict=True): + self.assertEqual(changed[1:5], original[1:5]) + if quiet and vehicle != CAR.HYUNDAI_IONIQ_6: + self.assertLess(abs(traces[0][-1][5]), abs(traces[1][-1][5])) + self.assertLess(abs(traces[0][-1][0]), abs(traces[1][-1][0])) + else: + self.assertEqual(traces[0], traces[1]) + + + def test_ioniq6_preserves_original_threshold_and_error(self): + for mode in ControllerMode: + _, controller = controller_for(CAR.HYUNDAI_IONIQ_6, mode) + self.assertFalse(controller.hkg_canfd_torque) + for mph in (0., 30., 50., 60., 65., 75., 100.): + for setpoint in (-1., -.65, -.25, 0., .25, .65, 1.): + for pressed in (False, True): + cs = SimpleNamespace(vEgo=mph * CV.MPH_TO_MS, steeringPressed=pressed) + for error in (-1., -.1, -.04, -.02, 0., .02, .04, .1, 1.): + for threshold in (.2, .39, .9): + with self.subTest(mode=mode, mph=mph, setpoint=setpoint, pressed=pressed, + error=error, threshold=threshold): + with mock.patch.object(latcontrol_torque, 'get_friction', wraps=get_friction) as friction: + value = controller.friction(error, .01, threshold, cs, setpoint) + self.assertEqual(friction.call_args.args, (error, .01, threshold, controller.torque_params)) + self.assertEqual(value, get_friction(error, .01, threshold, controller.torque_params)) diff --git a/openpilot/starpilot/lateral/tests/test_hkg_torque_context.py b/openpilot/starpilot/lateral/tests/test_hkg_torque_context.py new file mode 100644 index 0000000000..90fd9c308b --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_hkg_torque_context.py @@ -0,0 +1,119 @@ +"""Exact Dom leaves must reach actual selected controllers, preserving native ownership.""" + +from types import SimpleNamespace +import math +import pytest +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.cereal import log +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection +from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping + + +@pytest.mark.parametrize( + 'vehicle,helper,speed,curvature', + [ + (CAR.KIA_STINGER_2022, 'get_kia_stinger_2022_friction_threshold', 28.0, 0.00005), + (CAR.HYUNDAI_IONIQ_5, 'get_ioniq_5_low_speed_output_limit', 1.0, 0.02), + (CAR.KIA_CARNIVAL_HEV_4TH_GEN, 'get_kia_carnival_unwind_output_scale', 15.0, 0.0002), + (CAR.HYUNDAI_KONA_EV_2022, 'get_kona_ev_2022_friction_threshold', 110.0 / 3.6, 0.00065), + ], +) +@pytest.mark.parametrize('direction', [-1.0, 1.0]) +@pytest.mark.parametrize('pressed', [False, True]) +def test_actual_controller_context_defaults_live_tune_limits_and_off(monkeypatch, vehicle, helper, speed, curvature, direction, pressed): + cp = interfaces[vehicle].get_non_essential_params(vehicle) + ci = interfaces[vehicle](cp) + assert default_selection(cp).mode == ControllerMode.STANDARD + native = LatControlTorque(cp.as_reader(), ci, 0.01, controller_mode=ControllerMode.STANDARD) + assert native.starpilot_extension is None + actual = LatControlTorque(cp.as_reader(), ci, 0.01, controller_mode=ControllerMode.STARPILOT) + actual.update_torque_parameters(3.0, 0.02, 0.12) + tune = actual.torque_params.to_dict() + limits = (actual.pid.pos_limit, actual.pid.neg_limit, actual.steer_max) + calls = [] + original = getattr(shaping, helper) + + def capture(*args, **kwargs): + calls.append((args, kwargs)) + return original(*args, **kwargs) + + monkeypatch.setattr(shaping, helper, capture) + cs = SimpleNamespace(vEgo=speed, steeringAngleDeg=0.0, steeringPressed=pressed) + params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0) + for _ in range(80): + output, _, state = actual.update(True, cs, VehicleModel(cp), params, False, direction * curvature, False, 0.28) + assert math.isfinite(output) and abs(output) <= actual.steer_max + 1e-6 + assert state.active and calls + args, kwargs = calls[-1] + if vehicle == CAR.KIA_STINGER_2022: + assert args[3] == pytest.approx(direction * curvature * speed**2) + assert args[4] is pressed + elif vehicle == CAR.HYUNDAI_IONIQ_5: + assert args[3] == pytest.approx(0.0 if pressed else direction * curvature) + assert math.isfinite(args[4]) + elif vehicle == CAR.KIA_CARNIVAL_HEV_4TH_GEN: + assert (kwargs['output_torque'] is None) is pressed + assert actual.torque_params.to_dict() == tune + assert (actual.pid.pos_limit, actual.pid.neg_limit, actual.steer_max) == limits + calls.clear() + output, _, state = actual.update(False, cs, VehicleModel(cp), params, False, direction * curvature, False, 0.28) + assert output == 0.0 and not state.active and calls == [] + + +@pytest.mark.parametrize('direction', [-1.0, 1.0]) +def test_stinger_threshold_increment_bypasses_override_and_future_turn(direction): + speed = 60.0 * 0.44704 + old = shaping.get_kia_stinger_2022_friction_threshold(speed, direction * 0.1, 0.0, 0.0, True) + assert shaping.get_kia_stinger_2022_friction_threshold(speed, direction * 0.1, 0.0) == pytest.approx(old + 0.2) + assert shaping.get_kia_stinger_2022_friction_threshold(speed, direction * 0.1, 0.0, direction * 0.4) == pytest.approx(old) + + +@pytest.mark.parametrize('direction', [-1.0, 1.0]) +def test_ioniq5_creep_floor_only_for_requested_turn_direction(direction): + old = shaping.get_ioniq_5_low_speed_output_limit(0.01, 0.0, 1.0) + actual = shaping.get_ioniq_5_low_speed_output_limit(0.01, 0.0, 1.0, direction * 0.02, direction * 0.1) + assert actual >= 0.3 and actual > old + assert shaping.get_ioniq_5_low_speed_output_limit(0.01, 0.0, 1.0, direction * 0.02, -direction * 0.1) == old + + +@pytest.mark.parametrize('direction', [-1.0, 1.0]) +def test_carnival_countersteer_is_retained(direction): + assert shaping.get_kia_carnival_unwind_output_scale(direction * 0.2, direction * 0.5, -direction, 25.0, output_torque=-direction * 0.1) == 1.0 + assert shaping.get_kia_carnival_unwind_output_scale(direction * 0.2, direction * 0.5, -direction, 25.0, output_torque=direction * 0.1) < 1.0 + + +@pytest.mark.parametrize( + 'vehicle,helper', [(CAR.KIA_STINGER_2022, 'get_kia_stinger_2022_friction_threshold'), (CAR.HYUNDAI_IONIQ_5, 'get_ioniq_5_low_speed_output_limit')] +) +def test_actual_first_driver_release_tick_preserves_dom_override_bypass(monkeypatch, vehicle, helper): + cp = interfaces[vehicle].get_non_essential_params(vehicle) + controller = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), 0.01, controller_mode=ControllerMode.STARPILOT) + cs = SimpleNamespace(vEgo=28.0, steeringAngleDeg=0.0, steeringPressed=True) + params = log.VehicleParameters.new_message(angleOffsetDeg=0.0, roll=0.0) + vm = VehicleModel(cp) + controller.update(True, cs, vm, params, False, 0.00005, False, 0.2) + calls = [] + original = getattr(shaping, helper) + + def capture(*args, **kwargs): + calls.append((args, kwargs)) + return original(*args, **kwargs) + + monkeypatch.setattr(shaping, helper, capture) + cs.steeringPressed = False + controller.update(True, cs, vm, params, False, 0.00005, False, 0.2) + args, _ = calls[-1] + if vehicle == CAR.KIA_STINGER_2022: + assert args[4] is True + else: + assert args[3] == 0.0 + calls.clear() + controller.update(True, cs, vm, params, False, 0.00005, False, 0.2) + args, _ = calls[-1] + if vehicle == CAR.KIA_STINGER_2022: + assert args[4] is False + else: + assert args[3] == pytest.approx(0.00005) diff --git a/openpilot/starpilot/lateral/tests/test_hkg_vehicle_policy.py b/openpilot/starpilot/lateral/tests/test_hkg_vehicle_policy.py new file mode 100644 index 0000000000..0b7fc32b04 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_hkg_vehicle_policy.py @@ -0,0 +1,138 @@ +import pytest + +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection, replace_mode, selection_from_bytes +from openpilot.starpilot.lateral.hkg_vehicle_policy import HKGVehicleTorquePolicy, PROFILES, policy_for + + +CASES = [(vehicle, profile, factor) for profile, (vehicles, factor) in PROFILES.items() for vehicle in sorted(vehicles, key=str)] + + +@pytest.mark.parametrize('vehicle,profile,factor', CASES) +def test_exact_profile_defaults_and_saved_controller_selection(vehicle, profile, factor): + cp = interfaces[vehicle].get_non_essential_params(vehicle) + assert policy_for(cp) == profile + expected = (ControllerMode.STARPILOT if vehicle in (CAR.KIA_FORTE_2019_NON_SCC, CAR.KIA_FORTE_2021_NON_SCC) + else ControllerMode.STANDARD) + assert default_selection(cp).mode == expected + for raw in (b'{', b'{"version":true,"vehicles":{}}', b'{"version":1,"vehicles":{},"vehicles":{}}', b'x' * 4097): + assert selection_from_bytes(cp, raw).mode == expected + unrelated = interfaces[CAR.HYUNDAI_IONIQ_6].get_non_essential_params(CAR.HYUNDAI_IONIQ_6) + assert selection_from_bytes(cp, replace_mode(None, unrelated, ControllerMode.STANDARD)).mode == expected + saved = replace_mode(None, cp, ControllerMode.STARPILOT) + choice = selection_from_bytes(cp, saved) + assert choice.mode == ControllerMode.STARPILOT + assert choice.policy == profile + ci = interfaces[vehicle](cp) + default = LatControlTorque(cp.as_reader(), ci, 0.01) + assert bool(default.starpilot_extension) == (expected == ControllerMode.STARPILOT) + standard_choice = selection_from_bytes(cp, replace_mode(saved, cp, ControllerMode.STANDARD)) + standard = LatControlTorque(cp.as_reader(), ci, 0.01, controller_mode=standard_choice.mode) + assert standard.starpilot_extension is None + assert standard.torque_params.latAccelFactor == pytest.approx(cp.lateralTuning.torque.latAccelFactor) + controller = LatControlTorque(cp.as_reader(), ci, 0.01, controller_mode=choice.mode) + policy = controller.starpilot_extension.policy + assert isinstance(policy, HKGVehicleTorquePolicy) + assert policy.profile == profile + assert controller.torque_params.latAccelFactor == pytest.approx(cp.lateralTuning.torque.latAccelFactor * factor) + assert controller.pid.pos_limit == pytest.approx(controller.lateral_accel_from_torque(controller.steer_max, cp.lateralTuning.torque)) + for _ in range(3): + controller.update_torque_parameters(3.0, 0.02, 0.12) + assert controller.torque_params.latAccelFactor == pytest.approx(3.0 * factor) + assert controller.torque_params.latAccelOffset == pytest.approx(0.02) + assert controller.torque_params.friction == pytest.approx(0.12) + assert selection_from_bytes(cp, replace_mode(saved, cp, ControllerMode.STANDARD)).mode == ControllerMode.STANDARD + + +@pytest.mark.parametrize('vehicle,profile,factor', CASES) +def test_unadmitted_and_cross_vehicle_choices_are_not_applied(vehicle, profile, factor): + cp = interfaces[vehicle].get_non_essential_params(vehicle) + for field in ('passive', 'dashcamOnly', 'notCar'): + other = cp.as_reader().as_builder() + setattr(other, field, True) + assert policy_for(other) is None + cp.brand = 'gm' + assert policy_for(cp) is None + cp.brand = 'hyundai' + saved = replace_mode(None, cp, ControllerMode.STARPILOT) + other = interfaces[CAR.HYUNDAI_IONIQ_5].get_non_essential_params(CAR.HYUNDAI_IONIQ_5) + if vehicle != CAR.HYUNDAI_IONIQ_5: + assert selection_from_bytes(other, saved).mode == ControllerMode.STANDARD + + +@pytest.mark.parametrize('vehicle,profile,factor', CASES) +def test_invalid_torque_contract_cannot_select_vehicle_policy(vehicle, profile, factor): + cp = interfaces[vehicle].get_non_essential_params(vehicle) + for field, value in (('latAccelFactor', 0.0), ('latAccelFactor', float('nan')), ('latAccelOffset', float('inf')), ('friction', -1.0)): + other = cp.as_reader().as_builder() + setattr(other.lateralTuning.torque, field, value) + assert policy_for(other) is None + assert selection_from_bytes(other, None).mode == ControllerMode.STANDARD + cp.lateralTuning.init('pid') + assert policy_for(cp) is None + + +@pytest.mark.parametrize('kph,desired,increment', ( + (60., 0., 0.), (100., .65, 0.), (110., .65, .30), (120., .65, .30), + (120., -.65, .30), (120., 1.6, .15), (120., -1.6, .15), + (120., 2.0, 0.), (140., 0., .30), (140., -2., 0.), +)) +def test_kona_2022_high_speed_friction_boundaries(monkeypatch, kph, desired, increment): + from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping + threshold = shaping.get_kona_ev_2022_friction_threshold(kph / 3.6, desired) + monkeypatch.setattr(shaping, 'KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_GAIN', 0.) + baseline = shaping.get_kona_ev_2022_friction_threshold(kph / 3.6, desired) + assert threshold == pytest.approx(baseline + increment) + assert baseline <= threshold <= baseline + .30 + + +def test_kona_2022_high_speed_friction_reaches_selected_controller(monkeypatch): + from types import SimpleNamespace + from openpilot.cereal import log + from opendbc.car.vehicle_model import VehicleModel + from opendbc.car.lateral import get_friction + from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping + + cp = interfaces[CAR.HYUNDAI_KONA_EV_2022].get_non_essential_params(CAR.HYUNDAI_KONA_EV_2022) + cs = SimpleNamespace(vEgo=120. / 3.6, steeringAngleDeg=0., steeringPressed=False) + params = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=0.) + + def run(): + controller = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), .01, controller_mode=ControllerMode.STARPILOT) + for _ in range(60): + output, _, state = controller.update(True, cs, VehicleModel(cp), params, False, .0001, False, .28) + return controller, output, state + + controller, output, state = run() + threshold = shaping.get_kona_ev_2022_friction_threshold(cs.vEgo, .65) + standard = shaping.get_standard_friction_threshold(cs.vEgo) + for direction in (-1., 1.): + small = get_friction(direction * .1, 0., threshold, controller.torque_params) + assert abs(small) < abs(get_friction(direction * .1, 0., standard, controller.torque_params)) + assert small * direction > 0. + assert get_friction(direction, 0., threshold, controller.torque_params) == pytest.approx( + get_friction(direction, 0., standard, controller.torque_params)) + monkeypatch.setattr(shaping, 'KONA_EV_2022_HIGH_SPEED_FRICTION_THRESHOLD_GAIN', 0.) + baseline, base_output, base_state = run() + assert state.active and base_state.active + assert state.desiredLateralAccel == pytest.approx(base_state.desiredLateralAccel) + assert state.p == pytest.approx(base_state.p) + assert abs(state.f) < abs(base_state.f) + assert abs(output) < abs(base_output) + assert controller.steer_max == baseline.steer_max + + +@pytest.mark.parametrize('vehicle', (CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_KONA_NON_SCC)) +def test_kona_2022_friction_does_not_change_sibling_policies(monkeypatch, vehicle): + from openpilot.starpilot.lateral import hkg_vehicle_shaping as shaping + + def unexpected(*_): + raise AssertionError('Kona 2022 friction used by sibling policy') + + monkeypatch.setattr(shaping, 'get_kona_ev_2022_friction_threshold', unexpected) + cp = interfaces[vehicle].get_non_essential_params(vehicle) + controller = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), .01, controller_mode=ControllerMode.STARPILOT) + threshold, scale = controller.starpilot_extension.policy.friction_context(.1, 0., 0., 120. / 3.6) + assert threshold > 0. and scale > 0. diff --git a/openpilot/starpilot/lateral/tests/test_ioniq6_center_assist.py b/openpilot/starpilot/lateral/tests/test_ioniq6_center_assist.py new file mode 100644 index 0000000000..8a1439d345 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_ioniq6_center_assist.py @@ -0,0 +1,44 @@ +import math +import pytest + +from openpilot.starpilot.lateral.ioniq6_policy import get_ioniq_6_low_speed_angle_assist_torque as assist + + +@pytest.mark.parametrize('speed', [0., 1., 3., 6., 20.]) +def test_small_error_sign_change_is_continuous(speed): + epsilon = 1e-5 + # No model error means no additional angle-assist torque. + assert assist(0., 0., 0., speed) == 0. + assert abs(assist(epsilon, 0., 0., speed) - assist(-epsilon, 0., 0., speed)) < epsilon + # Same continuity while tracking a turn and releasing error across zero. + assert abs(assist(10. + epsilon, 10., 0., speed) - assist(10. - epsilon, 10., 0., speed)) < epsilon + + +def test_center_assist_is_bounded_and_does_not_reverse_direction(): + for speed in (0., 1., 3., 7., 20.): + for desired in (-.9, -.1, 0., .1, .9): + output = assist(desired, 0., 0., speed) + assert math.isfinite(output) and abs(output) <= 1. + assert output * desired <= 0. + + +@pytest.mark.parametrize('desired, actual, torque, speed, expected', [ + (10., 0., 0., 1., -.39409320626365096), + (-10., 0., 0., 1., .39409320626365096), + (0., 10., 0., 1., .07246171113121364), + (0., -10., 0., 1., -.07246171113121364), + (10., 8., -.5, 3., -.5322587905665043), +]) +def test_existing_turn_and_unwind_response_outside_center_is_unchanged(desired, actual, torque, speed, expected): + assert assist(desired, actual, torque, speed) == expected + + +def test_center_correction_retains_output_saturation_and_symmetry(): + for speed in (0., 1., 3., 10., 40.): + for actual in (0., 10., 30.): + for error in (-.9, -.5, -.01, 0., .01, .5, .9): + for torque in (-1., -.99, -.5, 0., .5, .99, 1.): + positive = assist(actual + error, actual, torque, speed) + negative = assist(-actual - error, -actual, -torque, speed) + assert -1. <= positive <= 1. + assert positive == -negative diff --git a/openpilot/starpilot/lateral/tests/test_ioniq6_firmware_buffers.py b/openpilot/starpilot/lateral/tests/test_ioniq6_firmware_buffers.py new file mode 100644 index 0000000000..796d145373 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_ioniq6_firmware_buffers.py @@ -0,0 +1,70 @@ +"""Firmware buffer representation must not change the selected Ioniq 6 controller.""" + +from types import SimpleNamespace + +import pytest + +from opendbc.car import structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.controller_selection import ControllerMode +from openpilot.starpilot.lateral.ioniq6_policy import is_ioniq_6_2025_model +from openpilot.starpilot.lateral.torque_extension import selected_policy + + +class FirmwareReader: + """Expose a bytes-like firmware boundary while preserving the real factory CarParams.""" + + def __init__(self, cp, versions): + self.cp = cp + self.carFw = [SimpleNamespace(fwVersion=version) for version in versions] + + def __getattr__(self, name): + return getattr(self.cp, name) + + +@pytest.mark.parametrize('buffer_type', [bytes, bytearray, memoryview]) +@pytest.mark.parametrize('versions, expected', [([b'IONIQ6-230915', b'IONIQ6-240206'], True), ([b'IONIQ6-230915'], False), ([], False)]) +def test_real_controller_firmware_selection_and_output_match_bytes(buffer_type, versions, expected): + cp = interfaces[CAR.HYUNDAI_IONIQ_6].get_non_essential_params(CAR.HYUNDAI_IONIQ_6) + native = cp.init('carFw', len(versions)) + for fw, version in zip(native, versions, strict=True): + fw.fwVersion = version + ci = interfaces[CAR.HYUNDAI_IONIQ_6](cp) + wrapped = FirmwareReader(cp.as_reader(), [buffer_type(version) for version in versions]) + assert is_ioniq_6_2025_model(wrapped) is expected + baseline = LatControlTorque(cp.as_reader(), ci, DT_CTRL, controller_mode=ControllerMode.STARPILOT) + actual = LatControlTorque(wrapped, ci, DT_CTRL, controller_mode=ControllerMode.STARPILOT) + actual_policy = selected_policy(actual) + baseline_policy = selected_policy(baseline) + assert actual_policy is not None and baseline_policy is not None + assert actual_policy.is_2025 is expected + assert baseline_policy.is_2025 is expected + assert actual.torque_params.to_dict() == baseline.torque_params.to_dict() + assert (actual.pid.pos_limit, actual.pid.neg_limit) == (baseline.pid.pos_limit, baseline.pid.neg_limit) + state = structs.CarState() + state.gearShifter = structs.CarState.GearShifter.drive + params = SimpleNamespace(angleOffsetDeg=0.0, roll=0.0) + for speed, curvature, pressed in ((2.0, 0.0001, False), (12.0, -0.0003, False), (20.0, 0.0004, True)): + state.vEgo = speed + state.steeringPressed = pressed + args = (True, state, VehicleModel(cp), params, False, curvature, False, 0.1) + actual_output = actual.update(*args) + baseline_output = baseline.update(*args) + assert actual_output[0:2] == baseline_output[0:2] + assert actual_output[2].to_dict() == baseline_output[2].to_dict() + + +@pytest.mark.parametrize( + 'cp', + [ + SimpleNamespace(carFingerprint=CAR.HYUNDAI_IONIQ_6), + SimpleNamespace(carFingerprint=CAR.HYUNDAI_IONIQ_6, carFw=None), + SimpleNamespace(carFingerprint=CAR.HYUNDAI_IONIQ_5, carFw=[SimpleNamespace(fwVersion=b'230915 240206')]), + ], +) +def test_missing_or_other_vehicle_firmware_keeps_standard_gate(cp): + assert not is_ioniq_6_2025_model(cp) diff --git a/openpilot/starpilot/lateral/tests/test_ioniq6_policy.py b/openpilot/starpilot/lateral/tests/test_ioniq6_policy.py new file mode 100644 index 0000000000..db6d4225d3 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_ioniq6_policy.py @@ -0,0 +1,159 @@ +"""Frozen 678af783 Ioniq 6 traces and committed-parent non-Ioniq traces.""" + +import json +import math +from pathlib import Path +from types import SimpleNamespace +import unittest + +from opendbc.car import structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR as HYUNDAI +from opendbc.car.toyota.values import CAR as TOYOTA +from opendbc.car.vehicle_model import VehicleModel +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.torque_extension import selected_policy +from openpilot.starpilot.lateral import ioniq6_policy as tune +from openpilot.starpilot.lateral.torque_shaping import INTERP_SPEEDS + +TESTDATA = Path(__file__).parent / 'testdata' + + +def _rows(car, sequence, *, profile='2023', matched_frozen_cp=False, passive=False, direct_original_controller=False): + cp = interfaces[car].get_non_essential_params(car) + cp.passive = passive + if matched_frozen_cp: + for name, value in (('mass', 2084.0), ('wheelbase', 2.97), ('centerToFront', 1.188), + ('steerRatio', 14.26), ('tireStiffnessFactor', 0.65)): + setattr(cp, name, value) + cp.lateralTuning.torque.latAccelFactor = 3.0 + cp.lateralTuning.torque.latAccelOffset = 0.0 + cp.lateralTuning.torque.friction = 0.09 + cp.lateralTuning.torque.steeringAngleDeadzoneDeg = 0.0 + if profile == '2025': + versions = cp.init('carFw', 2) + versions[0].fwVersion = b'IONIQ6-230915' + versions[1].fwVersion = b'IONIQ6-240206' + ci = interfaces[car](cp) + controller = LatControlTorque(cp.as_reader(), ci, DT_CTRL, turn_assist=True) + if direct_original_controller: + # The older frozen vectors called the controller directly, before the + # original controlsd drive loop replaced the PID gain curve each tick. + controller.pid._k_p = [INTERP_SPEEDS, tune.KP_INTERP] + vm = VehicleModel(cp) + cs = structs.CarState.new_message() + cs.gearShifter = structs.CarState.GearShifter.drive + params = SimpleNamespace(angleOffsetDeg=0.0, roll=0.03 if matched_frozen_cp else 0.02) + rows = [] + for active, speed, angle, curvature, pressed in sequence: + cs.vEgo = speed + cs.steeringAngleDeg = angle + cs.steeringPressed = pressed + output, _, state = controller.update(active, cs, vm, params, False, curvature, False, 0.1) + rows.append([output, state.desiredLateralAccel, state.desiredLateralJerk, state.p, state.i, state.f]) + return controller, rows + + +class Ioniq6PolicyTests(unittest.TestCase): + def test_ioniq_drive_loop_uses_original_flat_proportional_gain(self): + cp = interfaces[HYUNDAI.HYUNDAI_IONIQ_6].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_6) + cp.lateralTuning.torque.latAccelFactor = 3.0 + cp.lateralTuning.torque.friction = 0.09 + controller = LatControlTorque(cp.as_reader(), interfaces[HYUNDAI.HYUNDAI_IONIQ_6](cp), DT_CTRL) + self.assertEqual(controller.pid._k_p, [[0.0], [0.6]]) + vm = VehicleModel(cp) + cs = structs.CarState.new_message() + params = SimpleNamespace(angleOffsetDeg=0.0, roll=0.0) + sequence = ((False, 2.0, 0.0002, False), + (True, 2.0, 0.0002, False), + (True, 8.0, -0.0003, False), + (True, 8.0, -0.0003, True), + (True, 20.0, 0.0004, True), + (True, 20.0, 0.0004, False), + (False, 0.0, 0.0, False), + (True, 2.0, -0.0002, False)) + for active, speed, curvature, pressed in sequence: + cs.vEgo = speed + cs.steeringAngleDeg = 0.0 + cs.steeringPressed = pressed + output, _, state = controller.update(active, cs, vm, params, False, curvature, False, 0.1) + self.assertTrue(math.isfinite(output)) + self.assertEqual(state.active, active) + if active: + self.assertAlmostEqual(state.p, 0.6 * state.error, places=6) + self.assertAlmostEqual(output, state.output, places=6) + else: + self.assertEqual(output, 0.0) + self.assertEqual(controller.pid._k_p, [[0.0], [0.6]]) + controller.update_torque_parameters(3.2, 0.01, 0.10) + self.assertEqual(controller.pid._k_p, [[0.0], [0.6]]) + + def test_actual_frozen_controller_sequences_and_pure_calibration(self): + sequence = ([(False, 0.3, 10.0, -0.001, False)] * 2 + + [(True, 0.3, 10.0, -0.001, False)] * 4 + + [(True, 2.0, 5.0, -0.001, False)] * 4 + + [(True, 20.0, 2.0, 0.0003, False)] * 4 + + [(True, 20.0, 2.0, -0.0003, True)] * 2 + + [(False, 0.0, 8.0, -0.001, False)] * 2 + + [(True, 3.0, 8.0, -0.001, False)] * 4) + samples = ((0.3, .1, .2, 8., 3., -.2), + (12., -.5, -1.3, -6., -10., .2), + (27., .02, .8, 1., 0., -.1)) + helpers = [] + for speed, accel, jerk, angle, actual, torque in samples: + helpers.append([ + tune.get_ioniq_6_ff_scale(accel, jerk, speed), + tune.get_ioniq_6_directional_taper_scale(accel, jerk, speed), + tune.get_ioniq_6_friction_threshold(speed, accel, jerk), + tune.get_ioniq_6_center_taper_scale(accel, speed), + tune.get_ioniq_6_low_speed_angle_assist_torque(angle, actual, torque, speed), + tune.get_ioniq_6_2025_center_output_scale(accel, speed), + ]) + for profile in ('2023', '2025'): + with self.subTest(profile=profile): + frozen = json.loads((TESTDATA / f'ioniq6_frozen_{profile}.json').read_text()) + controller, actual = _rows(HYUNDAI.HYUNDAI_IONIQ_6, sequence, profile=profile, + matched_frozen_cp=True, direct_original_controller=True) + self.assertIsNotNone(selected_policy(controller)) + self.assertEqual(selected_policy(controller).is_2025, profile == '2025') + self.assertAlmostEqual(controller.torque_params.latAccelFactor, frozen['factor'], places=6) + for row, expected in zip(actual, frozen['rows'], strict=True): + for value, reference in zip(row, expected, strict=True): + self.assertAlmostEqual(value, reference, places=6) + for row, expected in zip(helpers, frozen['helpers'], strict=True): + for value, reference in zip(row, expected, strict=True): + self.assertAlmostEqual(value, reference, places=6) + + def test_firmware_gate_and_live_factor_do_not_stack(self): + cp = interfaces[HYUNDAI.HYUNDAI_IONIQ_6].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_6) + self.assertFalse(tune.is_ioniq_6_2025_model(cp)) + single = cp.init('carFw', 1) + single[0].fwVersion = b'IONIQ6-230915' + self.assertFalse(tune.is_ioniq_6_2025_model(cp)) + both = cp.init('carFw', 2) + both[0].fwVersion = b'IONIQ6-230915' + both[1].fwVersion = b'IONIQ6-240206' + self.assertTrue(tune.is_ioniq_6_2025_model(cp)) + controller = LatControlTorque(cp.as_reader(), interfaces[HYUNDAI.HYUNDAI_IONIQ_6](cp), DT_CTRL) + raw = cp.lateralTuning.torque.latAccelFactor + for _ in range(3): + controller.update_torque_parameters(raw, 0.02, 0.10) + self.assertAlmostEqual(controller.torque_params.latAccelFactor, raw * 1.22, places=6) + self.assertAlmostEqual(controller.torque_params.latAccelOffset, 0.02, places=6) + self.assertAlmostEqual(controller.torque_params.friction, 0.10, places=6) + + def test_other_cars_match_committed_parent_trace(self): + sequence = ([(False, 2., 5., -.0005, False)] * 2 + + [(True, 2., 5., -.0005, False)] * 3 + + [(True, 15., -1., .0004, False)] * 3 + + [(True, 25., -2., .0001, True)] * 2 + + [(False, 0., 5., -.001, False)] * 2) + parent = json.loads((TESTDATA / 'non_ioniq_parent.json').read_text()) + for car in (TOYOTA.TOYOTA_COROLLA_TSS2, HYUNDAI.HYUNDAI_IONIQ_5): + with self.subTest(car=str(car)): + controller, actual = _rows(car, sequence, passive=car == TOYOTA.TOYOTA_COROLLA_TSS2) + self.assertIsNone(selected_policy(controller)) + for row, expected in zip(actual, parent[str(car)], strict=True): + for value, reference in zip(row, expected, strict=True): + self.assertAlmostEqual(value, reference, places=7) diff --git a/openpilot/starpilot/lateral/tests/test_lane_centering.py b/openpilot/starpilot/lateral/tests/test_lane_centering.py new file mode 100644 index 0000000000..b915e5ec2d --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_lane_centering.py @@ -0,0 +1,322 @@ +"""Native model-message fixtures for the standalone curvature contribution.""" +from dataclasses import replace +import math +from types import SimpleNamespace +from typing import Any, cast +import unittest +from unittest.mock import patch + +from openpilot.cereal import messaging +from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature +from openpilot.starpilot.lateral.lane_centering import (ControlMode, LaneCenteringController, LaneCenteringInput, + LaneCenteringSettings, MAX_CORRECTION, raw_correction) + + +XS = [float(i) for i in range(51)] + + +def model(*, left=-1.5, right=2.1, path=0.0, path_std=0.1, prob=0.9, std=0.1): + message = messaging.new_message("modelV2") + m = message.modelV2 + m.init("laneLines", 4) + for line, y in zip(m.laneLines, (0.0, left, right, 0.0), strict=True): + line.x = XS + line.y = [y] * len(XS) + m.laneLineProbs = [0.0, prob, prob, 0.0] + m.laneLineStds = [0.0, std, std, 0.0] + m.position.x = XS + m.position.y = [path] * len(XS) + m.position.yStd = [path_std] * len(XS) + return m + + +def observation(m=None, **updates): + source = LaneCenteringInput(m if m is not None else model(), 1, True, 0.0002, 20.0, + ControlMode.LATERAL_ONLY, True, LaneCenteringSettings(True, 0.0, 0.0, True), 0.01) + return replace(source, **updates) + + +def settled(controller, source, ticks=300): + result = None + for _ in range(ticks): + result = controller.update(source) + assert result is not None + return result + + +class TestLaneCentering(unittest.TestCase): + def test_reader_cache_detects_mutation_samples_and_reset(self): + from openpilot.starpilot.lateral import lane_centering as module + builder = model() + reader = builder.as_reader() + source = observation(reader) + controller = LaneCenteringController() + with patch.object(module, '_path_xy', wraps=module._path_xy) as evaluate: + first = controller.update(source) + second = controller.update(source) + self.assertGreater(second.correction, first.correction) + self.assertEqual(evaluate.call_count, 4) + builder.laneLines[2].y = [2.5] * len(XS) + controller.update(source) + self.assertEqual(evaluate.call_count, 8) + builder.laneLineProbs = [0., .1, .1, 0.] + self.assertEqual(controller.update(source).reason, 'unqualified_model') + self.assertEqual(evaluate.call_count, 12) + controller.update(replace(source, model_sample_id=0)) + self.assertEqual(evaluate.call_count, 16) + controller.update(replace(source, model=reader.as_builder().as_reader())) + self.assertEqual(evaluate.call_count, 20) + controller.reset() + controller.update(source) + self.assertEqual(evaluate.call_count, 24) + controller.update(replace(source, time_discontinuity=True)) + controller.update(source) + self.assertEqual(evaluate.call_count, 28) + + def test_explicit_axis_authority(self): + for mode, allowed in ((ControlMode.OFF, False), (ControlMode.LATERAL_ONLY, True), + (ControlMode.LONGITUDINAL_ONLY, False), (ControlMode.COMBINED, True)): + with self.subTest(mode=mode): + result = LaneCenteringController().update(observation(mode=mode)) + self.assertEqual(result.correction > 0.0, allowed) + assert result.candidate_curvature is not None + self.assertAlmostEqual(result.candidate_curvature, 0.0002 + result.correction) + + def test_held_model_sample_advances_filter_as_in_frozen_controller(self): + controller = LaneCenteringController() + source = observation() + first = controller.update(source).correction + second = controller.update(source).correction + raw = raw_correction(source.model, 20.0, 0.0, 0.0) + assert raw is not None + expected_target = raw * 0.30 * 1.5 + self.assertAlmostEqual(first, expected_target * (1.0 - math.exp(-0.01 / 0.4))) + self.assertAlmostEqual(second, expected_target * (1.0 - math.exp(-0.02 / 0.4))) + self.assertGreater(second, first) + + def test_strength_increases_sublimit_correction_without_changing_default(self): + source = observation() + standard, explicit, stronger = LaneCenteringController(), LaneCenteringController(), LaneCenteringController() + for _ in range(100): + before = standard.update(source) + same = explicit.update(replace(source, settings=replace(source.settings, strength=1.5))) + boosted = stronger.update(replace(source, settings=replace(source.settings, strength=3.0))) + self.assertEqual(before, same) + self.assertAlmostEqual(boosted.correction, before.correction * 2, delta=1e-15) + self.assertLess(abs(boosted.correction), MAX_CORRECTION) + + def test_strength_preserves_absolute_filter_and_axis_bounds(self): + for strength in (0.5, 1.5, 3.0): + controller = LaneCenteringController() + source = observation(model(left=-0.3, right=3.3), settings=LaneCenteringSettings(True, strength=strength)) + previous = 0.0 + for index in range(200): + source = replace(source, model=model(left=-0.3, right=3.3) if index < 100 else model(left=-3.3, right=0.3)) + correction = controller.update(source).correction + self.assertLessEqual(abs(correction), MAX_CORRECTION) + self.assertLessEqual(abs(correction - previous), 2 * MAX_CORRECTION * (1 - math.exp(-0.01 / 0.4)) + 1e-15) + previous = correction + self.assertEqual(controller.update(replace(source, lateral_active=False)).correction, 0) + self.assertEqual(controller.update(replace(source, time_discontinuity=True)).correction, 0) + for invalid in (True, float("nan"), 0.49, 3.001): + self.assertEqual(LaneCenteringController().update(replace(observation(), settings=LaneCenteringSettings(True, strength=invalid))).reason, + "invalid_input") + + def test_stronger_centering_remains_bounded_with_lane_uncertainty_and_sign_changes(self): + for speed in (5., 35.): + controller = LaneCenteringController() + previous = 0. + for index in range(120): + dt = (.01, .02, .05)[index % 3] + displacement = (.079, .081, .159, .161)[index % 4] * (1 if index < 60 else -1) + source = observation(model(left=-1.8 + displacement, right=1.8 + displacement, std=.29999998211860657), + speed_mps=speed, elapsed_seconds=dt, + settings=LaneCenteringSettings(True, e2e_authority=0., strength=3.0)) + result = controller.update(source) + self.assertEqual(result.reason, "qualified") + self.assertLessEqual(abs(result.correction), MAX_CORRECTION) + self.assertLessEqual(abs(result.correction - previous), + 2 * MAX_CORRECTION * (1 - math.exp(-dt / .4)) + 1e-15) + previous = result.correction + self.assertEqual(controller.update(replace(source, driver_override=True)).correction, 0.) + + def test_strength_does_not_replace_model_path_authority(self): + shifted = model(path=0.5) + for authority in (0.0, 0.5, 1.0): + source = observation(shifted, settings=LaneCenteringSettings(True, e2e_authority=authority)) + normal = settled(LaneCenteringController(), source).correction + stronger = settled(LaneCenteringController(), replace(source, settings=replace(source.settings, strength=3.0))).correction + self.assertAlmostEqual(stronger, normal * 2, delta=1e-15) + + def test_native_builder_trace_matches_frozen_source(self): + # Frozen controller at 678af783; native modelV2, ten held 0.01 s ticks. + expected = (8.147727264677505e-06, 1.6094286426420937e-05, 2.3844644343388891e-05, + 3.1403645241574984e-05, 3.8776013742606983e-05, 4.5966357816788907e-05, + 5.2979171663232737e-05, 5.9818838518878981e-05, 6.6489633398162377e-05, + 7.2995725765035352e-05) + controller = LaneCenteringController() + source = observation() + source = replace(source, settings=replace(source.settings, strength=1.0)) + for reference in expected: + self.assertAlmostEqual(controller.update(source).correction, reference, delta=1e-15) + + def test_filter_elapsed_time_matches_repeated_frozen_10ms_ticks(self): + source = observation() + ten_ms, twenty_ms = LaneCenteringController(), LaneCenteringController() + for _ in range(10): + result_ten = ten_ms.update(source) + for _ in range(5): + result_twenty = twenty_ms.update(replace(source, elapsed_seconds=0.02)) + self.assertAlmostEqual(result_ten.correction, result_twenty.correction, delta=1e-15) + + def test_candidate_passes_through_existing_curvature_limiter(self): + result = settled(LaneCenteringController(), observation()) + self.assertIsNotNone(result.candidate_curvature) + clipped, _ = clip_curvature(20.0, 0.0, result.candidate_curvature, 0.0) + self.assertGreater(clipped, 0.0) + self.assertLessEqual(clipped, result.candidate_curvature) + + def test_both_directions_offset_deadband_and_bounds(self): + right = settled(LaneCenteringController(), observation(model())).correction + left = settled(LaneCenteringController(), observation(model(left=-2.1, right=1.5))).correction + self.assertGreater(right, 0.0) + self.assertLess(left, 0.0) + self.assertEqual(settled(LaneCenteringController(), observation(model(left=-1.75, right=1.85))).correction, 0.0) + settings = LaneCenteringSettings(True, 0.3, 0.0) + narrow = observation(model(left=-1.3, right=1.3), settings=settings) + safe_limit = replace(narrow, settings=replace(settings, offset_m=0.2)) + self.assertAlmostEqual(settled(LaneCenteringController(), narrow).correction, + settled(LaneCenteringController(), safe_limit).correction) + extreme = settled(LaneCenteringController(), observation(model(left=0.0, right=3.0))) + self.assertAlmostEqual(extreme.correction, MAX_CORRECTION, delta=1e-6) + + def test_e2e_break_in_requires_confident_path(self): + shifted = model(left=-1.0, right=2.6) + no_e2e = settled(LaneCenteringController(), observation(shifted)).correction + full_e2e = settled(LaneCenteringController(), observation( + shifted, settings=LaneCenteringSettings(True, 0.0, 1.0))).correction + uncertain = settled(LaneCenteringController(), observation( + model(left=-1.0, right=2.6, path_std=0.6), settings=LaneCenteringSettings(True, 0.0, 1.0))).correction + self.assertGreater(no_e2e, 0.0) + self.assertAlmostEqual(full_e2e, 0.0) + self.assertAlmostEqual(uncertain, no_e2e) + + def test_hard_gates_reset_and_reenable(self): + for change in ("disabled", "override", "lane_change", "low_speed", "invalid_model", "long_only"): + with self.subTest(change=change): + controller = LaneCenteringController() + source = observation() + before = settled(controller, source).correction + if change == "disabled": + blocked = replace(source, settings=replace(source.settings, enabled=False)) + elif change == "override": + blocked = replace(source, driver_override=True) + elif change == "low_speed": + blocked = replace(source, speed_mps=4.9) + elif change == "invalid_model": + blocked = replace(source, model_valid=False) + elif change == "long_only": + blocked = replace(source, mode=ControlMode.LONGITUDINAL_ONLY) + else: + changed_model = model() + changed_model.meta.laneChangeState = "laneChangeStarting" + blocked = replace(source, model=changed_model) + self.assertGreater(before, 0.0) + self.assertEqual(controller.update(blocked).correction, 0.0) + reacquired = controller.update(source).correction + self.assertGreater(reacquired, 0.0) + self.assertLess(reacquired, before) + + def test_signal_and_confidence_release_then_reacquire(self): + controller = LaneCenteringController() + source = observation() + before = settled(controller, source).correction + signal = controller.update(replace(source, turn_signal_active=True)) + self.assertGreater(signal.correction, 0.0) + self.assertLess(signal.correction, before) + self.assertEqual(signal.reason, "signal_release") + no_pause = replace(source, settings=replace(source.settings, pause_on_signal=False), turn_signal_active=True) + self.assertGreater(controller.update(no_pause).correction, signal.correction) + fading = controller.update(replace(source, model=model(prob=0.2))) + self.assertGreater(fading.correction, 0.0) + self.assertLess(fading.correction, before) + self.assertEqual(fading.reason, "unqualified_model") + self.assertGreater(controller.update(source).correction, fading.correction) + + def test_signal_pause_precedes_lane_change_as_in_frozen_source(self): + controller = LaneCenteringController() + source = observation() + before = settled(controller, source).correction + changed_model = model() + changed_model.meta.laneChangeState = "laneChangeStarting" + signaled = controller.update(replace(source, model=changed_model, turn_signal_active=True)) + self.assertEqual(signaled.reason, "signal_release") + self.assertGreater(signaled.correction, 0.0) + self.assertLess(signaled.correction, before) + unsignaled = controller.update(replace(source, model=changed_model)) + self.assertEqual(unsignaled.reason, "lane_change") + self.assertEqual(unsignaled.correction, 0.0) + + def test_unqualified_native_models_have_no_new_correction(self): + for bad_model in (model(prob=0.2), model(std=0.4), model(left=-0.5, right=0.5), + model(path=float("nan"))): + with self.subTest(model=bad_model): + self.assertEqual(LaneCenteringController().update(observation(bad_model)).correction, 0.0) + + def test_missing_lookahead_and_nonmonotonic_path_rejected(self): + short = model() + short.laneLines[1].x = XS[:10] + short.laneLines[1].y = [-1.5] * 10 + self.assertIsNone(raw_correction(short, 20.0, 0.0, 0.0)) + reversed_path = model() + reversed_path.position.x = list(reversed(XS)) + self.assertIsNone(raw_correction(reversed_path, 20.0, 0.0, 0.0)) + + def test_malformed_inputs_rejected_without_unsafe_candidate(self): + for updates in ({"base_curvature": float("nan")}, {"speed_mps": float("inf")}, + {"elapsed_seconds": float("nan")}, {"elapsed_seconds": -0.01}, + {"base_curvature": True}, {"base_curvature": "0.0002"}, + {"speed_mps": False}, {"speed_mps": "20"}, + {"elapsed_seconds": True}, {"elapsed_seconds": "0.01"}, + {"settings": LaneCenteringSettings(True, float("nan"))}, + {"settings": LaneCenteringSettings(True, True)}, + {"settings": LaneCenteringSettings(True, cast(Any, "0.2"))}, + {"settings": LaneCenteringSettings(True, 0.0, cast(Any, "1.0"))}, + {"time_discontinuity": "false"}): + with self.subTest(updates=updates): + controller = LaneCenteringController() + settled(controller, observation()) + result = controller.update(observation(**updates)) + self.assertEqual(result.correction, 0.0) + self.assertEqual(result.reason, "invalid_input") + if "base_curvature" in updates: + self.assertIsNone(result.candidate_curvature) + + def test_invalid_observation_type_is_inert(self): + controller = LaneCenteringController() + settled(controller, observation()) + result = controller.update(cast(Any, object())) + self.assertEqual(result.reason, "invalid_input") + self.assertIsNone(result.candidate_curvature) + self.assertEqual(result.correction, 0.0) + + def test_numeric_strings_in_model_path_rejected(self): + native_model = model() + wrong_path = SimpleNamespace(x=[str(x) for x in XS], y=native_model.position.y, + yStd=native_model.position.yStd) + changed_model = SimpleNamespace(laneLines=native_model.laneLines, laneLineProbs=native_model.laneLineProbs, + laneLineStds=native_model.laneLineStds, position=wrong_path) + self.assertIsNone(raw_correction(changed_model, 20.0, 0.0, 0.0)) + + def test_time_discontinuity_explicitly_resets(self): + controller = LaneCenteringController() + settled(controller, observation()) + result = controller.update(observation(elapsed_seconds=0.2, time_discontinuity=True)) + self.assertEqual(result.reason, "time_discontinuity") + self.assertEqual(result.correction, 0.0) + + def test_elapsed_time_does_not_infer_a_discontinuity(self): + controller = LaneCenteringController() + result = controller.update(observation(elapsed_seconds=0.2)) + self.assertEqual(result.reason, "qualified") + self.assertGreater(result.correction, 0.0) diff --git a/openpilot/starpilot/lateral/tests/test_lane_change_defaults.py b/openpilot/starpilot/lateral/tests/test_lane_change_defaults.py new file mode 100644 index 0000000000..57925f572e --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_lane_change_defaults.py @@ -0,0 +1,22 @@ +import json + +from openpilot.starpilot.lateral.lane_change_preferences import LaneChangePolicy, decode, to_value, read_saved + + +def test_unset_defaults_on_without_writing(tmp_path): + class Source: + def get_param_path(self, key): + return str(tmp_path / key) + source = Source() + for _ in range(3): + assert read_saved(source).policy.one_per_signal + assert not list(tmp_path.iterdir()) + + +def test_explicit_off_survives_document_roundtrip(): + policy = LaneChangePolicy(one_per_signal=False) + raw = json.dumps(to_value(policy)).encode() + for _ in range(3): + saved = decode(raw) + assert saved is not None and not saved.one_per_signal + assert json.dumps(to_value(saved)).encode() == raw diff --git a/openpilot/starpilot/lateral/tests/test_lane_change_smoothing.py b/openpilot/starpilot/lateral/tests/test_lane_change_smoothing.py new file mode 100644 index 0000000000..88d83465ce --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_lane_change_smoothing.py @@ -0,0 +1,78 @@ +import math +from dataclasses import replace + +from openpilot.cereal import log +from openpilot.starpilot.lateral.lane_change_preferences import LaneChangePolicy, decode, to_value +from openpilot.starpilot.lateral.lane_change_smoothing import LaneChangeSmoother, limit_rate +from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature +import json + + +def test_duration_document_roundtrip_and_previous_versions(): + for seconds in (3., 5., 8.): + policy = replace(LaneChangePolicy(), duration_s=seconds) + assert decode(json.dumps(to_value(policy)).encode()) == policy + old = to_value(LaneChangePolicy()) + old['version'] = 3 + old.pop('durationS') + assert decode(json.dumps(old).encode()).duration_s == LaneChangePolicy().duration_s + for value in (2.9, 8.1, True, float('nan')): + current = to_value(LaneChangePolicy()) | {'durationS': value} + assert decode(json.dumps(current).encode()) is None + + +def test_rate_envelope_and_release_preserve_native_acceleration_limits(): + for speed in (1., 10., 30.): + smoother = LaneChangeSmoother() + previous = 0. + factors = [] + for tick in range(600): + desired = .005 if tick < 200 else -.005 + factor = smoother.factor(active=True, lane_change_state=log.LaneChangeState.laneChangeStarting if tick < 350 else + log.LaneChangeState.off, speed=speed, minimum_speed=0., duration=6., + previous=previous, desired=desired) + limited = limit_rate(speed, previous, desired, factor) + output, _ = clip_curvature(speed, previous, limited, 0.) + assert 0. < factor <= 1. + assert abs(output) <= .2 + assert abs(output - previous) <= 5. / max(speed, 1.) ** 2 * .01 + 1e-12 + previous = output + factors.append(factor) + assert factors[0] == math.pi ** 3 * 3.5 / 6. ** 3 * 1.3 / 5. + assert factors[-1] == 1. + assert max(factors[200:350]) > factors[0] # Arrest can recover quicker than turn-in. + + +def test_stock_duration_inactive_and_regular_driving_remain_exact(): + for active, duration, state in ((True, 3., log.LaneChangeState.laneChangeStarting), + (False, 6., log.LaneChangeState.laneChangeStarting), + (True, 6., log.LaneChangeState.off)): + smoother = LaneChangeSmoother() + factor = smoother.factor(active=active, lane_change_state=state, speed=20., minimum_speed=0., duration=duration, + previous=0., desired=.01) + assert factor == 1. + assert limit_rate(20., 0., .01, factor) == .01 + + +def test_intersection_exit_keeps_native_rate_after_crossing_lane_change_floor(): + smoother = LaneChangeSmoother() + def factor(speed, state, assist=True, active=True): + return smoother.factor(active=active, lane_change_state=state, speed=speed, minimum_speed=0., duration=7., + previous=-.005, desired=0., turn_assist=assist) + starting = log.LaneChangeState.laneChangeStarting + assert factor(8., starting) == 1. + assert factor(10., starting) == 1. + assert factor(12., log.LaneChangeState.off) == 1. + assert factor(25., starting) < 1. + + +def test_intersection_exemption_does_not_survive_disabled_turn_assist_or_lateral(): + for disable in ({"assist": False}, {"active": False}): + smoother = LaneChangeSmoother() + def factor(speed, smoother=smoother, **kwargs): + return smoother.factor(active=kwargs.get("active", True), lane_change_state=log.LaneChangeState.laneChangeStarting, + speed=speed, minimum_speed=0., duration=7., previous=-.005, desired=0., + turn_assist=kwargs.get("assist", True)) + assert factor(8.) == 1. + factor(10., **disable) + assert factor(25.) < 1. diff --git a/openpilot/starpilot/lateral/tests/test_lane_feedback.py b/openpilot/starpilot/lateral/tests/test_lane_feedback.py new file mode 100644 index 0000000000..55062e605e --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_lane_feedback.py @@ -0,0 +1,83 @@ +from types import SimpleNamespace +from unittest.mock import patch +import unittest + +import numpy as np +import pyray as rl + +from openpilot.cereal import messaging +from openpilot.starpilot.lateral.lane_centering import LaneCenteringResult +from openpilot.starpilot.lateral.lane_feedback import BLUE, MAX_AGE_NS, SERVICE, direction, feedback_message + + +class TestLaneFeedback(unittest.TestCase): + def reader(self, *, applied=.0003, requested=.0005): + now = 1_000_000_000 + event = feedback_message(LaneCenteringResult(.001, requested, 1, 'qualified'), applied, True, + model_mono_time=now, car_control_mono_time=now, valid=True) + event.logMonoTime = now + control = messaging.new_message('carControl', valid=True, logMonoTime=now) + control.carControl.latActive = True + model = messaging.new_message('modelV2', valid=True, logMonoTime=now) + sm = messaging.SubMaster([SERVICE, 'carControl', 'modelV2'], frequency=20) + sm.update_msgs(now / 1e9 - .01, []) + sm.update_msgs(now / 1e9, [event.as_reader(), control.as_reader(), model.as_reader()]) + return sm, now + + def test_real_messages_show_applied_sign_and_keep_legacy_fields_inactive(self): + for correction, expected in ((.0003, 1), (-.0003, -1), (0., 0), (.0000005, 0)): + sm, now = self.reader(applied=correction, requested=.0005 if correction >= 0 else -.0005) + self.assertEqual(direction(sm, now, 0), expected) + self.assertFalse(sm[SERVICE].active) + self.assertEqual(sm[SERVICE].frictionScale, 0.) + + def test_stale_invalid_old_drive_disabled_and_inconsistent_inputs_clear_blue(self): + for service in (SERVICE, 'carControl', 'modelV2'): + for flag in ('alive', 'valid'): + sm, now = self.reader() + getattr(sm, flag)[service] = False + self.assertEqual(direction(sm, now, 0), 0) + sm, now = self.reader() + self.assertEqual(direction(sm, now + MAX_AGE_NS + 1, 0), 0) + self.assertEqual(direction(sm, now - 1, 0), 0) + self.assertEqual(direction(sm, now, sm.frame), 0) + for applied, requested in ((.0013, .0013), (.0006, .0005), (-.0003, .0005), (float('nan'), .0005)): + sm, now = self.reader(applied=applied, requested=requested) + self.assertEqual(direction(sm, now, 0), 0) + for field, value in (('version', 0), ('lateralActive', False), ('modelMonoTime', 1), ('carControlMonoTime', 1)): + sm, now = self.reader() + event = messaging.new_message(SERVICE, valid=True, logMonoTime=now) + event.starpilotLateralState = sm[SERVICE] + setattr(event.starpilotLateralState.laneCentering, field, value) + sm.update_msgs(now / 1e9 + .01, [event.as_reader()]) + self.assertEqual(direction(sm, now, 0), 0) + sm, now = self.reader() + control = messaging.new_message('carControl', valid=True, logMonoTime=now) + control.carControl.latActive = False + sm.update_msgs(now / 1e9 + .01, [control.as_reader()]) + self.assertEqual(direction(sm, now, 0), 0) + + def test_model_renderer_blue_retains_alpha_and_overrides_torque_color(self): + from openpilot.selfdrive.ui.onroad import model_renderer as large + from openpilot.selfdrive.ui.mici.onroad import model_renderer as compact + from openpilot.selfdrive.ui.ui_state import UIStatus + + for module in (large, compact): + for sign in (-1, 0, 1): + renderer = module.ModelRenderer.__new__(module.ModelRenderer) + renderer._rect = rl.Rectangle(0, 0, 536, 240) + renderer._lane_lines = [SimpleNamespace(projected_points=np.array([[0., 0.], [5., 0.], [0., 5.]])) for _ in range(4)] + renderer._road_edges = [] + renderer._lane_line_probs = [.3, .6, .7, .4] + if module is compact: + renderer._visual_status = lambda: UIStatus.ENGAGED + renderer._torque_filter = SimpleNamespace(x=.9) + with patch.object(module, 'lane_centering_direction', return_value=sign), patch.object(module, 'draw_polygon') as draw: + renderer._draw_lane_lines() + colors = [(c.r, c.g, c.b, c.a) for call in draw.call_args_list for c in [call.args[2]]] + self.assertEqual([i for i, color in enumerate(colors) if color[:3] == BLUE], [1 if sign < 0 else 2] if sign else []) + if sign: + index = 1 if sign < 0 else 2 + self.assertEqual(colors[index][3], int(renderer._lane_line_probs[index] * 255)) + for index in (0, 3): + self.assertEqual(colors[index][3], int(renderer._lane_line_probs[index] * 255)) diff --git a/openpilot/starpilot/lateral/tests/test_lane_runtime.py b/openpilot/starpilot/lateral/tests/test_lane_runtime.py new file mode 100644 index 0000000000..528e87c7a0 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_lane_runtime.py @@ -0,0 +1,408 @@ +"""Saved preferences reach actual Controls only with fresh lateral authority.""" + +import os +import tempfile +from pathlib import Path +from types import SimpleNamespace +import unittest +from unittest import mock + +from openpilot.cereal import messaging +from openpilot.common.params import Params +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.selfdrive.controls.controlsd import Controls +from openpilot.starpilot.lateral.lane_centering import ControlMode +from openpilot.starpilot.lateral.lane_runtime import LaneCenteringHost, read_settings, runtime_supported +from openpilot.starpilot.lateral.tests.test_lane_centering import model +from openpilot.starpilot.aol.wire import SafetyState, encode_safety +from opendbc.car import gen_empty_fingerprint +from opendbc.car.car_helpers import interfaces +from opendbc.car.honda.interface import CarInterface as HondaCarInterface +from opendbc.car.honda.values import CAR as HondaCAR +from opendbc.car.hyundai.interface import CarInterface as HyundaiCarInterface +from opendbc.car.hyundai.ioniq6_handoff import build_ioniq6_hda2_long_candidate +from opendbc.car.hyundai.values import CAR as HyundaiCAR +from opendbc.car.structs import car +from opendbc.car.toyota.values import CAR + + +def ioniq_candidate(*, alternate=False): + fingerprint = gen_empty_fingerprint() + fingerprint[2].update({0x110: 32, 0x362: 32} if alternate else {0x50: 16, 0x2A4: 24}) + fingerprint[1].update({0x1CF: 8, 0x35: 32, 0x175: 24, 0xA0: 24, 0xEA: 24, + 0x1BA: 24, 0x1E5: 16, 0x36A: 16}) + fingerprint[0][0x3A5] = 24 + fingerprint[0][0x100] = 24 + stock = HyundaiCarInterface.get_params(HyundaiCAR.HYUNDAI_IONIQ_6, fingerprint, [], False, False, False) + return stock, build_ioniq6_hda2_long_candidate(stock, fingerprint) + + +class Frame: + def __init__(self, now_ns): + self.logMonoTime = dict.fromkeys(LaneCenteringHost.REQUIRED, now_ns) + self.checked = True + self.state = SimpleNamespace(canValid=True, canTimeout=False) + + def all_checks(self, _services): + return self.checked + + def __getitem__(self, _service): + return self.state + + +def feed(controls, timestamp, tick, *, active=True, enabled=True, fault=False, override=False, signal=False, model_age=0, + can_valid=True, can_timeout=False, aol_lat_only=False, native_session='lane-aol-session'): + events = [] + + def msg(service, size=None): + event = messaging.new_message(service, size, valid=True, logMonoTime=timestamp) + events.append(event) + return getattr(event, service) + + cs = msg('carState') + cs.vEgo = cs.vEgoRaw = 20.0 + cs.vCruise = cs.vCruiseCluster = 100.0 + cs.canValid = can_valid + cs.canTimeout = can_timeout + cs.steerFaultTemporary = fault + cs.steeringPressed = override + cs.leftBlinker = signal + state = msg('selfdriveState') + state.active, state.enabled = active, enabled + state.state = 'enabled' if enabled else 'disabled' + if tick % 5 == 0: + lp = msg('vehicleParameters') + lp.stiffnessFactor, lp.steerRatio = 1.0, controls.CP.steerRatio + msg('longitudinalPlan').aTarget = 0.5 + msg('lateralDelay').lateralDelay = 0.1 + msg('onroadEvents', 0) + if tick % 5 == 0: + event = messaging.new_message('modelV2', valid=True, logMonoTime=timestamp - model_age) + event.modelV2 = model() + event.modelV2.action.desiredCurvature = 0.0002 + events.append(event) + if aol_lat_only: + axis = msg('aolAxisState') + axis.qualified = axis.nativeAcknowledged = axis.desiredLateral = axis.lateralActive = True + axis.desiredLongitudinal = axis.longitudinalActive = False + axis.sessionId = 'lane-aol-session' + axis.observedMonoTime = timestamp + axis.validUntilMonoTime = timestamp + 30_000_000 + msg('aolSafetyWire', 0) + events[-1].aolSafetyWire = encode_safety(SafetyState( + 1, True, timestamp, timestamp + 200_000_000, int(car.CarParams.SafetyModel.hondaBosch), + controls.CP.safetyConfigs[-1].safetyParam, True, False, True, False, + 'lane-test-panda', native_session)) + controls.sm.update_msgs(timestamp / 1e9, [event.as_reader() for event in events]) + + +class LaneRuntimeTests(unittest.TestCase): + def test_strength_saved_values_and_live_refresh_keep_existing_admission(self): + with tempfile.TemporaryDirectory() as temporary: + root = Path(temporary) + params = SimpleNamespace(get_param_path=lambda key: str(root / key)) + (root / 'LaneCentering').write_bytes(b'1') + self.assertEqual(read_settings(params).strength, 1.0) + host = LaneCenteringHost(params) + now = 10_000_000_000 + first = host.sample(Frame(now), now_ns=now, lateral_active=True, longitudinal_active=False) + self.assertEqual(first.settings.strength, 1.0) + (root / 'LaneCenteringStrength').write_bytes(b'1.5') + now += host.REFRESH_NS + updated = host.sample(Frame(now), now_ns=now, lateral_active=True, longitudinal_active=False) + self.assertEqual(updated.settings.strength, 1.5) + self.assertTrue(updated.time_discontinuity) + for invalid in (b'nan', b'0.49', b'1.51', b'true'): + (root / 'LaneCenteringStrength').write_bytes(invalid) + self.assertFalse(read_settings(params).enabled) + now += host.REFRESH_NS + self.assertIsNone(host.sample(Frame(now), now_ns=now, lateral_active=True, longitudinal_active=False)) + + + def test_normal_reader_admission_keeps_exact_vehicle_and_saved_preference_gates(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0'}): + params = Params() + for alternate in (False, True): + with self.subTest(alternate=alternate): + stock, tagged = ioniq_candidate(alternate=alternate) + self.assertFalse(runtime_supported(stock)) + self.assertTrue(runtime_supported(tagged)) + self.assertFalse(read_settings(params).enabled) + params.put_bool('LaneCentering', True, block=True) + self.assertTrue(read_settings(params).enabled) + self.assertFalse(runtime_supported(interfaces[CAR.TOYOTA_COROLLA_TSS2].get_non_essential_params(CAR.TOYOTA_COROLLA_TSS2))) + tagged.safetyConfigs[0].safetyParam = 0 + self.assertFalse(runtime_supported(tagged)) + params.put_bool('LaneCentering', False, block=True) + _, tagged = ioniq_candidate() + params.put_bool('LaneCentering', True, block=True) + Path(params.get_param_path('LaneCenterOffset')).write_bytes(b'nan') + self.assertFalse(read_settings(params).enabled) + with mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '1'}): + self.assertTrue(runtime_supported(tagged)) + + def test_actual_tagged_ioniq_saved_host_uses_fresh_lateral_authority(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1', + 'AOL_REPLAY_RUNTIME': '0'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params = Params() + stock, tagged = ioniq_candidate() + params.put('LaneCenteringE2EAuthority', 0.0, block=True) + params.put('CarParams', tagged.to_bytes(), block=True) + disabled = Controls() + self.assertIsNotNone(disabled.lane_centering_host) + frame = Frame(1_000_000_000) + self.assertIsNone(disabled.lane_centering_host.sample(frame, now_ns=1_000_000_000, + lateral_active=True, longitudinal_active=False)) + params.put_bool('LaneCentering', True, block=True) + controls = disabled + self.assertIsNotNone(controls.lane_centering_host) + for tick in range(35): + now = 2_000_000_000 + tick * 10_000_000 + feed(controls, now, tick) + command, _ = controls.state_control() + self.assertTrue(command.latActive) + self.assertIsNotNone(controls.last_lane_centering_result) + self.assertGreater(controls.last_lane_centering_result.correction, 0.0) + feed(controls, now + 10_000_000, 35, model_age=200_000_000) + controls.state_control() + self.assertIsNone(controls.last_lane_centering_result) + params.put_bool('LaneCentering', False, block=True) + feed(controls, 3_000_000_000, 36) + controls.state_control() + self.assertIsNone(controls.last_lane_centering_result) + self.assertEqual(controls.lane_centering_applied, 0.0) + params.put('CarParams', stock.to_bytes(), block=True) + self.assertIsNone(Controls().lane_centering_host) + + def test_tagged_ioniq_four_axes_and_stale_reset(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1', + 'AOL_REPLAY_RUNTIME': '0'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params = Params() + _, tagged = ioniq_candidate() + params.put('CarParams', tagged.to_bytes(), block=True) + params.put_bool('LaneCentering', True, block=True) + params.put('LaneCenteringE2EAuthority', 0.0, block=True) + controls = Controls() + self.assertIsNotNone(controls.lane_centering_host) + # Inject already-qualified independent-axis results at the consumer seam; + # AOL's native/session checks are covered by its own wire tests. + controls.aol_replay = True + tick = 0 + for lat, lon in ((True, True), (True, False), (False, True), (False, False)): + with self.subTest(lat=lat, lon=lon), \ + mock.patch('openpilot.selfdrive.controls.controlsd.current_axis', return_value=SimpleNamespace( + sessionId='test-session', nativeAcknowledged=True, lateralActive=lat, desiredLateral=lat, + longitudinalActive=lon, desiredLongitudinal=lon)), \ + mock.patch('openpilot.selfdrive.controls.controlsd.current_native', return_value=SimpleNamespace( + requestedLateral=lat, lateralAllowed=lat, requestedLongitudinal=lon, longitudinalAllowed=lon)): + for _ in range(10): + now = 1_000_000_000 + tick * 10_000_000 + feed(controls, now, tick, active=lat, enabled=lon) + command, _ = controls.state_control() + tick += 1 + self.assertEqual((command.latActive, command.longActive), (lat, lon)) + self.assertIsNotNone(controls.last_lane_centering_result) + self.assertEqual(controls.last_lane_centering_result.correction > 0.0, lat) + with mock.patch('openpilot.selfdrive.controls.controlsd.current_axis', return_value=SimpleNamespace( + sessionId='test-session', nativeAcknowledged=True, lateralActive=True, desiredLateral=True, + longitudinalActive=False, desiredLongitudinal=False)), \ + mock.patch('openpilot.selfdrive.controls.controlsd.current_native', return_value=SimpleNamespace( + requestedLateral=True, lateralAllowed=True, requestedLongitudinal=False, longitudinalAllowed=False)): + now = 1_000_000_000 + tick * 10_000_000 + feed(controls, now, tick, active=False, enabled=False, model_age=200_000_000) + command, _ = controls.state_control() + self.assertTrue(command.latActive) + self.assertFalse(command.longActive) + self.assertIsNone(controls.last_lane_centering_result) + for recovery_tick in range(tick + 1, tick + 6): + now = 1_000_000_000 + recovery_tick * 10_000_000 + feed(controls, now, recovery_tick, active=False, enabled=False) + controls.state_control() + self.assertGreater(controls.last_lane_centering_result.correction, 0.0) + + def test_saved_settings_bounds_and_corruption(self): + with OpenpilotPrefix(): + params = Params() + self.assertFalse(read_settings(params).enabled) + self.assertTrue(read_settings(params).pause_on_signal) + params.put_bool('LaneCentering', True, block=True) + params.put('LaneCenterOffset', -0.2, block=True) + params.put('LaneCenteringE2EAuthority', 0.5, block=True) + settings = read_settings(params) + self.assertEqual((settings.enabled, settings.offset_m, settings.e2e_authority), (True, -0.2, 0.5)) + for key, values in (('LaneCenterOffset', (b'bad', b'nan', b'inf', b'0.31')), + ('LaneCenteringE2EAuthority', (b'', b'1.01', b'-0.1')), + ('LaneCenteringPauseOnSignal', (b'false', b'2'))): + path = Path(params.get_param_path(key)) + original = path.read_bytes() if path.exists() else None + for value in values: + with self.subTest(key=key, value=value): + path.write_bytes(value) + self.assertFalse(read_settings(params).enabled) + self.assertTrue(params.get_bool('LaneCentering')) + if original is None: + path.unlink() + else: + path.write_bytes(original) + + def test_authority_is_derived_from_actual_axes(self): + with OpenpilotPrefix(): + params = Params() + params.put_bool('LaneCentering', True, block=True) + host = LaneCenteringHost(params) + for tick, (lat, lon, mode) in enumerate(((False, False, ControlMode.OFF), (False, True, ControlMode.LONGITUDINAL_ONLY), + (True, False, ControlMode.LATERAL_ONLY), (True, True, ControlMode.COMBINED))): + now = 1_000_000_000 + tick * 10_000_000 + request = host.sample(Frame(now), now_ns=now, lateral_active=lat, longitudinal_active=lon) + self.assertIsNotNone(request) + self.assertEqual(request.mode, mode) + + def test_stale_future_invalid_input_and_clock_gap(self): + with OpenpilotPrefix(): + params = Params() + params.put_bool('LaneCentering', True, block=True) + host = LaneCenteringHost(params) + now = 1_000_000_000 + def sample(frame, at=now): + return host.sample(frame, now_ns=at, lateral_active=True, longitudinal_active=True) + self.assertIsNotNone(sample(Frame(now))) + for service in host.REQUIRED: + for age in (200_000_000, -1): + frame = Frame(now) + frame.logMonoTime[service] = now - age + self.assertIsNone(sample(frame)) + frame = Frame(now) + frame.checked = False + self.assertIsNone(sample(frame)) + frame.checked, frame.state.canValid = True, False + self.assertIsNone(sample(frame)) + frame.state.canValid, frame.state.canTimeout = True, True + self.assertIsNone(sample(frame)) + frame.state.canTimeout = False + frame.logMonoTime['modelV2'] = 0 + self.assertIsNone(sample(frame)) + self.assertIsNone(sample(Frame(0), 0)) + gap = sample(Frame(now + 100_000_000), now + 100_000_000) + self.assertTrue(gap.time_discontinuity) + self.assertFalse(sample(Frame(now + 110_000_000), now + 110_000_000).time_discontinuity) + self.assertTrue(sample(Frame(now), now).time_discontinuity) + + def test_live_settings_refresh_and_corrupt_setting_disables(self): + with OpenpilotPrefix(): + params = Params() + params.put_bool('LaneCentering', True, block=True) + host = LaneCenteringHost(params) + for now in (1_000_000_000, 1_010_000_000): + self.assertIsNotNone(host.sample(Frame(now), now_ns=now, lateral_active=True, longitudinal_active=False)) + Path(params.get_param_path('LaneCenterOffset')).write_bytes(b'bad') + now = 2_000_000_000 + self.assertIsNone(host.sample(Frame(now), now_ns=now, lateral_active=True, longitudinal_active=False)) + self.assertTrue(params.get_bool('LaneCentering')) + + def test_actual_controls_saved_settings_and_permission_gates(self): + scenarios = ('normal', 'disabled_setting', 'off', 'long_only', 'fault', 'override', 'signal', 'stale', 'invalid_can', 'can_timeout') + with OpenpilotPrefix(), mock.patch.dict(os.environ, {'AOL_REPLAY_RUNTIME': '0', 'REPLAY': '1'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params = Params() + cp = interfaces[CAR.TOYOTA_COROLLA_TSS2].get_non_essential_params(CAR.TOYOTA_COROLLA_TSS2) + params.put('CarParams', cp.to_bytes(), block=True) + for scenario in scenarios: + with self.subTest(scenario=scenario): + params.put_bool('LaneCentering', scenario != 'disabled_setting', block=True) + params.put('LaneCenteringE2EAuthority', 0.0, block=True) + with mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0'}): + baseline = Controls() + with mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '1'}): + candidate = Controls() + self.assertIsNone(baseline.lane_centering_host) + previous = 0.0 + for tick in range(120): + now = 1_000_000_000 + tick * 10_000_000 + phase = scenario if tick >= 80 else 'normal' + options = {'active': phase not in ('off', 'long_only'), 'enabled': phase != 'off', + 'fault': phase == 'fault', 'override': phase == 'override', 'signal': phase == 'signal', + 'model_age': 200_000_000 if phase == 'stale' else tick % 5 * 10_000_000, + 'can_valid': phase != 'invalid_can', 'can_timeout': phase == 'can_timeout'} + for controls in (baseline, candidate): + feed(controls, now, tick, **options) + reference, _ = baseline.state_control() + command, _ = candidate.state_control() + result = candidate.last_lane_centering_result + self.assertEqual(command.latActive, reference.latActive) + self.assertEqual(command.longActive, reference.longActive) + self.assertEqual(command.actuators.accel, reference.actuators.accel) + if scenario == 'disabled_setting': + self.assertEqual(command.to_dict(), reference.to_dict()) + elif phase in ('off', 'long_only', 'fault', 'override'): + self.assertEqual(result.correction, 0.0) + elif phase in ('stale', 'invalid_can', 'can_timeout'): + self.assertIsNone(result) + elif phase == 'signal': + self.assertLess(result.correction, previous) + elif tick >= 20: + self.assertGreater(result.correction, previous) + previous = result.correction if result is not None else 0.0 + + def test_actual_aol_lateral_only_controls_without_longitudinal(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, {'AOL_REPLAY_RUNTIME': '1', 'LANE_CENTERING_REPLAY_RUNTIME': '1', + 'REPLAY': '1'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params = Params() + params.put_bool('LaneCentering', True, block=True) + cp = HondaCarInterface.get_params(HondaCAR.HONDA_CIVIC_BOSCH, gen_empty_fingerprint(), [], True, False, False) + cp.safetyConfigs[-1].safetyParam |= 0x20 # Card's qualified AOL bit in serialized CarParams. + params.put('CarParams', cp.to_bytes(), block=True) + controls = Controls() + self.assertTrue(controls.aol_replay) + self.assertIsNotNone(controls.lane_centering_host) + for tick in range(40): + now = 1_000_000_000 + tick * 10_000_000 + feed(controls, now, tick, active=False, enabled=False, aol_lat_only=True) + command, _ = controls.state_control() + self.assertFalse(command.enabled) + self.assertTrue(command.latActive) + self.assertFalse(command.longActive) + self.assertEqual(command.actuators.accel, 0.0) + assert controls.last_lane_centering_result is not None + self.assertGreater(controls.last_lane_centering_result.correction, 0.0) + previous = controls.last_lane_centering_result.correction + now += 10_000_000 + feed(controls, now, 40, active=False, enabled=False, aol_lat_only=True, native_session='old-drive') + command, _ = controls.state_control() + self.assertFalse(command.latActive or command.longActive) + self.assertEqual(controls.last_lane_centering_result.correction, 0.0) + now += 10_000_000 + feed(controls, now, 41, active=False, enabled=False, aol_lat_only=True) + command, _ = controls.state_control() + self.assertTrue(command.latActive) + self.assertFalse(command.longActive) + self.assertLess(controls.last_lane_centering_result.correction, previous) + for tick in range(42, 48): + now += 10_000_000 + feed(controls, now, tick, active=False, enabled=False) + command, _ = controls.state_control() + self.assertFalse(command.latActive or command.longActive) + self.assertEqual(controls.last_lane_centering_result.correction, 0.0) + + def test_stock_acc_lateral_stays_separate_from_aol(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, {'AOL_REPLAY_RUNTIME': '0', 'LANE_CENTERING_REPLAY_RUNTIME': '1', + 'REPLAY': '1'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params = Params() + params.put_bool('LaneCentering', True, block=True) + cp = interfaces[CAR.TOYOTA_COROLLA_TSS2].get_non_essential_params(CAR.TOYOTA_COROLLA_TSS2) + cp.openpilotLongitudinalControl = False + cp.pcmCruise = True + params.put('CarParams', cp.to_bytes(), block=True) + controls = Controls() + self.assertFalse(controls.aol_replay) + for tick in range(40): + now = 1_000_000_000 + tick * 10_000_000 + feed(controls, now, tick) + command, _ = controls.state_control() + self.assertTrue(command.latActive) + self.assertFalse(command.longActive) + assert controls.last_lane_centering_result is not None + self.assertGreater(controls.last_lane_centering_result.correction, 0.0) diff --git a/openpilot/starpilot/lateral/tests/test_palisade_policy.py b/openpilot/starpilot/lateral/tests/test_palisade_policy.py new file mode 100644 index 0000000000..bbc8a99054 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_palisade_policy.py @@ -0,0 +1,87 @@ +import math +from types import SimpleNamespace +from unittest.mock import patch + +import pytest + +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.tests.test_palisade_2023 import params as topology_params +from opendbc.car.hyundai.values import CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.cereal import log +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection, replace_mode, selection_from_bytes +from openpilot.starpilot.lateral.palisade_policy import PalisadeTorquePolicy, supported_cp +from openpilot.starpilot.lateral import palisade_shaping as shaping +from openpilot.starpilot.lateral.torque_shaping import INTERP_SPEEDS, KP_INTERP, KI + + +@pytest.mark.parametrize('speed,accel,jerk,ff,threshold,friction,center,output', [ + (2., .18, .3, 1.1469630171215324, .16001729374366647, 1.034684825233675, .9981430724236608, .9979939519844233), + (8.9408, -.25, -.45, 1.131157979745277, .18376493402775368, 1.0340640585840144, .982732898487096, .9866506253915932), + (22., .8, -.6, 1.1001436880961013, .2366944877586695, 1.0045028671743204, .9999907698683685, .9999871475466129), + (33.528, -1.4, .8, 1.042046705933432, .2738465058489194, 1.0070488018554904, .9999999998280955, .9999999997426313), +]) +def test_frozen_phase_and_side_formula_vectors(speed, accel, jerk, ff, threshold, friction, center, output): + actual = (shaping.get_palisade_ff_scale(accel, jerk, speed), shaping.get_palisade_friction_threshold(speed, accel, jerk), + shaping.get_palisade_friction_scale(speed, accel, jerk), shaping.get_palisade_center_taper_scale(accel, speed), + shaping.get_palisade_center_output_scale(accel, speed)) + assert actual == pytest.approx((ff, threshold, friction, center, output), abs=1e-14, rel=0) + + +@pytest.mark.parametrize('vehicle', [CAR.HYUNDAI_PALISADE, CAR.HYUNDAI_PALISADE_2023]) +def test_default_choice_startup_and_repeated_live_factor(vehicle): + cp = interfaces[vehicle].get_non_essential_params(vehicle) + ci = interfaces[vehicle](cp) + controller = LatControlTorque(cp.as_reader(), ci, .01) + assert default_selection(cp).policy == 'palisade' + assert isinstance(controller.starpilot_extension.policy, PalisadeTorquePolicy) + assert controller.torque_params.latAccelFactor == pytest.approx(cp.lateralTuning.torque.latAccelFactor * .98) + assert controller.pid._k_p == [INTERP_SPEEDS, KP_INTERP] + assert controller.pid._k_i == ([0.], [KI]) + raw_limit = controller.lateral_accel_from_torque(controller.steer_max, cp.lateralTuning.torque) + assert controller.pid.pos_limit == pytest.approx(raw_limit) + for _ in range(3): + controller.update_torque_parameters(3., .02, .12) + assert controller.torque_params.latAccelFactor == pytest.approx(2.94) + assert controller.torque_params.latAccelOffset == pytest.approx(.02) + assert controller.torque_params.friction == pytest.approx(.12) + assert controller.pid.pos_limit == pytest.approx(controller.lateral_accel_from_torque(controller.steer_max, controller.torque_params)) + saved = replace_mode(None, cp, ControllerMode.STANDARD) + assert selection_from_bytes(cp, saved).mode == ControllerMode.STANDARD + assert LatControlTorque(cp.as_reader(), ci, .01, controller_mode=ControllerMode.STANDARD).starpilot_extension is None + + +@pytest.mark.parametrize('topology', ['hdai', 'hdaii', '110']) +def test_actual_selected_controller_output_and_inactive_recovery(topology): + cp = topology_params(topology) + controller = LatControlTorque(cp.as_reader(), interfaces[cp.carFingerprint](cp), .01) + vm = VehicleModel(cp) + state = SimpleNamespace(vEgo=12., steeringAngleDeg=0., steeringPressed=False) + parameters = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=.01) + for curvature in (.0002, -.0002, 0.): + for _ in range(25): + torque, _, trace = controller.update(True, state, vm, parameters, False, curvature, False, .2) + assert math.isfinite(torque) and math.isfinite(trace.f) + limit = max(abs(controller.torque_from_lateral_accel(bound, controller.torque_params)) + for bound in (controller.pid.pos_limit, controller.pid.neg_limit)) + assert abs(torque) <= limit + 1e-6 + assert trace.active + with patch.object(controller.pid, 'update', wraps=controller.pid.update) as update: + state.steeringPressed = True + controller.update(True, state, vm, parameters, False, .0002, False, .2) + assert update.call_args.kwargs['freeze_integrator'] + torque, _, trace = controller.update(False, state, vm, parameters, False, .0002, False, .2) + assert torque == 0 and not trace.active and controller.pid.i == 0 + state.steeringPressed = False + assert math.isfinite(controller.update(True, state, vm, parameters, False, .0002, False, .2)[0]) + + +def test_unrelated_or_unadmitted_profiles_cannot_select_palisade(): + cp = interfaces[CAR.HYUNDAI_PALISADE].get_non_essential_params(CAR.HYUNDAI_PALISADE) + for field in ('passive', 'dashcamOnly', 'notCar'): + candidate = cp.as_reader().as_builder() + setattr(candidate, field, True) + assert not supported_cp(candidate) + cp.carFingerprint = CAR.HYUNDAI_IONIQ_6 + assert not supported_cp(cp) diff --git a/openpilot/starpilot/lateral/tests/test_sonata_hybrid_policy.py b/openpilot/starpilot/lateral/tests/test_sonata_hybrid_policy.py new file mode 100644 index 0000000000..b7dcf3c3c2 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_sonata_hybrid_policy.py @@ -0,0 +1,48 @@ +import pytest + +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection, replace_mode, selection_from_bytes +from openpilot.starpilot.lateral.sonata_hybrid_policy import SonataHybridTorquePolicy, supported_cp +from openpilot.starpilot.lateral import sonata_hybrid_shaping as shaping + + +@pytest.mark.parametrize('speed,accel,jerk,ff,threshold,center,output', [ + [2.0, 0.18, 0.3, 1.0029066557938373, 0.3172954212921375, 0.9974817827847016, 0.9989658177814709], + [8.0, -0.25, -0.45, 0.8335966434725148, 0.3099892527937427, 0.9999609835312265, 0.9960715403512586], + [22.0, 0.8, -0.6, 0.8570825555968928, 0.3, 0.9999999999996189, 0.9999994360166975], + [33.0, -1.4, 0.8, 0.8442122416154261, 0.3, 1.0, 0.9999999999964582], +]) +def test_frozen_phase_side_and_center_vectors(speed, accel, jerk, ff, threshold, center, output): + actual = (shaping.get_sonata_hybrid_ff_scale(accel, jerk, speed), shaping.get_sonata_hybrid_friction_threshold(speed, accel), + shaping.get_sonata_hybrid_center_taper_scale(accel, speed), shaping.get_sonata_hybrid_center_output_scale(accel, speed)) + assert actual == pytest.approx((ff, threshold, center, output), abs=1e-14, rel=0) + + +def test_default_startup_raw_limits_and_live_factor_exactly_once(): + cp = interfaces[CAR.HYUNDAI_SONATA_HYBRID].get_non_essential_params(CAR.HYUNDAI_SONATA_HYBRID) + ci = interfaces[cp.carFingerprint](cp) + controller = LatControlTorque(cp.as_reader(), ci, .01) + assert default_selection(cp).policy == 'sonata_hybrid' + assert isinstance(controller.starpilot_extension.policy, SonataHybridTorquePolicy) + assert controller.torque_params.latAccelFactor == pytest.approx(cp.lateralTuning.torque.latAccelFactor * 1.05) + assert controller.pid.pos_limit == pytest.approx(controller.lateral_accel_from_torque(controller.steer_max, cp.lateralTuning.torque)) + for _ in range(3): + controller.update_torque_parameters(3., .02, .12) + assert controller.torque_params.latAccelFactor == pytest.approx(3.15) + assert controller.torque_params.latAccelOffset == pytest.approx(.02) + assert controller.torque_params.friction == pytest.approx(.12) + saved = replace_mode(None, cp, ControllerMode.STANDARD) + assert selection_from_bytes(cp, saved).mode == ControllerMode.STANDARD + assert LatControlTorque(cp.as_reader(), ci, .01, controller_mode=ControllerMode.STANDARD).starpilot_extension is None + + +def test_unrelated_and_unadmitted_do_not_select_hybrid_policy(): + cp = interfaces[CAR.HYUNDAI_SONATA_HYBRID].get_non_essential_params(CAR.HYUNDAI_SONATA_HYBRID) + for field in ('passive', 'dashcamOnly', 'notCar'): + other = cp.as_reader().as_builder() + setattr(other, field, True) + assert not supported_cp(other) + cp.carFingerprint = CAR.HYUNDAI_SONATA + assert not supported_cp(cp) diff --git a/openpilot/starpilot/lateral/tests/test_torque_application.py b/openpilot/starpilot/lateral/tests/test_torque_application.py new file mode 100644 index 0000000000..e6e0daab7a --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_torque_application.py @@ -0,0 +1,103 @@ +"""Actual source transitions must not silently alter vehicle startup limits.""" + +from dataclasses import replace +from unittest import mock +import unittest + +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR as HYUNDAI +from opendbc.car.toyota.values import CAR as TOYOTA +from openpilot.common.params import Params +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.torque_runtime import TorqueHost +from openpilot.starpilot.lateral.torque_tuning import TorqueSource + + +class TestTorqueApplication(unittest.TestCase): + def controller(self, vehicle): + cp = interfaces[vehicle].get_non_essential_params(vehicle) + controller = LatControlTorque(cp.as_reader(), interfaces[vehicle](cp), DT_CTRL) + return TorqueHost(Params(), cp), controller + + def test_initial_vehicle_preserves_ioniq_startup_and_explicit_equal_source_updates(self): + with OpenpilotPrefix(): + for source in (TorqueSource.LEARNED, TorqueSource.USER): + with self.subTest(source=source): + host, controller = self.controller(HYUNDAI.HYUNDAI_IONIQ_6) + with mock.patch.object(controller, 'update_torque_parameters', wraps=controller.update_torque_parameters) as update: + initial_limit = controller.pid.pos_limit + for _ in range(3): + self.assertFalse(host.apply(controller, host.vehicle)) + update.assert_not_called() + self.assertEqual(controller.pid.pos_limit, initial_limit) + self.assertAlmostEqual(initial_limit, 3.0) + selected = replace(host.vehicle, source=source) + self.assertTrue(host.apply(controller, selected)) + self.assertAlmostEqual(controller.pid.pos_limit, 3.0 * 1.22, places=6) + self.assertFalse(host.apply(controller, selected)) + self.assertEqual(update.call_count, 1) + self.assertAlmostEqual(controller.torque_params.latAccelFactor, 3.0 * 1.22, places=6) + + def test_changed_values_fallback_and_new_controller_do_not_inherit_application(self): + with OpenpilotPrefix(): + host, controller = self.controller(HYUNDAI.HYUNDAI_IONIQ_6) + custom = replace(host.vehicle, source=TorqueSource.USER, lat_accel_factor=3.3, friction=0.12) + self.assertTrue(host.apply(controller, custom)) + self.assertAlmostEqual(controller.torque_params.latAccelFactor, 3.3 * 1.22, places=6) + self.assertTrue(host.apply(controller, host.vehicle)) + self.assertAlmostEqual(controller.torque_params.latAccelFactor, 3.0 * 1.22, places=6) + self.assertAlmostEqual(controller.torque_params.friction, 0.09, places=6) + # A fallback restores values through the normal update API. It does not + # rewind the controller's history to its pre-update initialization state. + self.assertAlmostEqual(controller.pid.pos_limit, 3.0 * 1.22, places=6) + self.assertFalse(host.apply(controller, host.vehicle)) + _, other = self.controller(HYUNDAI.HYUNDAI_IONIQ_6) + self.assertFalse(host.apply(other, host.vehicle)) + self.assertAlmostEqual(other.pid.pos_limit, 3.0) + self.assertTrue(host.apply(other, custom)) + + def test_vehicle_only_samples_do_not_unlock_limit(self): + with OpenpilotPrefix(): + host, controller = self.controller(HYUNDAI.HYUNDAI_IONIQ_6) + # Inactive or malformed saved settings return the exact vehicle source. + sm = mock.Mock() + sm.all_checks.return_value = False + host.params.put_bool('AdvancedLateralTune', True, block=True) + host.params.put('TorqueOverrideDocument', {'invalid': True}, block=True) + for now, active in ((1_000_000_000, False), (1_010_000_000, True), (2_000_000_000, True)): + tune = host.sample(sm, now_ns=now, lat_active=active) + self.assertEqual(tune, host.vehicle) + self.assertFalse(host.apply(controller, tune)) + self.assertAlmostEqual(controller.pid.pos_limit, 3.0) + + def test_inactive_fallback_and_failed_application_are_not_cached_as_success(self): + with OpenpilotPrefix(): + host, controller = self.controller(HYUNDAI.HYUNDAI_IONIQ_6) + custom = replace(host.vehicle, source=TorqueSource.USER, lat_accel_factor=3.3, friction=0.12) + with mock.patch.object(controller, 'update_torque_parameters', side_effect=ValueError('rejected')): + with self.assertRaises(ValueError): + host.apply(controller, custom) + self.assertTrue(host.apply(controller, custom)) + self.assertAlmostEqual(controller.torque_params.latAccelFactor, 3.3 * 1.22, places=6) + fallback = host.sample(mock.Mock(), now_ns=1_000_000_000, lat_active=False) + self.assertEqual(fallback, host.vehicle) + self.assertTrue(host.apply(controller, fallback)) + self.assertAlmostEqual(controller.torque_params.latAccelFactor, 3.0 * 1.22, places=6) + self.assertAlmostEqual(controller.torque_params.friction, 0.09, places=6) + + def test_non_ioniq_controller_and_cross_vehicle_rejection(self): + with OpenpilotPrefix(): + host, controller = self.controller(TOYOTA.TOYOTA_COROLLA_TSS2) + original = (controller.pid.pos_limit, controller.pid.neg_limit) + self.assertFalse(host.apply(controller, host.vehicle)) + self.assertEqual((controller.pid.pos_limit, controller.pid.neg_limit), original) + self.assertTrue(host.apply(controller, replace(host.vehicle, source=TorqueSource.USER))) + self.assertEqual((controller.pid.pos_limit, controller.pid.neg_limit), original) + with self.assertRaises(ValueError): + host.apply(controller, replace(host.vehicle, vehicle='HYUNDAI_IONIQ_6')) + + +if __name__ == '__main__': + unittest.main() diff --git a/openpilot/starpilot/lateral/tests/test_torque_family.py b/openpilot/starpilot/lateral/tests/test_torque_family.py new file mode 100644 index 0000000000..ffed0dd28c --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_torque_family.py @@ -0,0 +1,169 @@ +"""Real Toyota CarParams and native controller coverage for the replay-only selector.""" + +import math +import os +from pathlib import Path +from types import SimpleNamespace +import unittest +from unittest import mock + +from opendbc.car import gen_empty_fingerprint, structs +from opendbc.car.car_helpers import interfaces +from opendbc.car.toyota.values import CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.common.params import Params +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.tests.test_torque_tuning import frame +from openpilot.starpilot.lateral.torque_runtime import TorqueHost, development_enabled, read_settings, supported_cp +from openpilot.starpilot.lateral.torque_supported import TOYOTA_VEHICLES +from openpilot.starpilot.lateral.torque_tuning import TorqueSource + + +def params_for(car_id): + return interfaces[car_id].get_non_essential_params(car_id) + + +def learner(now_ns, base): + state = SimpleNamespace(useParams=True, valid=True, version=1, latAccelFactorFiltered=base.lat_accel_factor * 1.1, + latAccelOffsetFiltered=0.1, frictionCoefficientFiltered=base.friction * 1.1) + + class Sample: + logMonoTime = {'lateralTorqueParameters': now_ns} + + def all_checks(self, _names): + return True + + def __getitem__(self, _name): + return state + + return Sample() + + +class ToyotaTorqueFamilyTests(unittest.TestCase): + def test_exact_registry_cohort_and_cp_guard(self): + registry = {str(car.value) for car in CAR if (cp := params_for(str(car.value))).brand == 'toyota' and + cp.steerControlType == structs.CarParams.SteerControlType.torque and cp.lateralTuning.which() == 'torque'} + self.assertEqual(len(registry), 38) + self.assertEqual(TOYOTA_VEHICLES, registry) + self.assertFalse(supported_cp(params_for('TOYOTA_RAV4_TSS2_2023'))) + self.assertFalse(supported_cp(params_for('CHEVROLET_BOLT_EUV'))) + with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}): + for car_id in sorted(TOYOTA_VEHICLES): + cp = params_for(car_id) + with self.subTest(car_id=car_id): + self.assertTrue(development_enabled(cp)) + cp.dashcamOnly = True + self.assertFalse(development_enabled(cp)) + cp.dashcamOnly = False + cp.steerControlType = structs.CarParams.SteerControlType.angle + self.assertFalse(development_enabled(cp)) + cp.steerControlType = structs.CarParams.SteerControlType.torque + cp.lateralTuning.torque.latAccelOffset = float('nan') + self.assertFalse(development_enabled(cp)) + cp = params_for('TOYOTA_COROLLA_TSS2') + cp.brand = 'gm' + self.assertFalse(development_enabled(cp)) + with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '0'}): + self.assertFalse(development_enabled(params_for('TOYOTA_COROLLA_TSS2'))) + + def test_each_cp_selects_complete_tuple_and_drives_real_controller(self): + for car_id in sorted(TOYOTA_VEHICLES): + with self.subTest(car_id=car_id), OpenpilotPrefix(): + cp = params_for(car_id) + saved = Params() + host = TorqueHost(saved, cp.as_reader()) + base = host.vehicle + self.assertEqual(base.vehicle, car_id) + self.assertEqual(base.upstream_update(), (cp.lateralTuning.torque.latAccelFactor, + cp.lateralTuning.torque.latAccelOffset, cp.lateralTuning.torque.friction)) + now = 1_000_000_000 + sm = learner(now, base) + host.settings = read_settings(saved, base) + selected = host._select(sm, now) + self.assertEqual(selected.source, TorqueSource.LEARNED) + self.assertAlmostEqual(selected.lat_accel_factor, base.lat_accel_factor * 1.1) + self.assertAlmostEqual(selected.lat_accel_offset, 0.1) + self.assertAlmostEqual(selected.friction, base.friction * 1.1) + + saved.put_bool('AdvancedLateralTune', True, block=True) + saved.put('SteerLatAccel', base.lat_accel_factor * 1.2, block=True) + host.settings = read_settings(saved, base) + factor_only = host._select(sm, now) + self.assertEqual(factor_only.source, TorqueSource.USER) + self.assertAlmostEqual(factor_only.lat_accel_factor, base.lat_accel_factor * 1.2) + self.assertAlmostEqual(factor_only.lat_accel_offset, base.lat_accel_offset) + self.assertAlmostEqual(factor_only.friction, base.friction * 1.1) + saved.remove('SteerLatAccel') + saved.put('SteerFriction', min(1.0, max(0.01, base.friction * 1.2)), block=True) + host.settings = read_settings(saved, base) + friction_only = host._select(sm, now) + self.assertEqual(friction_only.source, TorqueSource.USER) + self.assertAlmostEqual(friction_only.lat_accel_factor, base.lat_accel_factor * 1.1) + self.assertAlmostEqual(friction_only.lat_accel_offset, 0.1) + self.assertAlmostEqual(friction_only.friction, min(1.0, max(0.01, base.friction * 1.2))) + saved.put_bool('ForceAutoTuneOff', True, block=True) + host.settings = read_settings(saved, base) + self.assertAlmostEqual(host._select(sm, now).lat_accel_factor, base.lat_accel_factor) + saved.remove('SteerFriction') + Path(saved.get_param_path('SteerLatAccel')).write_bytes(b'nan') + self.assertFalse(read_settings(saved, base).valid) + self.assertEqual(host.sample(sm, now_ns=now, lat_active=False), base) + + interface = interfaces[car_id] + controller = LatControlTorque(cp.as_reader(), interface(cp), DT_CTRL) + initial_limit = controller.pid.pos_limit + controller.update_torque_parameters(*factor_only.upstream_update()) + self.assertAlmostEqual(controller.torque_params.latAccelFactor, factor_only.lat_accel_factor, delta=1e-6) + self.assertAlmostEqual(controller.torque_params.latAccelOffset, factor_only.lat_accel_offset, delta=1e-6) + self.assertAlmostEqual(controller.torque_params.friction, factor_only.friction, delta=1e-6) + self.assertAlmostEqual(controller.pid.pos_limit, initial_limit * 1.2, delta=1e-6) + vm = VehicleModel(cp) + for _ in range(50): + output, _, state = frame(controller, vm, curvature=0.001) + self.assertTrue(state.active) + self.assertTrue(math.isfinite(output)) + self.assertLessEqual(abs(output), controller.steer_max + 1e-6) + output, _, state = frame(controller, vm, active=False, curvature=0.001) + self.assertEqual(output, 0.0) + self.assertFalse(state.active) + + def test_stock_markers_do_not_carry_previous_vehicle_tune(self): + first = params_for('TOYOTA_COROLLA_TSS2') + second = params_for('TOYOTA_RAV4_TSS2') + with OpenpilotPrefix(): + saved = Params() + old = TorqueHost(saved, first).vehicle + new = TorqueHost(saved, second).vehicle + self.assertNotEqual(old.lat_accel_factor, new.lat_accel_factor) + saved.put_bool('AdvancedLateralTune', True, block=True) + saved.put('SteerLatAccel', old.lat_accel_factor, block=True) + saved.put('SteerLatAccelStock', old.lat_accel_factor, block=True) + saved.put('SteerFriction', old.friction, block=True) + saved.put('SteerFrictionStock', old.friction, block=True) + settings = read_settings(saved, new) + self.assertTrue(settings.valid) + self.assertIsNone(settings.user_factor) + self.assertIsNone(settings.user_friction) + host = TorqueHost(saved, second) + host.settings = settings + self.assertEqual(host._select(learner(1_000_000_000, new), 1_000_000_000).source, TorqueSource.LEARNED) + + def test_prius_eps_firmware_deadzone_variant_keeps_cp_and_selector(self): + car_id = 'TOYOTA_PRIUS' + interface = interfaces[car_id] + eps = structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.eps, fwVersion=b'other-prius-eps') + cp = interface.get_params(car_id, gen_empty_fingerprint(), [eps], False, False, False) + self.assertTrue(supported_cp(cp)) + self.assertAlmostEqual(cp.lateralTuning.torque.steeringAngleDeadzoneDeg, 0.2) + with OpenpilotPrefix(): + host = TorqueHost(Params(), cp) + self.assertEqual(host.vehicle.upstream_update(), (cp.lateralTuning.torque.latAccelFactor, + cp.lateralTuning.torque.latAccelOffset, cp.lateralTuning.torque.friction)) + controller = LatControlTorque(cp.as_reader(), interface(cp), DT_CTRL) + self.assertAlmostEqual(controller.steering_angle_deadzone_deg, 0.2) + + +if __name__ == '__main__': + unittest.main() diff --git a/openpilot/starpilot/lateral/tests/test_torque_learning.py b/openpilot/starpilot/lateral/tests/test_torque_learning.py new file mode 100644 index 0000000000..5b7766e9ae --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_torque_learning.py @@ -0,0 +1,275 @@ +"""Controller-specific learning is enforced by its producer and both consumers.""" + +import os +from pathlib import Path +from types import SimpleNamespace +import unittest +from unittest import mock + +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR as HYUNDAI +from opendbc.car.toyota.values import CAR as TOYOTA +from openpilot.cereal import messaging +from openpilot.common.params import Params +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.selfdrive.controls.controlsd import Controls +from openpilot.selfdrive.locationd import torqued +from openpilot.starpilot.lateral.controller_selection import ( + DOCUMENT_KEY, LEARNING_OFF_KEY, ControllerMode, learning_allowed, replace_mode, +) +from openpilot.starpilot.lateral.tests.test_lane_runtime import feed +from openpilot.starpilot.lateral.tests.test_torque_runtime import LearnedFrame +from openpilot.starpilot.lateral.torque_runtime import TorqueHost, read_settings +from openpilot.starpilot.lateral.torque_tuning import TorqueSource + + +VEHICLES = (HYUNDAI.HYUNDAI_IONIQ_6, HYUNDAI.GENESIS_G70_2020, TOYOTA.TOYOTA_COROLLA_TSS2) +ENV = {'REPLAY': '1', 'TORQUE_REPLAY_RUNTIME': '0', 'AOL_REPLAY_RUNTIME': '0', 'LANE_CENTERING_REPLAY_RUNTIME': '0'} + + +def cp_for(vehicle): + return interfaces[vehicle].get_non_essential_params(vehicle) + + +def select(params, cp, mode): + Path(params.get_param_path(DOCUMENT_KEY)).write_bytes(replace_mode(None, cp, mode)) + + +def learned_event(cp, now): + event = messaging.new_message('lateralTorqueParameters', valid=True, logMonoTime=now) + state = event.lateralTorqueParameters + state.useParams = state.valid = True + state.version = torqued.VERSION + state.latAccelFactorFiltered = cp.lateralTuning.torque.latAccelFactor * 1.1 + state.latAccelOffsetFiltered = 0.03 + state.frictionCoefficientFiltered = cp.lateralTuning.torque.friction * 1.1 + return event.as_reader() + + +class TestTorqueLearning(unittest.TestCase): + def test_exact_policy_defaults_and_saved_off_without_manual_adjustments(self): + with OpenpilotPrefix(): + params = Params() + for vehicle in VEHICLES: + with self.subTest(vehicle=vehicle): + cp = cp_for(vehicle) + params.remove(DOCUMENT_KEY) + params.remove(LEARNING_OFF_KEY) + self.assertFalse(learning_allowed(params, cp)) + select(params, cp, ControllerMode.STANDARD) + self.assertTrue(learning_allowed(params, cp)) + for saved in (b'1', b'invalid', b''): + Path(params.get_param_path(LEARNING_OFF_KEY)).write_bytes(saved) + self.assertFalse(learning_allowed(params, cp)) + params.put_bool(LEARNING_OFF_KEY, False, block=True) + self.assertTrue(learning_allowed(params, cp)) + params.put_bool(LEARNING_OFF_KEY, True, block=True) + for vehicle in (HYUNDAI.KIA_EV6, HYUNDAI.HYUNDAI_SONATA, TOYOTA.TOYOTA_RAV4_TSS2): + self.assertTrue(learning_allowed(params, cp_for(vehicle))) + + def test_learner_does_not_restore_collect_estimate_or_overwrite_disabled_cache(self): + for vehicle in VEHICLES: + with self.subTest(vehicle=vehicle), OpenpilotPrefix(), mock.patch.dict(os.environ, ENV): + params, cp = Params(), cp_for(vehicle) + params.put('CarParams', cp.to_bytes(), block=True) + cache = Path(params.get_param_path('LiveTorqueParameters')) + cache.write_bytes(b'preserved previous learning') + with mock.patch.object(torqued, 'get_cache') as restore: + estimator = torqued.TorqueEstimator(cp, allow_learning=learning_allowed(params, cp)) + restore.assert_not_called() + self.assertFalse(estimator.learning_allowed) + estimator.handle_log(1, 'carControl', SimpleNamespace(latActive=True)) + estimator.handle_log(1, 'carOutput', SimpleNamespace(actuatorsOutput=SimpleNamespace(torque=0.2))) + self.assertFalse(estimator.raw_points) + with mock.patch.object(estimator, 'estimate_params', side_effect=AssertionError('Learning is disabled')): + msg = estimator.get_msg(with_points=True).lateralTorqueParameters + self.assertFalse(msg.useParams) + self.assertFalse(msg.valid) + self.assertEqual(msg.totalBucketPoints, 0) + self.assertEqual(msg.latAccelFactorFiltered, cp.lateralTuning.torque.latAccelFactor) + sm = mock.Mock(frame=240, updated={}) + sm.update.side_effect = [None, StopIteration] + sm.all_checks.return_value = True + with mock.patch.object(torqued, 'prewarm_cache_contracts'), mock.patch.object(torqued, 'config_realtime_process'), \ + mock.patch.object(torqued.messaging, 'SubMaster', return_value=sm), mock.patch.object(torqued.messaging, 'PubMaster'), \ + mock.patch.object(torqued, 'put_cache') as persist, self.assertRaises(StopIteration): + torqued.main() + persist.assert_not_called() + self.assertEqual(cache.read_bytes(), b'preserved previous learning') + + def test_standard_and_untuned_learner_keep_upstream_collection_and_cache(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, ENV): + params = Params() + for vehicle in (*VEHICLES, HYUNDAI.KIA_EV6): + cp = cp_for(vehicle) + if vehicle in VEHICLES: + select(params, cp, ControllerMode.STANDARD) + with self.subTest(vehicle=vehicle), mock.patch.object(torqued, 'get_cache', return_value=None) as restore: + estimator = torqued.TorqueEstimator(cp) + self.assertEqual(restore.call_count, 2) + self.assertTrue(estimator.learning_allowed) + self.assertTrue(estimator.use_params) + estimator.handle_log(1, 'carControl', SimpleNamespace(latActive=True)) + self.assertEqual(list(estimator.raw_points['lat_active']), [True]) + select(params, cp_for(VEHICLES[0]), ControllerMode.STANDARD) + params.put_bool(LEARNING_OFF_KEY, True, block=True) + cp = cp_for(VEHICLES[0]) + estimator = torqued.TorqueEstimator(cp, allow_learning=learning_allowed(params, cp)) + self.assertFalse(estimator.learning_allowed) + + def test_offline_fitting_is_not_disabled_by_saved_controller_or_learning_preference(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, ENV): + params, cp = Params(), cp_for(HYUNDAI.HYUNDAI_IONIQ_6) + select(params, cp, ControllerMode.STARPILOT) + params.put_bool(LEARNING_OFF_KEY, True, block=True) + self.assertFalse(learning_allowed(params, cp)) + with mock.patch.object(torqued, 'get_cache', return_value=None): + estimator = torqued.TorqueEstimator(cp, decimated=True, track_all_points=True) + estimator.handle_log(1, 'carControl', SimpleNamespace(latActive=True)) + self.assertEqual(list(estimator.raw_points['lat_active']), [True]) + self.assertTrue(estimator.use_params) + with mock.patch.object(estimator.filtered_points, 'is_calculable', return_value=True), \ + mock.patch.object(estimator.filtered_points, 'is_valid', return_value=True), \ + mock.patch.object(estimator, 'estimate_params', return_value=(3.1, 0.01, 0.10)) as fit: + message = estimator.get_msg().lateralTorqueParameters + fit.assert_called_once() + self.assertTrue(message.valid) + + def test_direct_controller_consumer_rejects_learned_messages_and_latches_policy(self): + now = 1_000_000_000 + + def feed_learning(controls): + # Real SubMaster frequency checks need a measured interval. Host development + # defaults to SIMULATION=1, which otherwise hides an unqualified fixture. + self.assertFalse(controls.sm.simulation) + for tick in range(2): + stamp = now + tick * 250_000_000 # lateralTorqueParameters publishes at 4 Hz. + feed(controls, stamp, 0) + controls.sm.update_msgs(stamp / 1e9, [learned_event(cp, stamp)]) + self.assertEqual(controls.sm.all_checks(['lateralTorqueParameters']), tick == 1) + + for vehicle in (*VEHICLES, HYUNDAI.KIA_EV6): + with self.subTest(vehicle=vehicle), OpenpilotPrefix(), mock.patch.dict(os.environ, {**ENV, 'SIMULATION': '0'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params, cp = Params(), cp_for(vehicle) + params.put('CarParams', cp.to_bytes(), block=True) + controls = Controls() + self.assertIsNone(controls.torque_host) + feed_learning(controls) + with mock.patch.object(controls.LaC, 'update_torque_parameters') as update: + controls.state_control() + self.assertEqual(update.call_count, 0 if vehicle in VEHICLES else 1) + if vehicle in VEHICLES: + select(params, cp, ControllerMode.STANDARD) + controls.state_control() + update.assert_not_called() + self.assertEqual(controls.LaC.controller_mode, ControllerMode.STARPILOT) + next_drive = Controls() + self.assertTrue(next_drive.torque_learning_allowed) + self.assertEqual(next_drive.LaC.controller_mode, ControllerMode.STANDARD) + feed_learning(next_drive) + with mock.patch.object(next_drive.LaC, 'update_torque_parameters') as standard_update: + next_drive.state_control() + standard_update.assert_called_once() + invalid = learned_event(cp, now + 500_000_000).as_builder() + invalid.valid = False + next_drive.sm.update_msgs((now + 500_000_000) / 1e9, [invalid.as_reader()]) + self.assertFalse(next_drive.sm.all_checks(['lateralTorqueParameters'])) + next_drive.state_control() + standard_update.assert_called_once() + invalid.valid = True + next_drive.sm.update_msgs((now + 750_000_000) / 1e9, [invalid.as_reader()]) + self.assertTrue(next_drive.sm.all_checks(['lateralTorqueParameters'])) + next_drive.sm.update_msgs((now + 4_000_000_000) / 1e9, []) + self.assertFalse(next_drive.sm.all_checks(['lateralTorqueParameters'])) + next_drive.state_control() + standard_update.assert_called_once() + params.put_bool(LEARNING_OFF_KEY, True, block=True) + disabled = Controls() + self.assertFalse(disabled.torque_learning_allowed) + feed_learning(disabled) + with mock.patch.object(disabled.LaC, 'update_torque_parameters') as disabled_update: + disabled.state_control() + disabled_update.assert_not_called() + + def test_manual_factor_and_friction_do_not_inherit_learning_with_starpilot(self): + with OpenpilotPrefix(): + params, cp = Params(), cp_for(HYUNDAI.HYUNDAI_IONIQ_6) + params.put_bool('AdvancedLateralTune', True, block=True) + params.put('SteerLatAccel', cp.lateralTuning.torque.latAccelFactor * 1.1, block=True) + host = TorqueHost(params, cp, allow_learning=learning_allowed(params, cp)) + base = host.vehicle + now = 1_000_000_000 + learned = LearnedFrame(now, factor=base.lat_accel_factor * 1.2, offset=0.03, friction=base.friction * 1.1) + host.sample(learned, now_ns=now, lat_active=True) + self.assertEqual(host.selected.source, TorqueSource.USER) + self.assertAlmostEqual(host.selected.lat_accel_factor, base.lat_accel_factor * 1.1) + self.assertEqual(host.selected.lat_accel_offset, base.lat_accel_offset) + self.assertEqual(host.selected.friction, base.friction) + params.remove('SteerLatAccel') + params.put('SteerFriction', base.friction * 1.2, block=True) + host.settings = read_settings(params, base) + self.assertEqual(host._select(learned, now).lat_accel_factor, base.lat_accel_factor) + self.assertAlmostEqual(host._select(learned, now).friction, base.friction * 1.2) + select(params, cp, ControllerMode.STANDARD) + self.assertFalse(host.allow_learning) + self.assertEqual(host._select(learned, now).lat_accel_factor, base.lat_accel_factor) + params.put_bool('AdvancedLateralTune', False, block=True) + params.put_bool(LEARNING_OFF_KEY, True, block=True) + host.settings = read_settings(params, base) + self.assertFalse(host.settings.advanced) + self.assertFalse(TorqueHost(params, cp, allow_learning=learning_allowed(params, cp)).allow_learning) + + def test_running_manual_host_latches_both_learning_directions_and_keeps_manual_edits(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, ENV), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params, cp = Params(), cp_for(HYUNDAI.HYUNDAI_IONIQ_6) + params.put('CarParams', cp.to_bytes(), block=True) + params.put_bool('AdvancedLateralTune', True, block=True) + select(params, cp, ControllerMode.STANDARD) + controls = Controls() + host, base = controls.torque_host, controls.torque_host.vehicle + now = 1_000_000_000 + learned = LearnedFrame(now, factor=base.lat_accel_factor * 1.2, offset=0.03, friction=base.friction * 1.1) + host.sample(learned, now_ns=now, lat_active=True) + self.assertEqual(host.selected.source, TorqueSource.LEARNED) + select(params, cp, ControllerMode.STARPILOT) + params.put_bool(LEARNING_OFF_KEY, True, block=True) + for tick in range(1, 102): + stamp = now + tick * 10_000_000 + learned.logMonoTime['lateralTorqueParameters'] = stamp + host.sample(learned, now_ns=stamp, lat_active=True) + self.assertEqual(controls.LaC.controller_mode, ControllerMode.STANDARD) + self.assertEqual(host.selected.source, TorqueSource.LEARNED) + params.put('SteerLatAccel', base.lat_accel_factor * 1.1, block=True) + host.last_refresh_ns = None + host.sample(learned, now_ns=stamp + 10_000_000, lat_active=True) + self.assertEqual(host.selected.source, TorqueSource.USER) + self.assertAlmostEqual(host.selected.lat_accel_factor, base.lat_accel_factor * 1.1) + self.assertEqual(host.selected.friction, learned.state.frictionCoefficientFiltered) + + next_drive = Controls() + disabled = next_drive.torque_host + self.assertFalse(disabled.allow_learning) + select(params, cp, ControllerMode.STANDARD) + params.put_bool(LEARNING_OFF_KEY, False, block=True) + disabled.sample(learned, now_ns=stamp + 20_000_000, lat_active=True) + self.assertEqual(next_drive.LaC.controller_mode, ControllerMode.STARPILOT) + self.assertEqual(disabled.selected.source, TorqueSource.USER) + self.assertEqual(disabled.selected.friction, base.friction) + + def test_unrelated_manual_host_preserves_master_off_behavior(self): + with OpenpilotPrefix(): + params, cp = Params(), cp_for(TOYOTA.TOYOTA_RAV4_TSS2) + host = TorqueHost(params, cp) + for raw in (b'1', b'invalid'): + Path(params.get_param_path(LEARNING_OFF_KEY)).write_bytes(raw) + settings = read_settings(params, host.vehicle) + self.assertTrue(settings.valid) + self.assertFalse(settings.advanced) + self.assertFalse(settings.force_auto_off) + self.assertIsNone(TorqueHost(params, cp).allow_learning) + + +if __name__ == '__main__': + unittest.main() diff --git a/openpilot/starpilot/lateral/tests/test_torque_runtime.py b/openpilot/starpilot/lateral/tests/test_torque_runtime.py new file mode 100644 index 0000000000..69bc5bb70f --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_torque_runtime.py @@ -0,0 +1,291 @@ +"""Native Corolla torque source selection, saved choices, and controller gates.""" + +import math +import os +from pathlib import Path +from types import SimpleNamespace +import unittest +from unittest import mock + +from openpilot.cereal import messaging +from openpilot.common.params import ParamKeyFlag, Params +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.selfdrive.controls.controlsd import Controls +from openpilot.selfdrive.locationd.torqued import TorqueEstimator +from openpilot.starpilot.lateral.tests.test_lane_runtime import feed +from openpilot.starpilot.lateral.controller_selection import DOCUMENT_KEY as CONTROLLER_KEY, ControllerMode, replace_mode +from openpilot.starpilot.lateral.torque_runtime import TorqueHost, development_enabled, read_settings +from openpilot.starpilot.lateral.torque_tuning import TorqueSource +from opendbc.car.car_helpers import interfaces +from opendbc.car.toyota.values import CAR + + +def corolla(): + return interfaces[CAR.TOYOTA_COROLLA_TSS2].get_non_essential_params(CAR.TOYOTA_COROLLA_TSS2) + + +class LearnedFrame: + def __init__(self, now_ns, *, factor=2.0, offset=0.1, friction=0.1, version=1, valid=True): + self.logMonoTime = {'lateralTorqueParameters': now_ns} + self.checked = True + self.state = SimpleNamespace(useParams=True, valid=valid, version=version, latAccelFactorFiltered=factor, + latAccelOffsetFiltered=offset, frictionCoefficientFiltered=friction) + + def all_checks(self, _names): + return self.checked + + def __getitem__(self, _name): + return self.state + + +class TorqueRuntimeTests(unittest.TestCase): + def test_saved_partial_precedence_and_corrupt_raw_bytes(self): + with OpenpilotPrefix(): + params, cp = Params(), corolla() + host = TorqueHost(params, cp) + base = host.vehicle + self.assertFalse(read_settings(params, base).advanced) + params.put_bool('AdvancedLateralTune', True, block=True) + params.put('SteerLatAccel', base.lat_accel_factor * 1.2, block=True) + params.put('SteerFriction', min(1.0, base.friction * 1.2), block=True) + settings = read_settings(params, base) + self.assertTrue(settings.valid) + host.settings = settings + learned = LearnedFrame(1_000_000_000, factor=base.lat_accel_factor * 1.1, + offset=0.1, friction=base.friction * 1.1) + selected = host._select(learned, 1_000_000_000) + self.assertEqual(selected.source, TorqueSource.USER) + self.assertEqual(selected.lat_accel_factor, settings.user_factor) + self.assertEqual(selected.lat_accel_offset, base.lat_accel_offset) + self.assertEqual(selected.friction, settings.user_friction) + Path(params.get_param_path('SteerLatAccel')).write_bytes(b'nan') + self.assertFalse(read_settings(params, base).valid) + self.assertTrue(params.get_bool('AdvancedLateralTune')) + host.settings = read_settings(params, base) + self.assertEqual(host._select(learned, 1_000_000_000), base) + self.assertEqual(host.sample(learned, now_ns=1_000_000_000, lat_active=True), base) + + def test_frozen_auto_synced_stock_and_individual_overrides(self): + with OpenpilotPrefix(): + params, cp = Params(), corolla() + host = TorqueHost(params, cp) + base = host.vehicle + now = 1_000_000_000 + learned = LearnedFrame(now, factor=base.lat_accel_factor * 1.1, + offset=0.1, friction=base.friction * 1.1) + params.put_bool('AdvancedLateralTune', True, block=True) + params.put('SteerLatAccel', base.lat_accel_factor, block=True) + params.put('SteerFriction', base.friction, block=True) + host.settings = read_settings(params, base) + self.assertIsNone(host.settings.user_factor) + self.assertIsNone(host.settings.user_friction) + self.assertEqual(host._select(learned, now).source, TorqueSource.LEARNED) + + params.put('SteerLatAccel', base.lat_accel_factor * 1.2, block=True) + host.settings = read_settings(params, base) + factor_only = host._select(learned, now) + self.assertEqual(factor_only.source, TorqueSource.USER) + self.assertAlmostEqual(factor_only.lat_accel_factor, base.lat_accel_factor * 1.2) + self.assertEqual(factor_only.lat_accel_offset, base.lat_accel_offset) + self.assertAlmostEqual(factor_only.friction, base.friction * 1.1) + + params.put('SteerLatAccel', base.lat_accel_factor, block=True) + params.put('SteerFriction', base.friction * 1.2, block=True) + host.settings = read_settings(params, base) + friction_only = host._select(learned, now) + self.assertEqual(friction_only.source, TorqueSource.USER) + self.assertAlmostEqual(friction_only.lat_accel_factor, base.lat_accel_factor * 1.1) + self.assertAlmostEqual(friction_only.lat_accel_offset, 0.1) + self.assertAlmostEqual(friction_only.friction, base.friction * 1.2) + + # A frozen stock-sync file distinguishes an old stock save from a user edit + # after a later CarParams retune changes the current stock value. + old_stock = base.lat_accel_factor * 0.9 + params.put('SteerLatAccel', old_stock, block=True) + params.put('SteerLatAccelStock', old_stock, block=True) + params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START) + self.assertAlmostEqual(params.get('SteerLatAccelStock'), old_stock) + host.settings = read_settings(params, base) + self.assertIsNone(host.settings.user_factor) + self.assertAlmostEqual(host._select(learned, now).lat_accel_factor, learned.state.latAccelFactorFiltered) + + # Old untouched stock may be outside today's safe custom range: it is a + # stock-tracking marker, not a custom override to reject or apply. + older_stock = base.lat_accel_factor * 2.0 + params.put('SteerLatAccel', older_stock, block=True) + params.put('SteerLatAccelStock', older_stock, block=True) + host.settings = read_settings(params, base) + self.assertTrue(host.settings.valid) + self.assertIsNone(host.settings.user_factor) + self.assertAlmostEqual(host._select(learned, now).lat_accel_factor, learned.state.latAccelFactorFiltered) + params.put('SteerFriction', base.friction, block=True) + params.put_bool('ForceAutoTuneOff', True, block=True) + host.settings = read_settings(params, base) + self.assertEqual(host._select(learned, now), base) + + def test_malformed_saved_bool_and_numeric_settings_fail_closed(self): + with OpenpilotPrefix(): + params, cp = Params(), corolla() + host = TorqueHost(params, cp) + params.put_bool('AdvancedLateralTune', True, block=True) + for key, bad in (('AdvancedLateralTune', b'yes'), ('ForceAutoTuneOff', b'2'), + ('SteerLatAccel', b'nan'), ('SteerFriction', b'inf'), + ('SteerLatAccelStock', b'bad'), ('SteerFrictionStock', b'bad')): + with self.subTest(key=key): + path = Path(params.get_param_path(key)) + original = path.read_bytes() if path.exists() else None + path.write_bytes(bad) + self.assertFalse(read_settings(params, host.vehicle).valid) + if original is None: + path.unlink() + else: + path.write_bytes(original) + + def test_learned_source_freshness_version_and_override(self): + with OpenpilotPrefix(): + params, cp = Params(), corolla() + host = TorqueHost(params, cp) + base = host.vehicle + frame = LearnedFrame(1_000_000_000, factor=base.lat_accel_factor * 1.1, + offset=0.1, friction=base.friction * 1.1) + self.assertEqual(host._select(frame, 1_000_000_000).source, TorqueSource.LEARNED) + frame.state.version = 99 + self.assertEqual(host._select(frame, 1_000_000_000), base) + frame.state.version = 1 + frame.logMonoTime['lateralTorqueParameters'] = 1 + self.assertEqual(host._select(frame, 1_000_000_000), base) + frame.logMonoTime['lateralTorqueParameters'] = 1_000_000_000 + frame.checked = False + self.assertEqual(host._select(frame, 1_000_000_000), base) + params.put_bool('AdvancedLateralTune', True, block=True) + params.put_bool('ForceAutoTuneOff', True, block=True) + host.settings = read_settings(params, base) + frame.checked = True + self.assertEqual(host._select(frame, 1_000_000_000), base) + + def test_exact_car_development_policy(self): + cp = corolla() + with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}): + self.assertTrue(development_enabled(cp)) + cp.carFingerprint = 'TOYOTA_RAV4_TSS2_2023' + self.assertFalse(development_enabled(cp)) + with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '0'}): + self.assertFalse(development_enabled(corolla())) + + def test_opt_in_learner_skips_old_cache_without_deleting_it(self): + cp = corolla() + with OpenpilotPrefix(), mock.patch('openpilot.selfdrive.locationd.torqued.get_cache', return_value=None) as get_cache: + with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}): + fresh = TorqueEstimator(cp.as_reader()) + self.assertEqual(get_cache.call_count, 0) + self.assertEqual(fresh.filtered_params['latAccelFactor'].x, cp.lateralTuning.torque.latAccelFactor) + cp.carFingerprint = 'TOYOTA_RAV4_TSS2_2023' + with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}): + TorqueEstimator(cp.as_reader()) + self.assertEqual(get_cache.call_count, 2) + + def test_native_controls_default_equivalence_and_selected_torque(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, {'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0', + 'LANE_CENTERING_REPLAY_RUNTIME': '0'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params, cp = Params(), corolla() + params.put('CarParams', cp.to_bytes(), block=True) + with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '0'}): + baseline = Controls() + with mock.patch.dict(os.environ, {'TORQUE_REPLAY_RUNTIME': '1'}): + selected = Controls() + self.assertIsNone(baseline.torque_host) + self.assertIsNotNone(selected.torque_host) + for tick in range(110): + now = 1_000_000_000 + tick * 10_000_000 + for controls in (baseline, selected): + feed(controls, now, tick) + a, _ = baseline.state_control() + b, _ = selected.state_control() + self.assertEqual(a.to_dict(), b.to_dict()) + params.put_bool('AdvancedLateralTune', True, block=True) + native_factor = selected.torque_host.vehicle.lat_accel_factor + params.put('SteerLatAccel', native_factor * 1.3, block=True) + torque_differences = [] + for tick in range(110, 230): + now = 1_000_000_000 + tick * 10_000_000 + for controls in (baseline, selected): + feed(controls, now, tick) + reference, _ = baseline.state_control() + command, _ = selected.state_control() + torque_differences.append(abs(command.actuators.torque - reference.actuators.torque)) + self.assertTrue(command.latActive) + self.assertGreater(selected.torque_host.applied.lat_accel_factor, native_factor) + self.assertLessEqual(selected.torque_host.applied.lat_accel_factor, native_factor * 1.3) + self.assertGreater(selected.LaC.pid.pos_limit, baseline.LaC.pid.pos_limit) + self.assertGreater(max(torque_differences), 1e-6) + feed(selected, now + 10_000_000, 230, active=False, enabled=False) + off, _ = selected.state_control() + self.assertFalse(off.latActive) + self.assertEqual(off.actuators.torque, 0.0) + self.assertEqual(selected.torque_host.applied, selected.torque_host.vehicle) + self.assertEqual(selected.LaC.pid.i, 0.0) + feed(selected, now + 20_000_000, 231, can_valid=False) + invalid, _ = selected.state_control() + self.assertFalse(invalid.latActive) + self.assertEqual(invalid.actuators.torque, 0.0) + feed(selected, now + 30_000_000, 232) + resumed, _ = selected.state_control() + self.assertTrue(resumed.latActive) + self.assertLess(abs(resumed.actuators.torque), selected.LaC.steer_max) + feed(selected, now + 40_000_000, 233, can_timeout=True) + timeout, _ = selected.state_control() + self.assertFalse(timeout.latActive) + self.assertEqual(timeout.actuators.torque, 0.0) + feed(selected, now + 50_000_000, 234, override=True) + prior_integral = selected.LaC.pid.i + override, _ = selected.state_control() + self.assertTrue(override.latActive) + self.assertTrue(math.isfinite(override.actuators.torque)) + self.assertEqual(selected.LaC.pid.i, prior_integral) + + def test_actual_controls_learned_to_user_to_stale_source(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, {'REPLAY': '1', 'TORQUE_REPLAY_RUNTIME': '1', + 'AOL_REPLAY_RUNTIME': '0', 'LANE_CENTERING_REPLAY_RUNTIME': '0'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params, cp = Params(), corolla() + Path(params.get_param_path(CONTROLLER_KEY)).write_bytes(replace_mode(None, cp, ControllerMode.STANDARD)) + params.put('CarParams', cp.to_bytes(), block=True) + controls = Controls() + base = controls.torque_host.vehicle + target = base.lat_accel_factor * 1.1 + for tick in range(120): + now = 1_000_000_000 + tick * 10_000_000 + feed(controls, now, tick) + if tick % 25 == 0: + event = messaging.new_message('lateralTorqueParameters', valid=True, logMonoTime=now) + state = event.lateralTorqueParameters + state.useParams = state.valid = True + state.version = 1 + state.latAccelFactorFiltered = target + state.latAccelOffsetFiltered = 0.1 + state.frictionCoefficientFiltered = base.friction + controls.sm.update_msgs(now / 1e9, [event.as_reader()]) + controls.state_control() + self.assertEqual(controls.torque_host.selected.source, TorqueSource.LEARNED) + self.assertGreater(controls.torque_host.applied.lat_accel_factor, base.lat_accel_factor) + params.put_bool('AdvancedLateralTune', True, block=True) + params.put('SteerLatAccel', base.lat_accel_factor * 1.2, block=True) + for tick in range(120, 230): + now = 1_000_000_000 + tick * 10_000_000 + feed(controls, now, tick) + controls.state_control() + self.assertEqual(controls.torque_host.selected.source, TorqueSource.USER) + self.assertEqual(controls.torque_host.selected.lat_accel_offset, base.lat_accel_offset) + self.assertGreater(controls.torque_host.applied.lat_accel_factor, target) + params.put_bool('AdvancedLateralTune', False, block=True) + for tick in range(230, 340): + now = 1_000_000_000 + tick * 10_000_000 + feed(controls, now, tick) + controls.state_control() + self.assertEqual(controls.torque_host.selected, base) + self.assertAlmostEqual(controls.torque_host.applied.lat_accel_factor, base.lat_accel_factor) + + +if __name__ == '__main__': + unittest.main() diff --git a/openpilot/starpilot/lateral/tests/test_torque_settings.py b/openpilot/starpilot/lateral/tests/test_torque_settings.py new file mode 100644 index 0000000000..f50ea7e994 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_torque_settings.py @@ -0,0 +1,86 @@ +"""Versioned Torque preference document against real temporary Params.""" + +from pathlib import Path +import json +import unittest + +from openpilot.common.params import ParamKeyType, Params +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.starpilot.lateral.torque_runtime import TorqueHost, read_settings +from openpilot.starpilot.lateral.torque_settings import ( + DOCUMENT_KEY, LEGACY_KEYS, FieldChoice, LegacyMode, PlatformProfile, interpret_legacy, parse_document, + replace_field, resolve_document, serialize_document, +) +from opendbc.car.car_helpers import interfaces + + +def corolla(): + return interfaces["TOYOTA_COROLLA_TSS2"].get_non_essential_params("TOYOTA_COROLLA_TSS2") + + +class TorqueDocumentTests(unittest.TestCase): + def test_native_key_is_optional_nondefault_json(self): + with OpenpilotPrefix(): + params = Params() + self.assertEqual(params.get_type(DOCUMENT_KEY), ParamKeyType.JSON) + self.assertIsNone(params.get(DOCUMENT_KEY, return_default=True)) + + def test_legacy_absence_and_explicit_document_ignore_old_bytes(self): + with OpenpilotPrefix(): + params = Params() + host = TorqueHost(params, corolla()) + basis = (host.vehicle.lat_accel_factor, host.vehicle.lat_accel_offset, host.vehicle.friction) + params.put_bool("AdvancedLateralTune", True, block=True) + old_factor = basis[0] * 1.2 + params.put("SteerLatAccel", old_factor, block=True) + before = {key: Path(params.get_param_path(key)).read_bytes() if Path(params.get_param_path(key)).exists() else None + for key in LEGACY_KEYS} + self.assertEqual(read_settings(params, host.vehicle).user_factor, old_factor) + raw = serialize_document({host.vehicle.vehicle: PlatformProfile(basis, FieldChoice("custom", basis[0]), FieldChoice())}) + params.put(DOCUMENT_KEY, json.loads(raw), block=True) + after = read_settings(params, host.vehicle) + self.assertTrue(after.valid) + self.assertIsNone(after.user_factor) + self.assertEqual(parse_document(raw)[host.vehicle.vehicle].factor.mode, "custom") + self.assertEqual({key: Path(params.get_param_path(key)).read_bytes() if Path(params.get_param_path(key)).exists() else None + for key in LEGACY_KEYS}, before) + + def test_partial_fields_round_trip_and_model_scope(self): + cp = corolla() + host = TorqueHost(Params(), cp) + basis = (host.vehicle.lat_accel_factor, host.vehicle.lat_accel_offset, host.vehicle.friction) + profiles = replace_field({}, host.vehicle.vehicle, basis, "factor", "custom", basis[0] * 1.1) + profiles = replace_field(profiles, host.vehicle.vehicle, basis, "friction", "custom", basis[2] * 1.2) + profiles = replace_field(profiles, host.vehicle.vehicle, basis, "factor", "source", None) + parsed = parse_document(serialize_document(profiles)) + self.assertEqual(parsed[host.vehicle.vehicle].factor.custom_value, basis[0] * 1.1) + self.assertEqual(resolve_document(parsed, host.vehicle.vehicle, basis), (None, basis[2] * 1.2, False)) + self.assertEqual(resolve_document(parsed, "TOYOTA_CAMRY", basis), (None, None, False)) + changed_basis = (basis[0] * 1.1, basis[1], basis[2]) + self.assertEqual(resolve_document(parsed, host.vehicle.vehicle, changed_basis), (None, None, True)) + + def test_bad_document_fails_closed_without_legacy_fallback(self): + with OpenpilotPrefix(): + params = Params() + host = TorqueHost(params, corolla()) + params.put_bool("AdvancedLateralTune", True, block=True) + params.put("SteerLatAccel", host.vehicle.lat_accel_factor * 1.2, block=True) + for raw in (b"{}", b'{"schemaVersion":1,"schemaVersion":1,"vehicles":{}}', + b'{"schemaVersion":1,"vehicles":{"TOYOTA_COROLLA_TSS2":{"basis":{},"factor":{},"friction":{}}}}', + b'[' * 1100 + b'0' + b']' * 1100, + b'{"schemaVersion":1,"vehicles":{}}' + b' ' * 20_000, + b'{"schemaVersion":' + b'9' * 5000 + b',"vehicles":{}}', + b'{"schemaVersion":1,"vehicles":{"TOYOTA_COROLLA_TSS2":{"basis":{"latAccelFactor":' + + b'9' * 350 + b',"latAccelOffset":0,"friction":0},"factor":{"mode":"source","customValue":null},' + + b'"friction":{"mode":"source","customValue":null}}}}'): + Path(params.get_param_path(DOCUMENT_KEY)).write_bytes(raw) + self.assertFalse(read_settings(params, host.vehicle).valid) + self.assertEqual(Path(params.get_param_path(DOCUMENT_KEY)).read_bytes(), raw) + + def test_shared_legacy_parser_rejects_bad_stock_marker(self): + basis = (2.0, 0.1, 0.12) + with self.assertRaises(ValueError): + interpret_legacy({"SteerLatAccel": b"2.1", "SteerLatAccelStock": b"bad"}, basis) + old_stock = interpret_legacy({"SteerLatAccel": b"2.2", "SteerLatAccelStock": b"2.2"}, basis) + self.assertEqual((old_stock.factor_mode, old_stock.factor, old_stock.factor_saved), + (LegacyMode.STOCK, None, 2.2)) diff --git a/openpilot/starpilot/lateral/tests/test_torque_tuning.py b/openpilot/starpilot/lateral/tests/test_torque_tuning.py new file mode 100644 index 0000000000..df7701b3c7 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_torque_tuning.py @@ -0,0 +1,128 @@ +"""Explicit torque values exercised against the current native controller.""" + +import math +import unittest + +from openpilot.cereal import log +from opendbc.car.structs import car +from opendbc.car.car_helpers import interfaces +from opendbc.car.toyota.values import CAR as TOYOTA +from opendbc.car.vehicle_model import VehicleModel +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.torque_tuning import TorqueSource, TorqueTuning, from_record + + +PLATFORM = TOYOTA.TOYOTA_RAV4_TSS2 + + +def native_controller(): + interface = interfaces[PLATFORM] + cp = interface.get_non_essential_params(PLATFORM) + controller = LatControlTorque(cp.as_reader(), interface(cp), DT_CTRL) + return cp, controller, VehicleModel(cp) + + +def record(cp, source="vehicle"): + tune = cp.lateralTuning.torque + return {"source": source, "vehicle": cp.carFingerprint, "latAccelFactor": tune.latAccelFactor, + "latAccelOffset": tune.latAccelOffset, "friction": tune.friction} + + +def frame(controller, vm, *, active=True, curvature=0.0, speed=20.0, steering_angle=0.0, + steer_limited=False, steering_pressed=False): + cs = car.CarState.new_message() + cs.vEgo = speed + cs.steeringAngleDeg = steering_angle + cs.steeringPressed = steering_pressed + return controller.update(active, cs, vm, log.VehicleParameters.new_message(), + steer_limited, curvature, False, 0.2) + + +class TestTorqueTuning(unittest.TestCase): + def test_stock_values_leave_native_limits_unchanged(self): + cp, controller, _ = native_controller() + stock = from_record(record(cp)) + self.assertIs(stock.source, TorqueSource.VEHICLE) + self.assertEqual(stock.vehicle, cp.carFingerprint) + for selected, actual in zip(stock.upstream_update(), (controller.torque_params.latAccelFactor, + controller.torque_params.latAccelOffset, + controller.torque_params.friction), strict=True): + self.assertAlmostEqual(selected, actual) + original_limits = controller.pid.neg_limit, controller.pid.pos_limit + controller.update_torque_parameters(*stock.upstream_update()) + for selected, actual in zip((controller.pid.neg_limit, controller.pid.pos_limit), original_limits, strict=True): + self.assertAlmostEqual(selected, actual) + + def test_selected_complete_values_change_native_parameters_and_limits(self): + cp, controller, _ = native_controller() + selected = record(cp, "user") + selected["latAccelFactor"] *= 1.2 + selected["latAccelOffset"] = 0.07 + selected["friction"] += 0.02 + tuning = from_record(selected) + self.assertIs(tuning.source, TorqueSource.USER) + initial_limit = controller.pid.pos_limit + controller.update_torque_parameters(*tuning.upstream_update()) + self.assertAlmostEqual(controller.pid.pos_limit, initial_limit * 1.2, delta=1e-6) + self.assertAlmostEqual(controller.pid.neg_limit, -controller.pid.pos_limit) + self.assertAlmostEqual(controller.torque_params.latAccelOffset, 0.07) + self.assertAlmostEqual(controller.torque_params.friction, selected["friction"]) + + def test_inactive_nonzero_command_and_reengagement_use_native_buffer(self): + cp, controller, vm = native_controller() + learned = record(cp, "learned") + learned["latAccelFactor"] *= 1.05 + controller.update_torque_parameters(*from_record(learned).upstream_update()) + for _ in range(controller.lat_accel_request_buffer_len): + output, _, state = frame(controller, vm, active=False, curvature=0.001) + self.assertEqual(output, 0.0) + self.assertFalse(state.active) + self.assertTrue(all(math.isclose(value, 0.4, abs_tol=1e-6) for value in controller.lat_accel_request_buffer)) + output, _, state = frame(controller, vm, curvature=0.001) + self.assertTrue(state.active) + self.assertAlmostEqual(state.desiredLateralAccel, 0.4) + self.assertTrue(math.isfinite(output)) + self.assertNotEqual(output, 0.0) + + def test_native_saturation_sets_then_clears_under_bounded_inputs(self): + _, controller, vm = native_controller() + for _ in range(100): + output, _, state = frame(controller, vm, curvature=0.02) + self.assertLessEqual(abs(output), controller.steer_max + 1e-6) + self.assertTrue(state.saturated) + self.assertAlmostEqual(controller.sat_time, controller.sat_limit) + for _ in range(controller.lat_accel_request_buffer_len + 100): + output, _, state = frame(controller, vm, curvature=0.0) + self.assertLessEqual(abs(output), controller.steer_max + 1e-6) + self.assertFalse(state.saturated) + self.assertAlmostEqual(controller.sat_time, 0.0) + + def test_invalid_numeric_inputs_are_rejected(self): + values = [("latAccelFactor", 0), ("latAccelFactor", -1), ("latAccelFactor", float("nan")), + ("latAccelFactor", True), ("latAccelOffset", float("inf")), ("latAccelOffset", False), + ("friction", -0.01), ("friction", float("nan")), ("friction", True), ("latAccelFactor", 10 ** 1000)] + for field, value in values: + with self.subTest(field=field, value=value): + source = {"source": "vehicle", "vehicle": "test-platform", "latAccelFactor": 2.0, + "latAccelOffset": 0.0, "friction": 0.1} + source[field] = value + with self.assertRaises(ValueError): + from_record(source) + + def test_incomplete_unknown_and_legacy_fields_are_rejected(self): + source = {"source": "vehicle", "vehicle": "test-platform", "latAccelFactor": 2.0, + "latAccelOffset": 0.0, "friction": 0.1} + for extra in ("kp", "ki", "kd", "kf", "steeringAngleDeadzoneDeg", "unknown"): + with self.subTest(extra=extra), self.assertRaises(ValueError): + from_record(source | {extra: 1.0}) + with self.assertRaises(ValueError): + from_record({key: value for key, value in source.items() if key != "friction"}) + with self.assertRaises(ValueError): + from_record(source | {"source": "unrecognized"}) + with self.assertRaises(ValueError): + TorqueTuning(TorqueSource.VEHICLE, "", 2.0, 0.0, 0.1) + + +if __name__ == "__main__": + unittest.main() diff --git a/openpilot/starpilot/lateral/tests/test_turn_lane_change_smoothing.py b/openpilot/starpilot/lateral/tests/test_turn_lane_change_smoothing.py new file mode 100644 index 0000000000..fe484f8113 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_turn_lane_change_smoothing.py @@ -0,0 +1,89 @@ +import json +from pathlib import Path + +from openpilot.cereal import log +from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN +from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature +from openpilot.starpilot.lateral.lane_change_smoothing import LaneChangeSmoother, limit_rate + + +def factor(smoother, speed, state, previous=0., desired=.01, assist=True): + return smoother.factor(active=True, lane_change_state=state, speed=speed, minimum_speed=0., duration=5.777777777777778, + previous=previous, desired=desired, turn_assist=assist) + + +def test_turn_assist_releases_intersection_curve_and_prior_lane_change_tail(): + smoother = LaneChangeSmoother() + assert factor(smoother, 20., log.LaneChangeState.laneChangeStarting) < 1. + for speed in (0., .8, 2., 4., 8.2, LANE_CHANGE_SPEED_MIN - 1e-6): + assert factor(smoother, speed, log.LaneChangeState.laneChangeStarting) == 1. + assert smoother.release == 0. + assert smoother.entry_sign == 0. + assert smoother.previous_factor == 1. + # An intersection stays exempt until the model ends that lane-change state. + assert factor(smoother, LANE_CHANGE_SPEED_MIN, log.LaneChangeState.laneChangeStarting) == 1. + assert factor(smoother, LANE_CHANGE_SPEED_MIN, log.LaneChangeState.off) == 1. + assert factor(smoother, LANE_CHANGE_SPEED_MIN, log.LaneChangeState.laneChangeStarting) < 1. + + +def test_manual_and_highway_lane_change_shaping_stays_exact(): + for speed, assist in ((1., False), (8.2, False), (LANE_CHANGE_SPEED_MIN, True), (30., True)): + baseline, candidate = LaneChangeSmoother(), LaneChangeSmoother() + previous = 0. + for tick in range(700): + state = log.LaneChangeState.laneChangeStarting if tick < 400 else log.LaneChangeState.off + desired = .01 if tick < 250 else -.01 + expected = factor(baseline, speed, state, previous, desired, False) + actual = factor(candidate, speed, state, previous, desired, assist) + assert actual == expected + previous, _ = clip_curvature(speed, previous, limit_rate(speed, previous, desired, actual), 0.) + + +def test_recorded_intersection_requests_retain_native_curvature_envelope(): + recorded = json.loads((Path(__file__).parent / 'fixtures/oct2_turns.json').read_text()) + for sequence in recorded['turns']: + smoother = LaneChangeSmoother() + previous = sequence[0][3] + exercised = 0 + for _, speed, desired, _, state_name in sequence: + state = getattr(log.LaneChangeState, state_name) + shaping = factor(smoother, speed, state, previous, desired) + limited = limit_rate(speed, previous, desired, shaping) + output, _ = clip_curvature(speed, previous, limited, 0.) + native, _ = clip_curvature(speed, previous, desired, 0.) + if speed < LANE_CHANGE_SPEED_MIN: + assert shaping == 1. + assert output == native + exercised += 1 + assert abs(output) <= .2 + assert abs(output) <= 3. / max(speed, 1.) ** 2 + 1e-12 + previous = output + assert exercised > 500 + + +def test_recorded_turn_onset_and_exit_are_not_delayed_by_lane_change_pacing(): + recorded = json.loads((Path(__file__).parent / 'fixtures/oct2_turns.json').read_text()) + results = [] + # The recorded sustained curvature-rate factor is 0.0863065, matching this + # supported duration. This is a fixed-input response, not a vehicle simulation. + duration = 6.888888888888889 + for sequence in recorded['turns']: + tracks = [] + for assist in (False, True): + smoother = LaneChangeSmoother() + previous = sequence[0][3] + trace = [] + for time, speed, desired, _, state_name in sequence: + shaping = smoother.factor(active=True, lane_change_state=getattr(log.LaneChangeState, state_name), + speed=speed, minimum_speed=0., duration=duration, + previous=previous, desired=desired, turn_assist=assist) + previous, _ = clip_curvature(speed, previous, limit_rate(speed, previous, desired, shaping), 0.) + trace.append((time, previous)) + onset = next(time for time, curvature in trace if curvature < -.02) + peak = min(range(len(trace)), key=lambda index: trace[index][1]) + exit_time = next(time for time, curvature in trace[peak:] if abs(curvature) < .01) + tracks.append((onset, exit_time)) + results.append(tracks) + # First bookmark is the unwind complaint; second is late turn initiation. + assert results[0][0][1] - results[0][1][1] > 1. + assert results[1][0][0] - results[1][1][0] > 2. diff --git a/openpilot/starpilot/lateral/tests/test_turn_model_runtime.py b/openpilot/starpilot/lateral/tests/test_turn_model_runtime.py new file mode 100644 index 0000000000..5682a88144 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_turn_model_runtime.py @@ -0,0 +1,88 @@ +"""Saved assist reaches the model/torque path only with existing rolling authority.""" + +import json +import os +import unittest +from unittest.mock import patch + +from openpilot.common.params import Params +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.selfdrive.controls.controlsd import Controls +from openpilot.starpilot.lateral.tests.test_lane_runtime import feed, ioniq_candidate +from openpilot.starpilot.lateral.controller_selection import ControllerMode, DOCUMENT_KEY, replace_mode + + +def rolling_feed(controls, timestamp, tick, *, speed=.5, standstill=False, fault=False, active=True, gear='drive'): + captured = [] + with patch.object(controls.sm, 'update_msgs', side_effect=lambda _time, events: captured.extend(events)): + feed(controls, timestamp, tick, fault=fault, active=active, enabled=active, signal=True) + events = [event.as_builder() for event in captured] + cs = next(event.carState for event in events if event.which() == 'carState') + cs.vEgo = cs.vEgoRaw = speed + cs.standstill = standstill + cs.gearShifter = gear + cs.aEgo = -.1 + cs.leftBlinker = False + cs.rightBlinker = True + if tick % 5 == 0: + md = next(event.modelV2 for event in events if event.which() == 'modelV2') + md.position.x = [float(x) for x in range(51)] + md.position.y = [.1 * x * x for x in range(51)] + md.action.desiredCurvature = .0001 + controls.sm.update_msgs(timestamp / 1e9, [event.as_reader() for event in events]) + + +class TestTurnModelRuntime(unittest.TestCase): + def construct(self, enabled, mode=ControllerMode.STARPILOT): + cp, _ = ioniq_candidate() + params = Params() + params.put('CarParams', cp.to_bytes(), block=True) + params.put_bool('TurnAssist', enabled, block=True) + params.put_bool('OpenpilotEnabledToggle', True, block=True) + params.put(DOCUMENT_KEY, json.loads(replace_mode(None, cp, mode)), block=True) + return Controls() + + def test_off_and_standard_preserve_existing_commands(self): + for mode in (ControllerMode.STARPILOT, ControllerMode.STANDARD): + with self.subTest(mode=mode), OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '1', 'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}): + selected = self.construct(False, mode) + reference = self.construct(mode == ControllerMode.STANDARD, mode) + for tick in range(20): + now = 1_000_000_000 + tick * 10_000_000 + rolling_feed(selected, now, tick) + rolling_feed(reference, now, tick) + actual, _ = selected.state_control() + expected, _ = reference.state_control() + self.assertEqual(actual.to_bytes(), expected.to_bytes()) + self.assertEqual(selected.model_turn_assist.turn_hold_curvature, 0.) + + def test_model_bias_reaches_actual_curvature_and_torque_limits(self): + with OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '1', 'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}): + controls = self.construct(True) + reference = self.construct(False) + before = controls.LaC.torque_params.to_dict() + maximum_hold = maximum_difference = 0. + with patch.object(controls.model_turn_assist, 'update', wraps=controls.model_turn_assist.update) as update: + for tick in range(120): + rolling_feed(controls, 1_000_000_000 + tick * 10_000_000, tick) + rolling_feed(reference, 1_000_000_000 + tick * 10_000_000, tick) + command, _ = controls.state_control() + reference.state_control() + maximum_hold = max(maximum_hold, abs(controls.model_turn_assist.turn_hold_curvature)) + maximum_difference = max(maximum_difference, abs(controls.desired_curvature - reference.desired_curvature)) + self.assertLessEqual(abs(command.actuators.torque), 1.) + self.assertEqual(update.call_count, 120) + self.assertGreater(maximum_hold, 0.) + self.assertGreater(maximum_difference, 0.) + self.assertEqual(controls.LaC.torque_params.to_dict(), before) + + def test_standstill_speed_fault_reverse_and_inactive_deny_assist(self): + for gate in ({'speed': 0.}, {'speed': .044}, {'standstill': True}, {'fault': True}, {'gear': 'reverse'}, {'active': False}): + with self.subTest(gate=gate), OpenpilotPrefix(), patch.dict(os.environ, {'SIMULATION': '1', 'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}): + controls = self.construct(True) + with patch.object(controls.model_turn_assist, 'update', wraps=controls.model_turn_assist.update) as update: + for tick in range(10): + rolling_feed(controls, 1_000_000_000 + tick * 10_000_000, tick, **gate) + controls.state_control() + update.assert_not_called() + self.assertEqual(controls.model_turn_assist.turn_hold_curvature, 0.) diff --git a/openpilot/starpilot/lateral/tests/testdata/README.md b/openpilot/starpilot/lateral/tests/testdata/README.md new file mode 100644 index 0000000000..310c1dbf52 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/testdata/README.md @@ -0,0 +1,8 @@ +# Lateral controller regression vectors + +These source-derived vectors are inputs to `test_ioniq6_policy.py`. + +- `ioniq6_frozen_2023.json` and `ioniq6_frozen_2025.json` cover the two Ioniq 6 controller variants through stop, crawl, pull-away, highway, steering override, and restart. They include controller output, requested lateral acceleration and jerk, PID terms, and feedforward. Their `helpers` entries cover the corresponding shaping functions. +- `non_ioniq_parent.json` checks that Corolla TSS2 and Ioniq 5 controller output remains unchanged by the Ioniq-specific policy. + +The tests use matching vehicle parameters for each comparison. These vectors check software behavior; they do not establish physical steering performance. diff --git a/openpilot/starpilot/lateral/tests/testdata/ioniq6_frozen_2023.json b/openpilot/starpilot/lateral/tests/testdata/ioniq6_frozen_2023.json new file mode 100644 index 0000000000..eb394c35f4 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/testdata/ioniq6_frozen_2023.json @@ -0,0 +1,208 @@ +{ + "profile": "2023", + "factor": 3.6600000858306885, + "rows": [ + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.10946385067439653, + -6.3100724219111726e-06, + -6.310072058113292e-05, + 0.1341981738805771, + 1.8787743556458736e-06, + 0.00032710772939026356 + ], + [ + -0.10883246512145582, + -1.2177733879070729e-05, + -0.00012177733879070729, + 0.1318926364183426, + 3.725271199073177e-06, + 0.00031992016010917723 + ], + [ + -0.10824537411387734, + -1.7634001778787933e-05, + -0.00017634002142585814, + 0.12974874675273895, + 5.541753580473596e-06, + 0.00031323812436312437 + ], + [ + -0.10769947128273318, + -2.2707719836034812e-05, + -0.00022707720927428454, + 0.12775516510009766, + 7.330325843213359e-06, + 0.00030702617368660867 + ], + [ + -0.14305795162332793, + -0.00030156326829455793, + -0.00301563274115324, + 0.6839030981063843, + 4.415587682160549e-05, + -0.22402147948741913 + ], + [ + -0.1359612868675231, + -0.0005608677165582776, + -0.00560867739841342, + 0.6581742167472839, + 7.959602953633294e-05, + -0.22430188953876495 + ], + [ + -0.12936356042287359, + -0.000801991845946759, + -0.008019918575882912, + 0.6342493295669556, + 0.00011374791210982949, + -0.22455888986587524 + ], + [ + -0.1232297452015651, + -0.0010262103751301765, + -0.010262103751301765, + 0.6120017766952515, + 0.00014670185919385403, + -0.2247941493988037 + ], + [ + 0.18898263724481582, + -0.3834262788295746, + 0.16573716700077057, + -0.363513708114624, + -0.0006830576458014548, + -0.3340778648853302 + ], + [ + 0.17043036934689756, + -0.3670603334903717, + 0.3293967843055725, + -0.3358006775379181, + -0.0014495592331513762, + -0.29714497923851013 + ], + [ + 0.15329577144049716, + -0.3518418073654175, + 0.4815819263458252, + -0.31003066897392273, + -0.002157237846404314, + -0.26136893033981323 + ], + [ + 0.13885098210221994, + -0.3376902937889099, + 0.6230970621109009, + -0.2860674262046814, + -0.0028102179057896137, + -0.23191167414188385 + ], + [ + 0.18527992219918452, + -0.3245309591293335, + 0.7546903491020203, + -0.2637842893600464, + -0.0028102179057896137, + -0.42906454205513 + ], + [ + 0.17206466577559282, + -0.31229427456855774, + 0.8770573139190674, + -0.24306349456310272, + -0.0028102179057896137, + -0.4004751741886139 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.18071963917045453, + -0.008999999612569809, + 0.0, + 0.8138636350631714, + 9.495075937593356e-05, + -0.2816694974899292 + ], + [ + -0.18078095032621577, + -0.008999999612569809, + 0.0, + 0.8138636350631714, + 0.00018990151875186712, + -0.28154006600379944 + ], + [ + -0.31736123368584657, + 0.0018796906806528568, + -0.008203093893826008, + 1.3015185594558716, + 0.0003417453553993255, + -0.2694607377052307 + ], + [ + -0.3078279365475176, + 0.0011168947676196694, + -0.015831053256988525, + 1.267328143119812, + 0.0004896002938039601, + -0.27031010389328003 + ] + ], + "helpers": [ + [ + 1.152007017756061, + 0.9895813688219441, + 0.37665466606411585, + 0.9995785592537053, + -0.48882890713177274, + 0.999949749695639 + ], + [ + 0.7143736867598727, + 0.705142017608503, + 0.3198, + 0.9980951598655177, + 0.2, + 0.9989500339924398 + ], + [ + 0.9990463679058011, + 0.9936653590982446, + 0.38946576694327467, + 0.8866936494147638, + -0.1, + 0.7281708549099692 + ] + ] +} diff --git a/openpilot/starpilot/lateral/tests/testdata/ioniq6_frozen_2025.json b/openpilot/starpilot/lateral/tests/testdata/ioniq6_frozen_2025.json new file mode 100644 index 0000000000..aa3e6dddc6 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/testdata/ioniq6_frozen_2025.json @@ -0,0 +1,208 @@ +{ + "profile": "2025", + "factor": 3.6600000858306885, + "rows": [ + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.1000419596173212, + -6.3100724219111726e-06, + -6.310072058113292e-05, + 0.09995962679386139, + 1.399434836457658e-06, + 0.00010129198199138045 + ], + [ + -0.09957267603318784, + -1.2177733879070729e-05, + -0.00012177733879070729, + 0.09824388474225998, + 2.774849235720467e-06, + 9.798955579753965e-05 + ], + [ + -0.09913630335274988, + -1.7634001778787933e-05, + -0.00017634002142585814, + 0.09664839506149292, + 4.127926786168246e-06, + 9.492012031842023e-05 + ], + [ + -0.0987305336073542, + -2.2707719836034812e-05, + -0.00022707720927428454, + 0.0951647162437439, + 5.460232841869583e-06, + 9.206731920130551e-05 + ], + [ + -0.09746098608574076, + -0.00030156326829455793, + -0.00301563274115324, + 0.521321177482605, + 3.353137071826495e-05, + -0.22827710211277008 + ], + [ + -0.09216927405030517, + -0.0005608677165582776, + -0.00560867739841342, + 0.5020400881767273, + 6.0564299928955734e-05, + -0.22839277982711792 + ], + [ + -0.08724454159432636, + -0.000801991845946759, + -0.008019918575882912, + 0.4840916097164154, + 8.663076732773334e-05, + -0.22849681973457336 + ], + [ + -0.08266166023362777, + -0.0010262103751301765, + -0.010262103751301765, + 0.46738457679748535, + 0.0001117976353270933, + -0.22859011590480804 + ], + [ + 0.17274071128924318, + -0.3834262788295746, + 0.16573716700077057, + -0.3635135591030121, + -0.000717961578629911, + -0.30158889293670654 + ], + [ + 0.15695589094557277, + -0.3670603334903717, + 0.3293967843055725, + -0.3358005881309509, + -0.0014844628749415278, + -0.2747289836406708 + ], + [ + 0.14488035756024556, + -0.3518418073654175, + 0.4815819263458252, + -0.31003063917160034, + -0.0021921414881944656, + -0.2578660845756531 + ], + [ + 0.13203018780276538, + -0.3376902937889099, + 0.6230970621109009, + -0.286067396402359, + -0.0028451215475797653, + -0.2338174432516098 + ], + [ + 0.17666741388048487, + -0.3245309591293335, + 0.7546903491020203, + -0.2637842893600464, + -0.0028451215475797653, + -0.4359583258628845 + ], + [ + 0.16482470244305603, + -0.31229427456855774, + 0.8770573139190674, + -0.24306349456310272, + -0.0028451215475797653, + -0.4119570255279541 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.13034704739944014, + -0.008999999612569809, + 0.0, + 0.639947772026062, + 7.466057286364958e-05, + -0.2920238971710205 + ], + [ + -0.1304028061718463, + -0.008999999612569809, + 0.0, + 0.639947772026062, + 0.00014932114572729915, + -0.29189446568489075 + ], + [ + -0.23666653614857244, + 0.0018796906806528568, + -0.008203093893826008, + 1.0229543447494507, + 0.0002686658117454499, + -0.28603285551071167 + ], + [ + -0.22962545662414624, + 0.0011168947676196694, + -0.015831053256988525, + 0.9974353313446045, + 0.0003850332577712834, + -0.2864045202732086 + ] + ], + "helpers": [ + [ + 1.152007017756061, + 0.9895813688219441, + 0.37665466606411585, + 0.9995785592537053, + -0.48882890713177274, + 0.999949749695639 + ], + [ + 0.7143736867598727, + 0.705142017608503, + 0.3198, + 0.9980951598655177, + 0.2, + 0.9989500339924398 + ], + [ + 0.9990463679058011, + 0.9936653590982446, + 0.38946576694327467, + 0.8866936494147638, + -0.1, + 0.7281708549099692 + ] + ] +} diff --git a/openpilot/starpilot/lateral/tests/testdata/non_ioniq_parent.json b/openpilot/starpilot/lateral/tests/testdata/non_ioniq_parent.json new file mode 100644 index 0000000000..8648a7a1ca --- /dev/null +++ b/openpilot/starpilot/lateral/tests/testdata/non_ioniq_parent.json @@ -0,0 +1,198 @@ +{ + "TOYOTA_COROLLA_TSS2": [ + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.1741040538961501, + 0.0, + -0.0125821428373456, + 0.5361478328704834, + 0.0, + -0.18948359787464142 + ], + [ + -0.1743630122313323, + 0.0, + -0.011699984781444073, + 0.5361478328704834, + 0.0, + -0.1889679729938507 + ], + [ + -0.1746038145029732, + 0.0, + -0.010879676789045334, + 0.5361478328704834, + 0.0, + -0.18848849833011627 + ], + [ + 0.21482844409667973, + 0.0, + 0.31239789724349976, + -0.2552758753299713, + -0.0001914569002110511, + -0.1722845435142517 + ], + [ + 0.12667966987594909, + 0.0, + 0.6130098700523376, + -0.2552758753299713, + -0.0003829138004221022, + 0.0034227967262268066 + ], + [ + 0.13939243693119238, + 0.0, + 0.5700305700302124, + -0.2552758753299713, + -0.0005743707297369838, + -0.021698547527194023 + ], + [ + 0.5066447263279833, + 0.0, + 0.4336607754230499, + -0.4848564863204956, + -0.0005743707297369838, + -0.52336585521698 + ], + [ + 0.5066447263279833, + 0.0, + 0.3068521022796631, + -0.4848564863204956, + -0.0005743707297369838, + -0.52336585521698 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "HYUNDAI_IONIQ_5": [ + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.09367755417862601, + 0.0, + -0.0125821428373456, + 0.4897056818008423, + 0.0, + -0.19247344136238098 + ], + [ + -0.09380461008165825, + 0.0, + -0.011699984781444073, + 0.4897056818008423, + 0.0, + -0.19207030534744263 + ], + [ + -0.09392275785262694, + 0.0, + -0.010879676789045334, + 0.4897056818008423, + 0.0, + -0.191695436835289 + ], + [ + 0.11027891320132133, + 0.0, + 0.31239789724349976, + -0.21928606927394867, + -0.00016446455265395343, + -0.13045662641525269 + ], + [ + 0.06703405539612153, + 0.0, + 0.6130098700523376, + -0.21928606927394867, + -0.00032892910530790687, + 0.006920696701854467 + ], + [ + 0.07327613223245147, + 0.0, + 0.5700305700302124, + -0.21928606927394867, + -0.0004933936288580298, + -0.012720505706965923 + ], + [ + 0.29068416058013297, + 0.0, + 0.4336607754230499, + -0.4834653437137604, + -0.0004933936288580298, + -0.4383614659309387 + ], + [ + 0.29068416058013297, + 0.0, + 0.3068521022796631, + -0.4834653437137604, + -0.0004933936288580298, + -0.4383614659309387 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + -0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ] +} diff --git a/openpilot/starpilot/lateral/torque_extension.py b/openpilot/starpilot/lateral/torque_extension.py new file mode 100644 index 0000000000..3737a09d66 --- /dev/null +++ b/openpilot/starpilot/lateral/torque_extension.py @@ -0,0 +1,102 @@ +"""Selected StarPilot torque behavior outside the upstream controller.""" + +from openpilot.starpilot.lateral.controller_selection import ControllerMode + + +class TorqueExtension: + def __init__(self, policy, *, parameter_factor=None): + self.policy = policy + self.parameter_factor = parameter_factor + self.ratio_scale = getattr(policy, 'steer_ratio_scale', None) + + def transform_torque_parameters(self, factor, offset, friction): + if self.parameter_factor is not None: + factor *= self.parameter_factor + return factor, offset, friction + + def vehicle_model_ratio(self, ratio, speed): + return ratio if self.ratio_scale is None else ratio * self.ratio_scale(speed) + + def update(self, active, cs, vm, params, safety_limited, curvature, curvature_limited, delay): + return self.policy.update(active, cs, vm, params, safety_limited, curvature, curvature_limited, delay) + + + +def create_extension(parent, cp, mode, selected, *, turn_assist=False): + if mode != ControllerMode.STARPILOT: + return None + from openpilot.starpilot.lateral.hkg_vehicle_policy import HKGVehicleTorquePolicy, PROFILES, policy_for + if selected in PROFILES: + if policy_for(cp) != selected: + raise ValueError('HKG torque selection does not match CarParams') + policy = HKGVehicleTorquePolicy(parent, cp) + return TorqueExtension(policy, parameter_factor=policy.FACTOR_MULT) + if selected == 'ioniq6': + from openpilot.starpilot.lateral.ioniq6_policy import Ioniq6TorquePolicy + policy = Ioniq6TorquePolicy(parent, cp, turn_assist=turn_assist) + return TorqueExtension(policy, parameter_factor=policy.FACTOR_MULT) + if selected == 'genesis_gv70_electrified': + from openpilot.starpilot.lateral.genesis_gv70_policy import GenesisGV70TorquePolicy, supported_cp + if supported_cp(cp): + return TorqueExtension(GenesisGV70TorquePolicy(parent, cp)) + if selected == 'genesis_g70_2020': + from openpilot.starpilot.lateral.genesis_g70_policy import GenesisG70TorquePolicy, supported_cp + if supported_cp(cp): + return TorqueExtension(GenesisG70TorquePolicy(parent, cp)) + if selected == 'rav4_tss2': + from openpilot.starpilot.lateral.rav4_tss2_policy import Rav4TSS2TorquePolicy, supported_cp + if supported_cp(cp): + return TorqueExtension(Rav4TSS2TorquePolicy(parent, cp)) + if selected == 'corolla_tss2': + from openpilot.starpilot.lateral.corolla_tss2_policy import CorollaTSS2TorquePolicy, supported_cp + if supported_cp(cp): + return TorqueExtension(CorollaTSS2TorquePolicy(parent, cp)) + if selected in ('ordinary_camera', 'silverado_cc'): + from openpilot.starpilot.lateral.camera_policy import CameraTorquePolicy + return TorqueExtension(CameraTorquePolicy(parent, cp)) + if selected == 'suburban': + from openpilot.starpilot.lateral.suburban_policy import SuburbanTorquePolicy + return TorqueExtension(SuburbanTorquePolicy(parent, cp)) + if selected == 'ordinary_cc': + from openpilot.starpilot.lateral.ordinary_cc_policy import OrdinaryCcTorquePolicy + return TorqueExtension(OrdinaryCcTorquePolicy(parent, cp)) + if selected == 'ordinary_sdgm': + from openpilot.starpilot.lateral.sdgm_policy import SdgmTorquePolicy + return TorqueExtension(SdgmTorquePolicy(parent, cp)) + if selected == 'ordinary_ascm': + from openpilot.starpilot.lateral.ascm_policy import AscmTorquePolicy + return TorqueExtension(AscmTorquePolicy(parent, cp)) + if selected == 'sonata_hybrid': + from openpilot.starpilot.lateral.sonata_hybrid_policy import SonataHybridTorquePolicy + policy = SonataHybridTorquePolicy(parent, cp) + return TorqueExtension(policy, parameter_factor=policy.FACTOR_MULT) + if selected == 'palisade': + from openpilot.starpilot.lateral.palisade_policy import PalisadeTorquePolicy + policy = PalisadeTorquePolicy(parent, cp) + return TorqueExtension(policy, parameter_factor=policy.FACTOR_MULT) + if selected == 'volt': + from openpilot.starpilot.lateral.volt_policy import VoltTorquePolicy + return TorqueExtension(VoltTorquePolicy(parent, cp)) + if selected == 'bolt': + from openpilot.starpilot.lateral.bolt_policy import BoltTorquePolicy + return TorqueExtension(BoltTorquePolicy(parent, cp)) + raise ValueError('No StarPilot torque policy for CarParams') + + +def selected_policy(controller): + extension = controller.starpilot_extension + return None if extension is None else extension.policy + + +def create_turn_assist(cp, enabled): + from openpilot.starpilot.lateral.controller_selection import turn_assist_supported + if not enabled or not turn_assist_supported(cp): + return None + from openpilot.starpilot.lateral.torque_turn_assist import TorqueTurnAssist + return TorqueTurnAssist(cp) + + +def apply_turn_assist(parent, cs, vm, params, curvature, output): + if parent.turn_assist is None: + return output + return parent.turn_assist.apply(cs, vm, params, curvature, output, parent.steer_max) diff --git a/openpilot/starpilot/lateral/torque_runtime.py b/openpilot/starpilot/lateral/torque_runtime.py new file mode 100644 index 0000000000..414f614ed1 --- /dev/null +++ b/openpilot/starpilot/lateral/torque_runtime.py @@ -0,0 +1,234 @@ +"""Exact-platform torque source selection for native controlsd.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +import os +from pathlib import Path + +from opendbc.car import structs +from openpilot.cereal.services import SERVICE_LIST +from openpilot.starpilot.lateral.torque_tuning import TorqueSource, TorqueTuning +from openpilot.starpilot.lateral.torque_supported import BOLT_VEHICLES, GM_VEHICLES, IONIQ6_VEHICLES, TOYOTA_VEHICLES +from openpilot.starpilot.lateral.torque_settings import ( + DOCUMENT_KEY, LEGACY_KEYS, MAX_DOCUMENT_BYTES, interpret_legacy, parse_document, resolve_document, +) +LEARNER_VERSION = 1 +REFRESH_NS = 1_000_000_000 + + +def _move(old: float, new: float, step: float) -> float: + return min(max(new, old - step), old + step) + + +GM_TORQUE_POLICIES = frozenset({'bolt', 'volt', 'ordinary_ascm', 'ordinary_sdgm', + 'ordinary_cc', 'ordinary_camera', 'silverado_cc', 'suburban'}) + + + +def gm_manual_supported_cp(CP) -> bool: + """Reuse exact finalized torque policies; no brand-wide or controller-mode admission.""" + if (str(CP.carFingerprint) not in GM_VEHICLES or str(CP.brand) != 'gm' or + CP.notCar or CP.passive or CP.dashcamOnly or + CP.steerControlType != structs.CarParams.SteerControlType.torque or CP.lateralTuning.which() != 'torque'): + return False + from openpilot.starpilot.lateral.controller_selection import policy_for + if str(CP.carFingerprint) not in BOLT_VEHICLES: + from opendbc.car.gm.aol import qualified_gm + if not qualified_gm(CP): + return False + return policy_for(CP) in GM_TORQUE_POLICIES + + + +def supported_cp(CP) -> bool: + fingerprint = str(CP.carFingerprint) + if fingerprint in BOLT_VEHICLES: + from openpilot.starpilot.lateral.bolt_policy import supported_cp as bolt_supported_cp + return bolt_supported_cp(CP) + platform = ((fingerprint in TOYOTA_VEHICLES and str(CP.brand) == 'toyota') or + (fingerprint in IONIQ6_VEHICLES and str(CP.brand) == 'hyundai')) + return (platform and + CP.steerControlType == structs.CarParams.SteerControlType.torque and not CP.dashcamOnly and + CP.lateralTuning.which() == 'torque' and + math.isfinite(float(CP.lateralTuning.torque.latAccelFactor)) and + float(CP.lateralTuning.torque.latAccelFactor) > 0 and + math.isfinite(float(CP.lateralTuning.torque.latAccelOffset)) and + math.isfinite(float(CP.lateralTuning.torque.friction)) and + float(CP.lateralTuning.torque.friction) >= 0) + + +def production_supported_cp(CP) -> bool: + """Exact production consumer scope; replay qualification does not expand UI.""" + return (supported_cp(CP) and str(CP.carFingerprint) in IONIQ6_VEHICLES and + not CP.passive and not CP.notCar) + + +def manual_overrides_present(CP, params) -> bool: + """Numeric intent alone admits the custom host and invalidates stale learning.""" + if params is None or not (production_supported_cp(CP) or os.getenv('TORQUE_REPLAY_RUNTIME') == '1' and supported_cp(CP)): + return False + settings = read_settings(params, TorqueHost(params, CP).vehicle) + return settings.valid and (settings.user_factor is not None or settings.user_friction is not None) + + +def runtime_enabled(CP, params=None) -> bool: + if not supported_cp(CP): + return False + if str(CP.carFingerprint) in BOLT_VEHICLES: + if params is None: + return False + settings = read_settings(params, TorqueHost(params, CP).vehicle) + return settings.advanced and settings.valid and settings.user_friction is not None + if os.getenv('TORQUE_REPLAY_RUNTIME') == '1': + return True + # Preserve native/default startup behavior. A first custom edit is saved for + # the next drive; a custom host already admitted can refresh/reset live. + return manual_overrides_present(CP, params) + + +development_enabled = runtime_enabled + + +@dataclass(frozen=True) +class TorqueSettings: + force_auto_off: bool = False + user_factor: float | None = None + user_friction: float | None = None + valid: bool = True + + +def _raw(params, key: str) -> bytes | None: + try: + with Path(params.get_param_path(key)).open('rb') as source: + return source.read(MAX_DOCUMENT_BYTES + 1) if key == DOCUMENT_KEY else source.read() + except FileNotFoundError: + return None + + +def _bool(raw: bytes | None, default: bool) -> bool: + if raw is None: + return default + if raw not in (b'0', b'1'): + raise ValueError('Malformed torque setting') + return raw == b'1' + + +def read_settings(params, vehicle: TorqueTuning, *, allow_learning: bool | None = None) -> TorqueSettings: + try: + force_off = _bool(_raw(params, 'ForceAutoTuneOff'), False) if allow_learning is None else not allow_learning + basis = (vehicle.lat_accel_factor, vehicle.lat_accel_offset, vehicle.friction) + document = _raw(params, DOCUMENT_KEY) + if vehicle.vehicle in BOLT_VEHICLES and document is None: + return TorqueSettings(valid=False) + if document is None: + legacy = interpret_legacy({key: _raw(params, key) for key in LEGACY_KEYS}, basis) + factor, friction = legacy.factor, legacy.friction + else: + factor, friction, review = resolve_document(parse_document(document), vehicle.vehicle, basis) + if review: + return TorqueSettings(valid=False) + return TorqueSettings(force_off, factor, friction) + except (OSError, ValueError, OverflowError): + return TorqueSettings(valid=False) + + +class TorqueHost: + """Choose one complete source and rate-bound changes to the native controller.""" + def __init__(self, params, CP, *, allow_learning: bool | None = None): + if not supported_cp(CP): + raise ValueError('Unsupported torque host') + tune = CP.lateralTuning.torque + self.vehicle = TorqueTuning(TorqueSource.VEHICLE, str(CP.carFingerprint), + float(tune.latAccelFactor), float(tune.latAccelOffset), float(tune.friction)) + self.params = params + # An exact controller supplies its startup decision. None preserves the + # existing live manual preference for other optional replay-host vehicles. + self.allow_learning = allow_learning + self.settings = TorqueSettings() + self.last_refresh_ns: int | None = None + self.last_tick_ns: int | None = None + self.applied = self.vehicle + self.selected = self.vehicle + self.applied_controller = None + self.last_application = self.vehicle + + def apply(self, controller, tune: TorqueTuning) -> bool: + """Apply a complete source transition, preserving an untouched vehicle tune. + + This host exclusively owns parameter updates on its controller. Reapplying + the initial vehicle values is not a harmless refresh: the Ioniq controller + intentionally retains its original startup PID limits until an actual + learned/custom update. Source identity matters even when numbers are equal. + """ + if not isinstance(tune, TorqueTuning) or tune.vehicle != self.vehicle.vehicle: + raise ValueError('Torque application does not match vehicle') + if controller is not self.applied_controller: + self.applied_controller = controller + self.last_application = self.vehicle + if tune == self.last_application: + return False + controller.update_torque_parameters(*tune.upstream_update()) + self.last_application = tune + return True + + def _learned(self, sm, now_ns: int) -> TorqueTuning | None: + name = 'lateralTorqueParameters' + stamp = int(sm.logMonoTime[name]) + if not sm.all_checks([name]) or stamp <= 0 or not 0 <= now_ns - stamp <= int(2e9 / SERVICE_LIST[name].frequency): + return None + state = sm[name] + if not state.useParams or not state.valid or int(state.version) != LEARNER_VERSION: + return None + factor = float(state.latAccelFactorFiltered) + offset = float(state.latAccelOffsetFiltered) + friction = float(state.frictionCoefficientFiltered) + vehicle = self.vehicle + if (not all(math.isfinite(v) for v in (factor, offset, friction)) or + not 0.7 * vehicle.lat_accel_factor <= factor <= 1.3 * vehicle.lat_accel_factor or + abs(offset) > min(0.5, 0.5 * vehicle.lat_accel_factor) or + not 0.5 * vehicle.friction <= friction <= max(0.1, 1.5 * vehicle.friction)): + return None + return TorqueTuning(TorqueSource.LEARNED, vehicle.vehicle, factor, offset, friction) + + def _select(self, sm, now_ns: int) -> TorqueTuning: + vehicle = self.vehicle + settings = self.settings + if not settings.valid: + return vehicle + allow_learning = not settings.force_auto_off if self.allow_learning is None else self.allow_learning + learned = self._learned(sm, now_ns) if allow_learning else None + base = learned or vehicle + if settings.user_factor is None and settings.user_friction is None: + return base + return TorqueTuning(TorqueSource.USER, vehicle.vehicle, + settings.user_factor if settings.user_factor is not None else base.lat_accel_factor, + vehicle.lat_accel_offset if settings.user_factor is not None else base.lat_accel_offset, + settings.user_friction if settings.user_friction is not None else base.friction) + + def sample(self, sm, *, now_ns: int, lat_active: bool) -> TorqueTuning: + if now_ns <= 0 or (self.last_tick_ns is not None and not 0 < now_ns - self.last_tick_ns <= 50_000_000): + self.applied = self.vehicle + self.last_refresh_ns = None + self.last_tick_ns = now_ns + self.selected = self.vehicle + return self.vehicle + dt = 0.01 if self.last_tick_ns is None else (now_ns - self.last_tick_ns) / 1e9 + self.last_tick_ns = now_ns + if self.last_refresh_ns is None or now_ns - self.last_refresh_ns >= REFRESH_NS: + self.settings = read_settings(self.params, self.vehicle, allow_learning=self.allow_learning) + self.last_refresh_ns = now_ns + if not lat_active or not self.settings.valid: + self.applied = self.selected = self.vehicle + return self.vehicle + self.selected = self._select(sm, now_ns) + prior, target = self.applied, self.selected + factor_step = 0.5 * self.vehicle.lat_accel_factor * dt + offset_step = 0.1 * dt + friction_step = max(0.05, self.vehicle.friction) * dt + self.applied = TorqueTuning(target.source, target.vehicle, + _move(prior.lat_accel_factor, target.lat_accel_factor, factor_step), + _move(prior.lat_accel_offset, target.lat_accel_offset, offset_step), + _move(prior.friction, target.friction, friction_step)) + return self.applied diff --git a/openpilot/starpilot/lateral/torque_settings.py b/openpilot/starpilot/lateral/torque_settings.py new file mode 100644 index 0000000000..673719915f --- /dev/null +++ b/openpilot/starpilot/lateral/torque_settings.py @@ -0,0 +1,285 @@ +"""Strict saved Torque policy shared by the optional host and native settings UI.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +from enum import StrEnum +import json +import math +from typing import cast + +from openpilot.starpilot.lateral.torque_supported import BOLT_VEHICLES, SUPPORTED_VEHICLES + +DOCUMENT_KEY = "TorqueOverrideDocument" +LEGACY_KEYS = ("SteerLatAccel", "SteerFriction", "SteerLatAccelStock", "SteerFrictionStock") +MAX_DOCUMENT_BYTES = 16_384 + + +@dataclass(frozen=True) +class FieldChoice: + mode: str = "source" + custom_value: float | None = None + + +@dataclass(frozen=True) +class GainBasis: + controller: str + source_table: tuple[tuple[float, ...], tuple[float, ...]] + torque_basis: tuple[float, float, float] + + +@dataclass(frozen=True) +class GeometryProfile: + basis: tuple[float, float] + ratio: FieldChoice = FieldChoice() + full_delay: FieldChoice = FieldChoice() + automatic_delay: bool = True + learning: str = "source" + + + +@dataclass(frozen=True) +class PlatformProfile: + basis: tuple[float, float, float] + factor: FieldChoice + friction: FieldChoice + proportional_gain: FieldChoice = FieldChoice() + gain_basis: GainBasis | None = None + + +class LegacyMode(StrEnum): + ABSENT = "absent" + STOCK = "stock" + CUSTOM = "custom" + + +@dataclass(frozen=True) +class LegacyChoice: + factor: float | None + friction: float | None + factor_mode: LegacyMode + friction_mode: LegacyMode + factor_saved: float | None + friction_saved: float | None + + +def bounds(basis: tuple[float, float, float], field: str) -> tuple[float, float]: + factor, _, friction = basis + if field == "factor": + return 0.5 * factor, 1.5 * factor + if field == "friction": + return 0.0, min(1.0, max(0.1, 2.0 * friction)) + raise ValueError("Unknown torque field") + + +def _finite_number(value: object) -> bool: + if type(value) not in (float, int): + return False + try: + return math.isfinite(cast(float | int, value)) + except OverflowError: + return False + + +def valid_basis(basis: tuple[float, float, float]) -> bool: + return (len(basis) == 3 and all(_finite_number(v) for v in basis) and + basis[0] > 0 and basis[2] >= 0) + + +def _number(raw: bytes | None) -> float | None: + if raw is None: + return None + value = float(raw) + if not math.isfinite(value): + raise ValueError("Invalid torque number") + return value + + +def interpret_legacy(raw: dict[str, bytes | None], basis: tuple[float, float, float]) -> LegacyChoice: + """Preserve the old stock-marker and two-decimal CP equality semantics.""" + if not valid_basis(basis): + raise ValueError("Invalid torque basis") + values = {key: _number(raw.get(key)) for key in LEGACY_KEYS} + result: list[float | None] = [] + modes: list[LegacyMode] = [] + saved: list[float | None] = [] + for key, marker_key, field, stock in ((LEGACY_KEYS[0], LEGACY_KEYS[2], "factor", basis[0]), + (LEGACY_KEYS[1], LEGACY_KEYS[3], "friction", basis[2])): + value, marker = values[key], values[marker_key] + saved.append(value) + if value is None: + result.append(None) + modes.append(LegacyMode.ABSENT) + elif round(value, 2) == round(stock, 2) or marker is not None and math.isclose(value, marker, abs_tol=1e-6): + result.append(None) + modes.append(LegacyMode.STOCK) + else: + low, high = bounds(basis, field) + if not low <= value <= high: + raise ValueError("Out-of-range torque setting") + result.append(value) + modes.append(LegacyMode.CUSTOM) + return LegacyChoice(result[0], result[1], modes[0], modes[1], saved[0], saved[1]) + + +def empty_document() -> dict: + return {"schemaVersion": 1, "vehicles": {}} + + +def _pairs(items: list[tuple[str, object]]) -> dict: + result = {} + for key, value in items: + if key in result: + raise ValueError("Duplicate torque document member") + result[key] = value + return result + + +def _bad_constant(_value: str): + raise ValueError("Nonfinite torque document number") + + +def _choice(value: object) -> FieldChoice: + if not isinstance(value, dict) or set(value) != {"mode", "customValue"}: + raise ValueError("Malformed torque field") + mode, custom = value["mode"], value["customValue"] + if mode not in ("source", "custom") or (custom is not None and not _finite_number(custom)) or \ + (mode == "custom" and custom is None): + raise ValueError("Malformed torque field") + return FieldChoice(mode, custom) + + +def parse_document(raw: bytes) -> dict[str, PlatformProfile]: + if len(raw) > MAX_DOCUMENT_BYTES: + raise ValueError("Oversized torque document") + try: + document = json.loads(raw.decode("utf-8"), object_pairs_hook=_pairs, parse_constant=_bad_constant) + except (RecursionError, OverflowError) as exc: + raise ValueError("Malformed torque document") from exc + if not isinstance(document, dict) or set(document) != {"schemaVersion", "vehicles"} or \ + type(document["schemaVersion"]) is not int or document["schemaVersion"] not in (1, 2) or \ + not isinstance(document["vehicles"], dict) or len(document["vehicles"]) > len(SUPPORTED_VEHICLES): + raise ValueError("Malformed torque document") + profiles: dict[str, PlatformProfile] = {} + for fingerprint, value in document["vehicles"].items(): + expected = {"basis", "factor", "friction"} + if document["schemaVersion"] == 2 and isinstance(value, dict) and "proportionalGain" in value: + expected |= {"proportionalGain", "gainBasis"} + if fingerprint not in SUPPORTED_VEHICLES or not isinstance(value, dict) or set(value) != expected: + raise ValueError("Malformed torque profile") + basis_obj = value["basis"] + if not isinstance(basis_obj, dict) or set(basis_obj) != {"latAccelFactor", "latAccelOffset", "friction"}: + raise ValueError("Malformed torque basis") + basis = tuple(basis_obj[key] for key in ("latAccelFactor", "latAccelOffset", "friction")) + if not valid_basis(basis): + raise ValueError("Malformed torque basis") + gain = FieldChoice() + gain_basis = None + if "proportionalGain" in value: + if fingerprint not in BOLT_VEHICLES: + raise ValueError("Unsupported gain profile") + gain = _choice(value["proportionalGain"]) + gain_basis = parse_gain_basis(value["gainBasis"]) + if gain.mode == "custom" and (gain.custom_value is None or not 0.3 <= gain.custom_value <= 0.9): + raise ValueError("Out-of-range proportional gain") + profiles[fingerprint] = PlatformProfile(basis, _choice(value["factor"]), _choice(value["friction"]), gain, gain_basis) + return profiles + + +def serialize_document(profiles: dict[str, PlatformProfile]) -> bytes: + vehicles = {} + version = 2 if any(p.gain_basis is not None for p in profiles.values()) else 1 + for fingerprint, profile in profiles.items(): + vehicles[fingerprint] = { + "basis": dict(zip(("latAccelFactor", "latAccelOffset", "friction"), profile.basis, strict=True)), + "factor": {"mode": profile.factor.mode, "customValue": profile.factor.custom_value}, + "friction": {"mode": profile.friction.mode, "customValue": profile.friction.custom_value}, + } + if profile.gain_basis is None and profile.proportional_gain != FieldChoice(): + raise ValueError("Gain choice requires a bound basis") + if profile.gain_basis is not None: + vehicles[fingerprint]["proportionalGain"] = {"mode": profile.proportional_gain.mode, "customValue": profile.proportional_gain.custom_value} + vehicles[fingerprint]["gainBasis"] = {"controller": profile.gain_basis.controller, + "sourceTable": profile.gain_basis.source_table, + "torqueBasis": profile.gain_basis.torque_basis} + raw = json.dumps({"schemaVersion": version, "vehicles": vehicles}, sort_keys=True, separators=(",", ":"), allow_nan=False).encode() + parse_document(raw) + return raw + + +def resolve_document(profiles: dict[str, PlatformProfile], fingerprint: str, + basis: tuple[float, float, float]) -> tuple[float | None, float | None, bool]: + profile = profiles.get(fingerprint) + if profile is None: + return None, None, False + if (profile.factor.mode == "custom" or profile.friction.mode == "custom") and profile.basis != basis: + return None, None, True + if fingerprint in BOLT_VEHICLES and profile.factor.mode == "custom": + return None, None, True + result = [] + for field, choice in (("factor", profile.factor), ("friction", profile.friction)): + if choice.mode == "custom": + low, high = bounds(basis, field) + if choice.custom_value is None or not low <= choice.custom_value <= high: + raise ValueError("Out-of-range torque profile") + result.append(None if round(choice.custom_value, 2) == round(basis[0 if field == "factor" else 2], 2) else choice.custom_value) + else: + result.append(None) + return result[0], result[1], False + + +def replace_field(profiles: dict[str, PlatformProfile], fingerprint: str, basis: tuple[float, float, float], + field: str, mode: str, value: float | None) -> dict[str, PlatformProfile]: + if fingerprint in BOLT_VEHICLES and field == "factor": + raise ValueError("Bolt factor editing is not supported") + if mode not in ("source", "custom") or field not in ("factor", "friction") or not valid_basis(basis): + raise ValueError("Invalid torque edit") + if value is not None and not _finite_number(value): + raise ValueError("Invalid torque edit") + if mode == "custom": + low, high = bounds(basis, field) + if value is None or not low <= value <= high: + raise ValueError("Out-of-range torque edit") + profile = profiles.get(fingerprint, PlatformProfile(basis, FieldChoice(), FieldChoice())) + if profile.basis != basis and (profile.factor.mode == "custom" or profile.friction.mode == "custom"): + raise ValueError("Torque basis needs review") + current = profile.factor if field == "factor" else profile.friction + choice = FieldChoice(mode, current.custom_value if mode == "source" else value) + updated = replace(profile, basis=basis, factor=choice if field == "factor" else profile.factor, + friction=choice if field == "friction" else profile.friction) + result = dict(profiles) + result[fingerprint] = updated + return result + + +def parse_gain_basis(value: object) -> GainBasis: + if not isinstance(value, dict) or set(value) != {"controller", "sourceTable", "torqueBasis"}: + raise ValueError("Malformed gain basis") + table, basis = value["sourceTable"], value["torqueBasis"] + if (value["controller"] not in ("standard", "starpilot") or + not isinstance(table, (list, tuple)) or len(table) != 2 or + any(not isinstance(row, (list, tuple)) or not row for row in table) or + len(table[0]) != len(table[1]) or len(table[0]) > 32 or + any(not _finite_number(n) for row in table for n in row) or + any(a >= b for a, b in zip(table[0], table[0][1:], strict=False)) or + any(n <= 0 for n in table[1]) or + not isinstance(basis, (list, tuple)) or not valid_basis(tuple(basis))): + raise ValueError("Malformed gain basis") + return GainBasis(value["controller"], tuple(tuple(row) for row in table), tuple(basis)) + + +def replace_gain(profiles: dict[str, PlatformProfile], fingerprint: str, basis: GainBasis, + mode: str, value: float | None, *, review: bool = False) -> dict[str, PlatformProfile]: + if fingerprint not in BOLT_VEHICLES or mode not in ("source", "custom"): + raise ValueError("Unsupported gain edit") + if value is not None and (not _finite_number(value) or not 0.3 <= value <= 0.9): + raise ValueError("Out-of-range proportional gain") + if mode == "custom" and value is None: + raise ValueError("Missing proportional gain") + prior = profiles.get(fingerprint, PlatformProfile(basis.torque_basis, FieldChoice(), FieldChoice())) + if mode == "custom" and prior.proportional_gain.mode == "custom" and prior.gain_basis != basis and not review: + raise ValueError("Gain basis needs review") + result = dict(profiles) + result[fingerprint] = replace(prior, proportional_gain=FieldChoice(mode, value if mode == "custom" else prior.proportional_gain.custom_value), + gain_basis=basis) + return result diff --git a/openpilot/starpilot/lateral/torque_shaping.py b/openpilot/starpilot/lateral/torque_shaping.py new file mode 100644 index 0000000000..4c67b8e195 --- /dev/null +++ b/openpilot/starpilot/lateral/torque_shaping.py @@ -0,0 +1,50 @@ +"""Shared default torque-shaping calibration and pure math. + +Vehicle admission, calibration overrides, state and output remain in each policy. +Defaults derive from the frozen StarPilot torque controller tune. +""" + +import math + +import numpy as np + + +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) + + +KP = 0.6 +KI = 0.35 +INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30] +KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP] +LOW_SPEED_X = [0, 10, 20, 30] +LOW_SPEED_Y = [12, 10.5, 8, 5] +MAX_LAT_JERK_UP = 2.5 +LP_FILTER_CUTOFF_HZ = 1.2 +JERK_GAIN = 0.22 +LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 +MIN_SPEED = 1.0 +UNWIND_D_DES_THRESHOLD = -1.0 +UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3 +FF_ROLL_OFFSET_FADE_BP = [0.5, 2.5] +FF_ROLL_OFFSET_FADE_V = [0.0, 1.0] +CENTER_CHATTER_JERK_DEADZONE_SPEED_BP = [0.0, 5.0, 12.0, 25.0] +CENTER_CHATTER_JERK_DEADZONE_SPEED_V = [0.08, 0.12, 0.18, 0.18] +CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_BP = [0.0, 0.18, 0.35] +CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_V = [1.0, 1.0, 0.0] + + +def center_chatter_friction_jerk_deadzone(v_ego: float, setpoint: float, vehicle_deadzone: float) -> float: + speed_deadzone = np.interp(max(v_ego, 0.0), CENTER_CHATTER_JERK_DEADZONE_SPEED_BP, + CENTER_CHATTER_JERK_DEADZONE_SPEED_V) + center_weight = np.interp(abs(setpoint), CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_BP, + CENTER_CHATTER_JERK_DEADZONE_LAT_ACCEL_V) + return max(vehicle_deadzone, float(speed_deadzone * center_weight)) + + +def get_standard_friction_threshold(v_ego): + return 0.30 diff --git a/openpilot/starpilot/lateral/torque_supported.py b/openpilot/starpilot/lateral/torque_supported.py new file mode 100644 index 0000000000..111da8f4c0 --- /dev/null +++ b/openpilot/starpilot/lateral/torque_supported.py @@ -0,0 +1,63 @@ +"""Exact development-only Torque platform cohorts.""" + +TOYOTA_VEHICLES = frozenset({ + 'TOYOTA_ALPHARD_TSS2', 'TOYOTA_AVALON', 'TOYOTA_AVALON_2019', 'TOYOTA_AVALON_TSS2', + 'TOYOTA_CAMRY', 'TOYOTA_CAMRY_TSS2', 'TOYOTA_CHR', 'TOYOTA_CHR_TSS2', + 'TOYOTA_COROLLA', 'TOYOTA_COROLLA_TSS2', 'TOYOTA_HIGHLANDER', 'TOYOTA_HIGHLANDER_TSS2', + 'TOYOTA_PRIUS', 'TOYOTA_PRIUS_V', 'TOYOTA_PRIUS_TSS2', 'TOYOTA_RAV4', 'TOYOTA_RAV4H', + 'TOYOTA_RAV4_TSS2', 'TOYOTA_RAV4_TSS2_2022', 'TOYOTA_RAV4_PRIME', 'TOYOTA_YARIS', + 'TOYOTA_MIRAI', 'TOYOTA_SIENNA', 'TOYOTA_SIENNA_4TH_GEN', 'LEXUS_CTH', 'LEXUS_ES', + 'LEXUS_ES_TSS2', 'LEXUS_IS', 'LEXUS_IS_TSS2', 'LEXUS_NX', 'LEXUS_NX_TSS2', + 'LEXUS_LC_TSS2', 'LEXUS_RC', 'LEXUS_RC_TSS2', 'LEXUS_RX', 'LEXUS_RX_TSS2', + 'LEXUS_GS_F', 'LEXUS_LS', +}) + +IONIQ6_VEHICLES = frozenset({'HYUNDAI_IONIQ_6'}) +BOLT_VEHICLES = frozenset({ + 'CHEVROLET_BOLT_CC_2017', 'CHEVROLET_BOLT_CC_2018_2021', 'CHEVROLET_BOLT_CC_2022_2023', + 'CHEVROLET_BOLT_ACC_2022_2023', 'CHEVROLET_BOLT_ACC_2022_2023_PEDAL', +}) +GM_VEHICLES = frozenset({ + 'BUICK_BABYENCLAVE', + 'BUICK_LACROSSE_ASCM', + 'BUICK_LACROSSE_ASCM_19US', + 'CADILLAC_CT6_CC', + 'CADILLAC_ESCALADE_ASCM', + 'CADILLAC_ESCALADE_ESV_2019_ASCM', + 'CADILLAC_XT4', + 'CADILLAC_XT4_CC', + 'CADILLAC_XT5', + 'CADILLAC_XT5_CC', + 'CADILLAC_XT6', + 'CHEVROLET_BLAZER', + 'CHEVROLET_BOLT_ACC_2022_2023', + 'CHEVROLET_BOLT_ACC_2022_2023_PEDAL', + 'CHEVROLET_BOLT_CC_2017', + 'CHEVROLET_BOLT_CC_2018_2021', + 'CHEVROLET_BOLT_CC_2022_2023', + 'CHEVROLET_EQUINOX', + 'CHEVROLET_EQUINOX_CC', + 'CHEVROLET_MALIBU_ASCM', + 'CHEVROLET_MALIBU_CC', + 'CHEVROLET_MALIBU_HYBRID_CC', + 'CHEVROLET_MALIBU_SDGM', + 'CHEVROLET_SILVERADO', + 'CHEVROLET_SILVERADO_CC', + 'CHEVROLET_SUBURBAN', + 'CHEVROLET_SUBURBAN_ASCM', + 'CHEVROLET_SUBURBAN_CAMERA', + 'CHEVROLET_SUBURBAN_CC', + 'CHEVROLET_TRAILBLAZER', + 'CHEVROLET_TRAILBLAZER_CC', + 'CHEVROLET_TRAVERSE', + 'CHEVROLET_TRAX', + 'CHEVROLET_VOLT', + 'CHEVROLET_VOLT_2019', + 'CHEVROLET_VOLT_ASCM', + 'CHEVROLET_VOLT_CAMERA', + 'CHEVROLET_VOLT_CC', + 'GMC_YUKON', + 'GMC_YUKON_CC', +}) +GAIN_VEHICLES = GM_VEHICLES +SUPPORTED_VEHICLES = TOYOTA_VEHICLES | IONIQ6_VEHICLES | GM_VEHICLES diff --git a/openpilot/starpilot/lateral/torque_tuning.py b/openpilot/starpilot/lateral/torque_tuning.py new file mode 100644 index 0000000000..89a339b01e --- /dev/null +++ b/openpilot/starpilot/lateral/torque_tuning.py @@ -0,0 +1,82 @@ +"""Validated torque values for the optional Corolla upstream controller update. + +Source selection lives in torque_runtime; this record does not infer a merge +order or interpret legacy gain fields. +""" + +from dataclasses import dataclass +from enum import StrEnum +import math +from collections.abc import Mapping + + +class TorqueSource(StrEnum): + VEHICLE = "vehicle" + USER = "user" + LEARNED = "learned" + + +_RECORD_FIELDS = frozenset({"source", "vehicle", "latAccelFactor", "latAccelOffset", "friction"}) + + +def _finite_number(value: object, field: str) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise ValueError(f"{field} must be a finite number") + try: + result = float(value) + except OverflowError as exc: + raise ValueError(f"{field} must be a finite number") from exc + if not math.isfinite(result): + raise ValueError(f"{field} must be a finite number") + return result + + +@dataclass(frozen=True) +class TorqueTuning: + """One complete source's values in the upstream lateral acceleration domain.""" + + source: TorqueSource + vehicle: str + lat_accel_factor: float + lat_accel_offset: float + friction: float + + def __post_init__(self) -> None: + if not isinstance(self.source, TorqueSource): + raise ValueError("source must be a TorqueSource") + if not isinstance(self.vehicle, str) or not self.vehicle.strip(): + raise ValueError("vehicle must identify the selected platform") + factor = _finite_number(self.lat_accel_factor, "latAccelFactor") + offset = _finite_number(self.lat_accel_offset, "latAccelOffset") + friction = _finite_number(self.friction, "friction") + if factor <= 0.0: + raise ValueError("latAccelFactor must be positive") + if friction < 0.0: + raise ValueError("friction must be nonnegative") + object.__setattr__(self, "lat_accel_factor", factor) + object.__setattr__(self, "lat_accel_offset", offset) + object.__setattr__(self, "friction", friction) + + def upstream_update(self) -> tuple[float, float, float]: + """Arguments for LatControlTorque.update_torque_parameters, in its API order.""" + return self.lat_accel_factor, self.lat_accel_offset, self.friction + + +def from_record(record: Mapping[str, object]) -> TorqueTuning: + """Parse only the named fields; serialized legacy gains are deliberately rejected.""" + if not isinstance(record, Mapping) or set(record) != _RECORD_FIELDS: + raise ValueError(f"torque record fields must be exactly {sorted(_RECORD_FIELDS)}") + raw_source = record["source"] + if not isinstance(raw_source, str): + raise ValueError("unknown torque source") + try: + source = TorqueSource(raw_source) + except (TypeError, ValueError) as exc: + raise ValueError("unknown torque source") from exc + vehicle = record["vehicle"] + if not isinstance(vehicle, str): + raise ValueError("vehicle must identify the selected platform") + return TorqueTuning(source, vehicle, + _finite_number(record["latAccelFactor"], "latAccelFactor"), + _finite_number(record["latAccelOffset"], "latAccelOffset"), + _finite_number(record["friction"], "friction")) diff --git a/openpilot/starpilot/lateral/torque_turn_assist.py b/openpilot/starpilot/lateral/torque_turn_assist.py new file mode 100644 index 0000000000..3da6651ba6 --- /dev/null +++ b/openpilot/starpilot/lateral/torque_turn_assist.py @@ -0,0 +1,21 @@ +import math + +from opendbc.car import structs +from openpilot.starpilot.lateral.ioniq6_policy import get_ioniq_6_low_speed_angle_assist_torque + + +class TorqueTurnAssist: + def __init__(self, cp): + minimum = float(cp.minSteerSpeed) + self.minimum_speed = max(0.044704, minimum) if math.isfinite(minimum) else math.inf + + def apply(self, cs, vm, params, curvature, output, limit): + if (not math.isfinite(cs.vEgo) or cs.vEgo < self.minimum_speed or cs.standstill or cs.steeringPressed or + cs.steerFaultTemporary or cs.steerFaultPermanent or cs.gearShifter not in (structs.CarState.GearShifter.drive, structs.CarState.GearShifter.low)): + return output + desired = math.degrees(vm.get_steer_from_curvature(-curvature, cs.vEgo, params.roll)) + actual = cs.steeringAngleDeg - params.angleOffsetDeg + if not all(math.isfinite(value) for value in (desired, actual, output, limit)) or limit < 0: + return output + adjusted = get_ioniq_6_low_speed_angle_assist_torque(desired, actual, output, cs.vEgo) + return max(-limit, min(limit, adjusted)) diff --git a/openpilot/starpilot/lateral/volt_policy.py b/openpilot/starpilot/lateral/volt_policy.py new file mode 100644 index 0000000000..12261ad15b --- /dev/null +++ b/openpilot/starpilot/lateral/volt_policy.py @@ -0,0 +1,53 @@ +"""Vehicle-owned default torque controller for the five Volt identities.""" + +import math + +import numpy as np + +from opendbc.car import structs +from opendbc.car.gm.values import CAR +from openpilot.starpilot.lateral.gm_ordinary_policy import GMOrdinaryTorquePolicy + +VOLT_IDENTITIES = { + CAR.CHEVROLET_VOLT, + CAR.CHEVROLET_VOLT_ASCM, + CAR.CHEVROLET_VOLT_CAMERA, + CAR.CHEVROLET_VOLT_CC, + CAR.CHEVROLET_VOLT_2019, +} + + +def supported_cp(cp): + return ( + cp.brand == 'gm' + and cp.carFingerprint in VOLT_IDENTITIES + and not cp.notCar + and not cp.passive + and not cp.dashcamOnly + and cp.steerControlType == structs.CarParams.SteerControlType.torque + and cp.lateralTuning.which() == 'torque' + and math.isfinite(cp.lateralTuning.torque.latAccelFactor) + and cp.lateralTuning.torque.latAccelFactor > 0 + and math.isfinite(cp.lateralTuning.torque.latAccelOffset) + and math.isfinite(cp.lateralTuning.torque.friction) + and cp.lateralTuning.torque.friction >= 0 + ) + + +class VoltTorquePolicy(GMOrdinaryTorquePolicy): + def __init__(self, parent, cp): + self.parent = parent + if not supported_cp(cp): + raise ValueError('Unsupported Volt torque profile') + self.dt = parent.dt + coefficients = ((1.525, 1.05, 0.155, 0.0), (1.525, 0.95, 0.150, 0.0)) + lateral_values = np.arange(-5.0, 5.0, 0.01) + torque_values = [] + for value in lateral_values: + a, b, c, d = coefficients[0 if value >= 0 else 1] + sig_input = a * value + sigmoid = np.sign(sig_input) * (1 / (1 + math.exp(-abs(sig_input))) - 0.5) + torque_values.append(float(sigmoid * b + value * c + d)) + parent.torque_from_lateral_accel = lambda value, tune: np.interp(value, lateral_values, torque_values) + parent.lateral_accel_from_torque = lambda value, tune: np.interp(value, torque_values, lateral_values) + super().__init__(parent, cp)