vehicle model types (commaai#1631)

.pre-commit-config.yaml

@@ -15,6 +15,7 @@ repos:
     hooks:
     -   id: mypy
         exclude: '^(pyextra)|(external)|(cereal)|(rednose)|(panda)|(laika)|(opendbc)|(laika_repo)|(rednose_repo)/'
+        additional_dependencies: ['git+https://github.com/numpy/numpy-stubs']
 -   repo: https://github.com/PyCQA/flake8
     rev: master
     hooks:

selfdrive/controls/lib/vehicle_model.py

@@ -14,83 +14,12 @@
 """
 import numpy as np
 from numpy.linalg import solve
+from typing import Tuple
+from cereal import car
 
 
-def create_dyn_state_matrices(u, VM):
-  """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 2x1 B matrix
-
-  Parameters in the vehicle model:
-    cF: Tire stiffnes Front [N/rad]
-    cR: Tire stiffnes 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, 1))
-  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)
-  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
-  return A, B
-
-
-def kin_ss_sol(sa, u, VM):
-  """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 dyn_ss_sol(sa, u, VM):
-  """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]
-    VM: Vehicle model
-
-  Returns:
-    2x1 matrix with steady state solution
-  """
-  A, B = create_dyn_state_matrices(u, VM)
-  return -solve(A, B) * sa
-
-
-def calc_slip_factor(VM):
-  """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)
-
-
-class VehicleModel():
-  def __init__(self, CP):
+class VehicleModel:
+  def __init__(self, CP: car.CarParams):
     """
     Args:
       CP: Car Parameters
@@ -107,13 +36,13 @@ def __init__(self, CP):
     self.cR_orig = CP.tireStiffnessRear
     self.update_params(1.0, CP.steerRatio)
 
-  def update_params(self, stiffness_factor, steer_ratio):
+  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 = stiffness_factor * self.cF_orig
     self.cR = stiffness_factor * self.cR_orig
     self.sR = steer_ratio
 
-  def steady_state_sol(self, sa, u):
+  def steady_state_sol(self, sa: float, u: float) -> np.ndarray:
     """Returns the steady state solution.
 
     If the speed is too small we can't use the dynamic model (tire slip is undefined),
@@ -131,7 +60,7 @@ def steady_state_sol(self, sa, u):
     else:
       return kin_ss_sol(sa, u, self)
 
-  def calc_curvature(self, sa, u):
+  def calc_curvature(self, sa: float, u: float) -> float:
     """Returns the curvature. Multiplied by the speed this will give the yaw rate.
 
     Args:
@@ -143,7 +72,7 @@ def calc_curvature(self, sa, u):
     """
     return self.curvature_factor(u) * sa / self.sR
 
-  def curvature_factor(self, 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.
 
@@ -156,7 +85,7 @@ def curvature_factor(self, u):
     sf = calc_slip_factor(self)
     return (1. - self.chi) / (1. - sf * u**2) / self.l
 
-  def get_steer_from_curvature(self, curv, u):
+  def get_steer_from_curvature(self, curv: float, u: float) -> float:
     """Calculates the required steering wheel angle for a given curvature
 
     Args:
@@ -169,7 +98,7 @@ def get_steer_from_curvature(self, curv, u):
 
     return curv * self.sR * 1.0 / self.curvature_factor(u)
 
-  def get_steer_from_yaw_rate(self, yaw_rate, u):
+  def get_steer_from_yaw_rate(self, yaw_rate: float, u: float) -> float:
     """Calculates the required steering wheel angle for a given yaw_rate
 
     Args:
@@ -182,7 +111,7 @@ def get_steer_from_yaw_rate(self, yaw_rate, u):
     curv = yaw_rate / u
     return self.get_steer_from_curvature(curv, u)
 
-  def yaw_rate(self, sa, u):
+  def yaw_rate(self, sa: float, u: float) -> float:
     """Calculate yaw rate
 
     Args:
@@ -193,3 +122,76 @@ def yaw_rate(self, sa, u):
       Yaw rate [rad/s]
     """
     return self.calc_curvature(sa, u) * 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 2x1 B matrix
+
+  Parameters in the vehicle model:
+    cF: Tire stiffnes Front [N/rad]
+    cR: Tire stiffnes 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, 1))
+  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)
+  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
+  return A, B
+
+
+def dyn_ss_sol(sa: float, u: 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]
+    VM: Vehicle model
+
+  Returns:
+    2x1 matrix with steady state solution
+  """
+  A, B = create_dyn_state_matrices(u, VM)
+  return -solve(A, B) * sa
+
+
+def calc_slip_factor(VM):
+  """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/debug/internal/test_paramsd.py

@@ -4,6 +4,7 @@
 import numpy as np
 import math
 from tqdm import tqdm
+from typing import cast
 
 import seaborn as sns
 import matplotlib.pyplot as plt
@@ -34,7 +35,7 @@
 
 angle_offsets = math.radians(1.0) * np.ones_like(ts)
 angle_offsets[ts > 60] = 0
-steering_angles = np.radians(5 * np.cos(2 * np.pi * ts / 100.))
+steering_angles = cast(np.ndarray, np.radians(5 * np.cos(2 * np.pi * ts / 100.)))
 
 xs = []
 ys = []