From 6723106bf5feeb52181e59055cfcedeba84e9a78 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 26 Feb 2025 19:35:43 -0600 Subject: [PATCH] Move vehicle_model.py to opendbc (#34681) * move * fix * move test too * bump * better * bump to master --- opendbc_repo | 2 +- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 2 +- .../controls/lib/tests/test_latcontrol.py | 2 +- .../controls/lib/tests/test_vehicle_model.py | 67 ----- selfdrive/controls/lib/vehicle_model.py | 230 ------------------ selfdrive/locationd/models/car_kf.py | 2 +- selfdrive/locationd/torqued.py | 2 +- tools/joystick/joystickd.py | 2 +- 9 files changed, 7 insertions(+), 304 deletions(-) delete mode 100644 selfdrive/controls/lib/tests/test_vehicle_model.py delete mode 100755 selfdrive/controls/lib/vehicle_model.py diff --git a/opendbc_repo b/opendbc_repo index 27a50f817a..706567945a 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 27a50f817a3030aa6ee182110be284d4d136836a +Subproject commit 706567945a80e0baff18592585d4c1a29e1feea1 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c215a42a6d..c45a3e92bd 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -10,13 +10,13 @@ from openpilot.common.realtime import config_realtime_process, Priority, Ratekee from openpilot.common.swaglog import cloudlog from opendbc.car.car_helpers import get_car_interface +from opendbc.car.vehicle_model import VehicleModel from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl -from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d66e5be28d..f677b7f897 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -3,9 +3,9 @@ import numpy as np from cereal import log from opendbc.car.interfaces import LatControlInputs +from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.pid import PIDController -from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index ba4bd0faec..c7038dd581 100644 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -5,10 +5,10 @@ from opendbc.car.car_helpers import interfaces from opendbc.car.honda.values import CAR as HONDA from opendbc.car.toyota.values import CAR as TOYOTA from opendbc.car.nissan.values import CAR as NISSAN +from opendbc.car.vehicle_model import VehicleModel from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle -from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel from openpilot.selfdrive.locationd.helpers import Pose from openpilot.common.mock.generators import generate_livePose diff --git a/selfdrive/controls/lib/tests/test_vehicle_model.py b/selfdrive/controls/lib/tests/test_vehicle_model.py deleted file mode 100644 index d15519a866..0000000000 --- a/selfdrive/controls/lib/tests/test_vehicle_model.py +++ /dev/null @@ -1,67 +0,0 @@ -import pytest -import math - -import numpy as np - -from opendbc.car.honda.interface import CarInterface -from opendbc.car.honda.values import CAR -from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel, dyn_ss_sol, create_dyn_state_matrices - - -class TestVehicleModel: - def setup_method(self): - CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - self.VM = VehicleModel(CP) - - def test_round_trip_yaw_rate(self): - # TODO: fix VM to work at zero speed - for u in np.linspace(1, 30, num=10): - for roll in np.linspace(math.radians(-20), math.radians(20), num=11): - for sa in np.linspace(math.radians(-20), math.radians(20), num=11): - yr = self.VM.yaw_rate(sa, u, roll) - new_sa = self.VM.get_steer_from_yaw_rate(yr, u, roll) - - assert sa == pytest.approx(new_sa) - - def test_dyn_ss_sol_against_yaw_rate(self): - """Verify that the yaw_rate helper function matches the results - from the state space model.""" - - for roll in np.linspace(math.radians(-20), math.radians(20), num=11): - for u in np.linspace(1, 30, num=10): - for sa in np.linspace(math.radians(-20), math.radians(20), num=11): - - # Compute yaw rate based on state space model - _, yr1 = dyn_ss_sol(sa, u, roll, self.VM) - - # Compute yaw rate using direct computations - yr2 = self.VM.yaw_rate(sa, u, roll) - assert float(yr1[0]) == pytest.approx(yr2) - - def test_syn_ss_sol_simulate(self): - """Verifies that dyn_ss_sol matches a simulation""" - - for roll in np.linspace(math.radians(-20), math.radians(20), num=11): - for u in np.linspace(1, 30, num=10): - A, B = create_dyn_state_matrices(u, self.VM) - - # Convert to discrete time system - dt = 0.01 - top = np.hstack((A, B)) - full = np.vstack((top, np.zeros_like(top))) * dt - Md = sum([np.linalg.matrix_power(full, k) / math.factorial(k) for k in range(25)]) - Ad = Md[:A.shape[0], :A.shape[1]] - Bd = Md[:A.shape[0], A.shape[1]:] - - for sa in np.linspace(math.radians(-20), math.radians(20), num=11): - inp = np.array([[sa], [roll]]) - - # Simulate for 1 second - x1 = np.zeros((2, 1)) - for _ in range(100): - x1 = Ad @ x1 + Bd @ inp - - # Compute steady state solution directly - x2 = dyn_ss_sol(sa, u, roll, self.VM) - - np.testing.assert_almost_equal(x1, x2, decimal=3) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py deleted file mode 100755 index b6f50b4ba8..0000000000 --- a/selfdrive/controls/lib/vehicle_model.py +++ /dev/null @@ -1,230 +0,0 @@ -#!/usr/bin/env python3 -""" -Dynamic bicycle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani" - -The state is x = [v, r]^T -with v lateral speed [m/s], and r rotational speed [rad/s] - -The input u is the steering angle [rad], and roll [rad] - -The system is defined by -x_dot = A*x + B*u - -A depends on longitudinal speed, u [m/s], and vehicle parameters CP -""" - -import numpy as np -from numpy.linalg import solve - -from cereal import car - -ACCELERATION_DUE_TO_GRAVITY = 9.8 - - -class VehicleModel: - def __init__(self, CP: car.CarParams): - """ - Args: - CP: Car Parameters - """ - # for math readability, convert long names car params into short names - self.m: float = CP.mass - self.j: float = CP.rotationalInertia - self.l: float = CP.wheelbase - self.aF: float = CP.centerToFront - self.aR: float = CP.wheelbase - CP.centerToFront - self.chi: float = CP.steerRatioRear - - self.cF_orig: float = CP.tireStiffnessFront - self.cR_orig: float = CP.tireStiffnessRear - self.update_params(1.0, CP.steerRatio) - - def update_params(self, stiffness_factor: float, steer_ratio: float) -> None: - """Update the vehicle model with a new stiffness factor and steer ratio""" - self.cF: float = stiffness_factor * self.cF_orig - self.cR: float = stiffness_factor * self.cR_orig - self.sR: float = steer_ratio - - def steady_state_sol(self, sa: float, u: float, roll: float) -> np.ndarray: - """Returns the steady state solution. - - If the speed is too low we can't use the dynamic model (tire slip is undefined), - we then have to use the kinematic model - - Args: - sa: Steering wheel angle [rad] - u: Speed [m/s] - roll: Road Roll [rad] - - Returns: - 2x1 matrix with steady state solution (lateral speed, rotational speed) - """ - if u > 0.1: - return dyn_ss_sol(sa, u, roll, self) - else: - return kin_ss_sol(sa, u, self) - - def calc_curvature(self, sa: float, u: float, roll: float) -> float: - """Returns the curvature. Multiplied by the speed this will give the yaw rate. - - Args: - sa: Steering wheel angle [rad] - u: Speed [m/s] - roll: Road Roll [rad] - - Returns: - Curvature factor [1/m] - """ - return (self.curvature_factor(u) * sa / self.sR) + self.roll_compensation(roll, u) - - def curvature_factor(self, u: float) -> float: - """Returns the curvature factor. - Multiplied by wheel angle (not steering wheel angle) this will give the curvature. - - Args: - u: Speed [m/s] - - Returns: - Curvature factor [1/m] - """ - sf = calc_slip_factor(self) - return (1. - self.chi) / (1. - sf * u**2) / self.l - - def get_steer_from_curvature(self, curv: float, u: float, roll: float) -> float: - """Calculates the required steering wheel angle for a given curvature - - Args: - curv: Desired curvature [1/m] - u: Speed [m/s] - roll: Road Roll [rad] - - Returns: - Steering wheel angle [rad] - """ - - return (curv - self.roll_compensation(roll, u)) * self.sR * 1.0 / self.curvature_factor(u) - - def roll_compensation(self, roll: float, u: float) -> float: - """Calculates the roll-compensation to curvature - - Args: - roll: Road Roll [rad] - u: Speed [m/s] - - Returns: - Roll compensation curvature [rad] - """ - sf = calc_slip_factor(self) - - if abs(sf) < 1e-6: - return 0 - else: - return (ACCELERATION_DUE_TO_GRAVITY * roll) / ((1 / sf) - u**2) - - def get_steer_from_yaw_rate(self, yaw_rate: float, u: float, roll: float) -> float: - """Calculates the required steering wheel angle for a given yaw_rate - - Args: - yaw_rate: Desired yaw rate [rad/s] - u: Speed [m/s] - roll: Road Roll [rad] - - Returns: - Steering wheel angle [rad] - """ - curv = yaw_rate / u - return self.get_steer_from_curvature(curv, u, roll) - - def yaw_rate(self, sa: float, u: float, roll: float) -> float: - """Calculate yaw rate - - Args: - sa: Steering wheel angle [rad] - u: Speed [m/s] - roll: Road Roll [rad] - - Returns: - Yaw rate [rad/s] - """ - return self.calc_curvature(sa, u, roll) * u - - -def kin_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray: - """Calculate the steady state solution at low speeds - At low speeds the tire slip is undefined, so a kinematic - model is used. - - Args: - sa: Steering angle [rad] - u: Speed [m/s] - VM: Vehicle model - - Returns: - 2x1 matrix with steady state solution - """ - K = np.zeros((2, 1)) - K[0, 0] = VM.aR / VM.sR / VM.l * u - K[1, 0] = 1. / VM.sR / VM.l * u - return K * sa - - -def create_dyn_state_matrices(u: float, VM: VehicleModel) -> tuple[np.ndarray, np.ndarray]: - """Returns the A and B matrix for the dynamics system - - Args: - u: Vehicle speed [m/s] - VM: Vehicle model - - Returns: - A tuple with the 2x2 A matrix, and 2x2 B matrix - - Parameters in the vehicle model: - cF: Tire stiffness Front [N/rad] - cR: Tire stiffness Front [N/rad] - aF: Distance from CG to front wheels [m] - aR: Distance from CG to rear wheels [m] - m: Mass [kg] - j: Rotational inertia [kg m^2] - sR: Steering ratio [-] - chi: Steer ratio rear [-] - """ - A = np.zeros((2, 2)) - B = np.zeros((2, 2)) - A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u) - A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u - A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u) - A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u) - - # Steering input - B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR - B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR - - # Roll input - B[0, 1] = -ACCELERATION_DUE_TO_GRAVITY - - return A, B - - -def dyn_ss_sol(sa: float, u: float, roll: float, VM: VehicleModel) -> np.ndarray: - """Calculate the steady state solution when x_dot = 0, - Ax + Bu = 0 => x = -A^{-1} B u - - Args: - sa: Steering angle [rad] - u: Speed [m/s] - roll: Road Roll [rad] - VM: Vehicle model - - Returns: - 2x1 matrix with steady state solution - """ - A, B = create_dyn_state_matrices(u, VM) - inp = np.array([[sa], [roll]]) - return -solve(A, B) @ inp # type: ignore - - -def calc_slip_factor(VM: VehicleModel) -> float: - """The slip factor is a measure of how the curvature changes with speed - it's positive for Oversteering vehicle, negative (usual case) otherwise. - """ - return VM.m * (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR) diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 87a93cdd0c..5f40f19e46 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -5,7 +5,7 @@ from typing import Any import numpy as np -from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY +from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.locationd.models.constants import ObservationKind from openpilot.common.swaglog import cloudlog diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 986c5349f5..bdbb2e62b4 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -4,11 +4,11 @@ from collections import deque, defaultdict import cereal.messaging as messaging from cereal import car, log +from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose HISTORY = 5 # secs diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index f76466dfce..2f44618538 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -4,10 +4,10 @@ import math import numpy as np from cereal import messaging, car +from opendbc.car.vehicle_model import VehicleModel from openpilot.common.realtime import DT_CTRL, Ratekeeper from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel LongCtrlState = car.CarControl.Actuators.LongControlState MAX_LAT_ACCEL = 2.5