Move vehicle_model.py to opendbc (#34681)

* move

* fix

* move test too

* bump

* better

* bump to master

opendbc_repo

@@ -1 +1 @@
-Subproject commit 27a50f817a3030aa6ee182110be284d4d136836a
+Subproject commit 706567945a80e0baff18592585d4c1a29e1feea1

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
 
 

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

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
 

selfdrive/controls/lib/tests/test_vehicle_model.py

@@ -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)

selfdrive/controls/lib/vehicle_model.py

@@ -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)

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
 

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

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