diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index d6319e48e7..01b88ce1d0 100644 --- a/.pre-commit-config.yaml +++ b/.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: diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 85acca54d9..dc3d1f4b25 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -14,83 +14,12 @@ A depends on longitudinal speed, u [m/s], and vehicle parameters CP """ 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 @@ class VehicleModel(): 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 @@ class VehicleModel(): 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 @@ class VehicleModel(): """ 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 @@ class VehicleModel(): 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 @@ class VehicleModel(): 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 @@ class VehicleModel(): 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 @@ class VehicleModel(): 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) diff --git a/selfdrive/debug/internal/test_paramsd.py b/selfdrive/debug/internal/test_paramsd.py index 3553949ac8..fa8c31ef1e 100755 --- a/selfdrive/debug/internal/test_paramsd.py +++ b/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 @@ speeds = 10 * np.sin(2 * np.pi * ts / 200.) + 25 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 = []