Move vehicle_model.py to opendbc (#34681)
* move * fix * move test too * bump * better * bump to masterpull/34719/head
Shane Smiskol
3 months ago
committed by
GitHub
parent
bd879be27a
commit
6723106bf5
9 changed files with 7 additions and 304 deletions
@ -1 +1 @@ |
||||
Subproject commit 27a50f817a3030aa6ee182110be284d4d136836a |
||||
Subproject commit 706567945a80e0baff18592585d4c1a29e1feea1 |
||||
@ -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) |
||||
@ -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) |
||||
Loading…
Reference in new issue