From b1856c5f09c312b836d4d0e327ea9a72227d4ee4 Mon Sep 17 00:00:00 2001 From: nandant Date: Mon, 23 Sep 2024 23:32:15 +0200 Subject: [PATCH] Working MB commit --- f1tenth_gym/envs/base_classes.py | 4 +- f1tenth_gym/envs/dynamic_models/__init__.py | 13 +- f1tenth_gym/envs/dynamic_models/kinematic.py | 76 ++- f1tenth_gym/envs/dynamic_models/multi_body.py | 0 .../dynamic_models/multi_body/__init__.py | 121 ++++ .../dynamic_models/multi_body/multi_body.py | 596 ++++++++++++++++++ .../dynamic_models/multi_body/tire_model.py | 184 ++++++ .../envs/dynamic_models/single_track.py | 130 ++-- .../envs/dynamic_models/tire_models.py | 0 f1tenth_gym/envs/dynamic_models/utils.py | 1 + f1tenth_gym/envs/f110_env.py | 35 + f1tenth_gym/envs/integrator.py | 108 +--- 12 files changed, 1061 insertions(+), 207 deletions(-) delete mode 100644 f1tenth_gym/envs/dynamic_models/multi_body.py create mode 100644 f1tenth_gym/envs/dynamic_models/multi_body/__init__.py create mode 100644 f1tenth_gym/envs/dynamic_models/multi_body/multi_body.py create mode 100644 f1tenth_gym/envs/dynamic_models/multi_body/tire_model.py delete mode 100644 f1tenth_gym/envs/dynamic_models/tire_models.py diff --git a/f1tenth_gym/envs/base_classes.py b/f1tenth_gym/envs/base_classes.py index 57afa81b..0dea4e6e 100644 --- a/f1tenth_gym/envs/base_classes.py +++ b/f1tenth_gym/envs/base_classes.py @@ -104,7 +104,7 @@ def __init__( self.model = model # state of the vehicle - self.state = self.model.get_initial_state() + self.state = self.model.get_initial_state(params=self.params) # pose of opponents in the world self.opp_poses = None @@ -205,7 +205,7 @@ def reset(self, pose): # clear collision indicator self.in_collision = False # init state from pose - self.state = self.model.get_initial_state(pose=pose) + self.state = self.model.get_initial_state(pose=pose, params=self.params) self.steer_buffer = np.empty((0,)) # reset scan random generator diff --git a/f1tenth_gym/envs/dynamic_models/__init__.py b/f1tenth_gym/envs/dynamic_models/__init__.py index 3f0bfaf6..9f79507c 100644 --- a/f1tenth_gym/envs/dynamic_models/__init__.py +++ b/f1tenth_gym/envs/dynamic_models/__init__.py @@ -2,14 +2,17 @@ This module contains the dynamic models available in the F1Tenth Gym. Each submodule contains a single model, and the equations or their source is documented alongside it. Many of the models are from the CommonRoad repository, available here: https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/ """ + import warnings from enum import Enum import numpy as np from .kinematic import vehicle_dynamics_ks from .single_track import vehicle_dynamics_st +from .multi_body import init_mb, vehicle_dynamics_mb from .utils import pid_steer, pid_accl + class DynamicModel(Enum): KS = 1 # Kinematic Single Track ST = 2 # Single Track @@ -29,7 +32,10 @@ def from_string(model: str): else: raise ValueError(f"Unknown model type {model}") - def get_initial_state(self, pose=None): + def get_initial_state(self, pose=None, params: dict = None): + # Assert that if self is MB, params is not None + if self == DynamicModel.MB and params is None: + raise ValueError("MultiBody model requires parameters to be provided.") # initialize zero state if self == DynamicModel.KS: # state is [x, y, steer_angle, vel, yaw_angle] @@ -48,6 +54,9 @@ def get_initial_state(self, pose=None): state[0:2] = pose[0:2] state[4] = pose[2] + # If state is MultiBody, we must inflate the state to 29D + if self == DynamicModel.MB: + state = init_mb(state, params) return state @property @@ -56,5 +65,7 @@ def f_dynamics(self): return vehicle_dynamics_ks elif self == DynamicModel.ST: return vehicle_dynamics_st + elif self == DynamicModel.MB: + return vehicle_dynamics_mb else: raise ValueError(f"Unknown model type {self}") diff --git a/f1tenth_gym/envs/dynamic_models/kinematic.py b/f1tenth_gym/envs/dynamic_models/kinematic.py index 84feb08f..1af38b03 100644 --- a/f1tenth_gym/envs/dynamic_models/kinematic.py +++ b/f1tenth_gym/envs/dynamic_models/kinematic.py @@ -3,27 +3,8 @@ from .utils import steering_constraint, accl_constraints -@njit(cache=True) -def vehicle_dynamics_ks( - x, - u_init, - mu, - C_Sf, - C_Sr, - lf, - lr, - h, - m, - I, - s_min, - s_max, - sv_min, - sv_max, - v_switch, - a_max, - v_min, - v_max, -): + +def vehicle_dynamics_ks(x: np.ndarray, u_init: np.ndarray, params: dict): """ Single Track Kinematic Vehicle Dynamics. Follows https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/-/blob/master/vehicleModels_commonRoad.pdf, section 5 @@ -38,22 +19,23 @@ def vehicle_dynamics_ks( u (numpy.ndarray (2, )): control input vector (u1, u2) u1: steering angle velocity of front wheels u2: longitudinal acceleration - mu (float): friction coefficient - C_Sf (float): cornering stiffness of front wheels - C_Sr (float): cornering stiffness of rear wheels - lf (float): distance from center of gravity to front axle - lr (float): distance from center of gravity to rear axle - h (float): height of center of gravity - m (float): mass of vehicle - I (float): moment of inertia of vehicle, about Z axis - s_min (float): minimum steering angle - s_max (float): maximum steering angle - sv_min (float): minimum steering velocity - sv_max (float): maximum steering velocity - v_switch (float): velocity above which the acceleration is no longer able to create wheel slip - a_max (float): maximum allowed acceleration - v_min (float): minimum allowed velocity - v_max (float): maximum allowed velocity + params (dict): dictionary containing the following parameters: + mu (float): friction coefficient + C_Sf (float): cornering stiffness of front wheels + C_Sr (float): cornering stiffness of rear wheels + lf (float): distance from center of gravity to front axle + lr (float): distance from center of gravity to rear axle + h (float): height of center of gravity + m (float): mass of vehicle + I (float): moment of inertia of vehicle, about Z axis + s_min (float): minimum steering angle + s_max (float): maximum steering angle + sv_min (float): minimum steering velocity + sv_max (float): maximum steering velocity + v_switch (float): velocity above which the acceleration is no longer able to create wheel slip + a_max (float): maximum allowed acceleration + v_min (float): minimum allowed velocity + v_max (float): maximum allowed velocity Returns: f (numpy.ndarray): right hand side of differential equations @@ -68,13 +50,27 @@ def vehicle_dynamics_ks( RAW_STEER_VEL = u_init[0] RAW_ACCL = u_init[1] # wheelbase - lwb = lf + lr + lwb = params["lf"] + params["lr"] # constraints u = np.array( [ - steering_constraint(DELTA, RAW_STEER_VEL, s_min, s_max, sv_min, sv_max), - accl_constraints(V, RAW_ACCL, v_switch, a_max, v_min, v_max), + steering_constraint( + DELTA, + RAW_STEER_VEL, + params["s_min"], + params["s_max"], + params["sv_min"], + params["sv_max"], + ), + accl_constraints( + V, + RAW_ACCL, + params["v_switch"], + params["a_max"], + params["v_min"], + params["v_max"], + ), ] ) # Corrected Actions diff --git a/f1tenth_gym/envs/dynamic_models/multi_body.py b/f1tenth_gym/envs/dynamic_models/multi_body.py deleted file mode 100644 index e69de29b..00000000 diff --git a/f1tenth_gym/envs/dynamic_models/multi_body/__init__.py b/f1tenth_gym/envs/dynamic_models/multi_body/__init__.py new file mode 100644 index 00000000..c6ef9afa --- /dev/null +++ b/f1tenth_gym/envs/dynamic_models/multi_body/__init__.py @@ -0,0 +1,121 @@ +""" +Multi-body model initialization functions +""" + +import numpy as np +from numba import njit + +from .multi_body import vehicle_dynamics_mb + + +def init_mb(init_state, params: dict) -> np.ndarray: + # init_MB - generates the initial state vector for the multi-body model + # + # Syntax: + # x0 = init_MB(init_state, p) + # + # Inputs: + # init_state - core initial states + # p - parameter vector + # + # Outputs: + # x0 - initial state vector + # + # Example: + # + # See also: --- + + # Author: Matthias Althoff + # Written: 11-January-2017 + # Last update: --- + # Last revision:--- + + ### Parameters + ## steering constraints + s_min = params["s_min"] # minimum steering angle [rad] + s_max = params["s_max"] # maximum steering angle [rad] + ## longitudinal constraints + v_min = params["v_min"] # minimum velocity [m/s] + v_max = params["v_max"] # minimum velocity [m/s] + ## masses + m_s = params["m_s"] # sprung mass [kg] SMASS + m_uf = params["m_uf"] # unsprung mass front [kg] UMASSF + m_ur = params["m_ur"] # unsprung mass rear [kg] UMASSR + ## axes distances + lf = params["lf"] + # distance from spring mass center of gravity to front axle [m] LENA + lr = params["lr"] + # distance from spring mass center of gravity to rear axle [m] LENB + + ## geometric parameters + K_zt = params["K_zt"] # vertical spring rate of tire [N/m] TSPRINGR + R_w = params["R_w"] + # effective wheel/tire radius chosen as tire rolling radius RR taken from ADAMS documentation [m] + # create equivalent bicycle parameters + g = 9.81 # [m/s^2] + + # obtain initial states from vector + sx0 = init_state[0] # x-position in a global coordinate system + sy0 = init_state[1] # y-position in a global coordinate system + delta0 = init_state[2] # steering angle of front wheels + vel0 = init_state[3] # speed of the car + Psi0 = init_state[4] # yaw angle + dotPsi0 = init_state[5] # yaw rate + beta0 = init_state[6] # slip angle + + if delta0 > s_max: + delta0 = s_max + + if delta0 < s_min: + delta0 = s_min + + if vel0 > v_max: + vel0 = v_max + + if vel0 < v_min: + vel0 = v_min + + # auxiliary initial states + F0_z_f = m_s * g * lr / (lf + lr) + m_uf * g + F0_z_r = m_s * g * lf / (lf + lr) + m_ur * g + + # sprung mass states + x0 = np.zeros((29,)) # init initial state vector + x0[0] = sx0 # x-position in a global coordinate system + x0[1] = sy0 # y-position in a global coordinate system + x0[2] = delta0 # steering angle of front wheels + x0[3] = np.cos(beta0) * vel0 # velocity in x-direction + x0[4] = Psi0 # yaw angle + x0[5] = dotPsi0 # yaw rate + x0[6] = 0 # roll angle + x0[7] = 0 # roll rate + x0[8] = 0 # pitch angle + x0[9] = 0 # pitch rate + x0[10] = np.sin(beta0) * vel0 # velocity in y-direction + x0[11] = 0 # z-position (zero height corresponds to steady state solution) + x0[12] = 0 # velocity in z-direction + + # unsprung mass states (front) + x0[13] = 0 # roll angle front + x0[14] = 0 # roll rate front + x0[15] = np.sin(beta0) * vel0 + lf * dotPsi0 # velocity in y-direction front + x0[16] = (F0_z_f) / (2 * K_zt) # z-position front + x0[17] = 0 # velocity in z-direction front + + # unsprung mass states (rear) + x0[18] = 0 # roll angle rear + x0[19] = 0 # roll rate rear + x0[20] = np.sin(beta0) * vel0 - lr * dotPsi0 # velocity in y-direction rear + x0[21] = (F0_z_r) / (2 * K_zt) # z-position rear + x0[22] = 0 # velocity in z-direction rear + + # wheel states + x0[23] = x0[3] / (R_w) # left front wheel angular speed + x0[24] = x0[3] / (R_w) # right front wheel angular speed + x0[25] = x0[3] / (R_w) # left rear wheel angular speed + x0[26] = x0[3] / (R_w) # right rear wheel angular speed + + x0[27] = 0 # delta_y_f + x0[28] = 0 # delta_y_r + + return x0 diff --git a/f1tenth_gym/envs/dynamic_models/multi_body/multi_body.py b/f1tenth_gym/envs/dynamic_models/multi_body/multi_body.py new file mode 100644 index 00000000..102239f5 --- /dev/null +++ b/f1tenth_gym/envs/dynamic_models/multi_body/multi_body.py @@ -0,0 +1,596 @@ +import numpy as np +from numba import njit + +from .tire_model import ( + formula_lateral, + formula_lateral_comb, + formula_longitudinal, + formula_longitudinal_comb, +) +from ..kinematic import vehicle_dynamics_ks + + +def vehicle_dynamics_mb(x: np.ndarray, u_init: np.ndarray, params: dict): + """ + vehicleDynamics_mb - multi-body vehicle dynamics based on the DoT (department of transportation) vehicle dynamics + reference point: center of mass + + Syntax: + f = vehicleDynamics_mb(x,u,p) + + Inputs: + :param x: vehicle state vector + :param uInit: vehicle input vector + :param params: vehicle parameter vector + + Outputs: + :return f: right-hand side of differential equations + """ + + # ------------- BEGIN CODE -------------- + + # set gravity constant + g = 9.81 # [m/s^2] + + # states + # x1 = x-position in a global coordinate system + # x2 = y-position in a global coordinate system + # x3 = steering angle of front wheels + # x4 = velocity in x-direction + # x5 = yaw angle + # x6 = yaw rate + + # x7 = roll angle + # x8 = roll rate + # x9 = pitch angle + # x10 = pitch rate + # x11 = velocity in y-direction + # x12 = z-position + # x13 = velocity in z-direction + + # x14 = roll angle front + # x15 = roll rate front + # x16 = velocity in y-direction front + # x17 = z-position front + # x18 = velocity in z-direction front + + # x19 = roll angle rear + # x20 = roll rate rear + # x21 = velocity in y-direction rear + # x22 = z-position rear + # x23 = velocity in z-direction rear + + # x24 = left front wheel angular speed + # x25 = right front wheel angular speed + # x26 = left rear wheel angular speed + # x27 = right rear wheel angular speed + + # x28 = delta_y_f + # x29 = delta_y_r + + # u1 = steering angle velocity of front wheels + # u2 = acceleration + + u = u_init + + # vehicle body dimensions + length = params["length"] # vehicle length [m] + width = params["width"] # vehicle width [m] + + # steering constraints + s_min = params["s_min"] # minimum steering angle [rad] + s_max = params["s_max"] # maximum steering angle [rad] + sv_min = params["sv_min"] # minimum steering velocity [rad/s] + sv_max = params["sv_max"] # maximum steering velocity [rad/s] + + # longitudinal constraints + v_min = params["s_min"] # minimum velocity [m/s] + v_max = params["s_max"] # minimum velocity [m/s] + v_switch = params["sv_min"] # switching velocity [m/s] + a_max = params["sv_max"] # maximum absolute acceleration [m/s^2] + + # masses + m = params["m"] # vehicle mass [kg] MASS + m_s = params["m_s"] # sprung mass [kg] SMASS + m_uf = params["m_uf"] # unsprung mass front [kg] UMASSF + m_ur = params["m_ur"] # unsprung mass rear [kg] UMASSR + + # axes distances + lf = params["lf"] + # distance from spring mass center of gravity to front axle [m] LENA + lr = params["lf"] + # distance from spring mass center of gravity to rear axle [m] LENB + + # moments of inertia of sprung mass + I_Phi_s = params["I_Phi_s"] + # moment of inertia for sprung mass in roll [kg m^2] IXS + I_y_s = params["I_y_s"] # moment of inertia for sprung mass in pitch [kg m^2] IYS + I_z = params["I_z"] # moment of inertia for sprung mass in yaw [kg m^2] IZZ + I_xz_s = params["I_xz_s"] # moment of inertia cross product [kg m^2] IXZ + + # suspension parameters + K_sf = params["K_sf"] # suspension spring rate (front) [N/m] KSF + K_sdf = params["K_sdf"] # suspension damping rate (front) [N s/m] KSDF + K_sr = params["K_sr"] # suspension spring rate (rear) [N/m] KSR + K_sdr = params["K_sdr"] # suspension damping rate (rear) [N s/m] KSDR + + # geometric parameters + T_f = params["T_f"] # track width front [m] TRWF + T_r = params["T_r"] # track width rear [m] TRWB + K_ras = params["K_ras"] + # lateral spring rate at compliant compliant pin joint between M_s and M_u [N/m] KRAS + + K_tsf = params["K_tsf"] + # auxiliary torsion roll stiffness per axle (normally negative) (front) [N m/rad] KTSF + K_tsr = params["K_tsr"] + # auxiliary torsion roll stiffness per axle (normally negative) (rear) [N m/rad] KTSR + K_rad = params["K_rad"] + # damping rate at compliant compliant pin joint between M_s and M_u [N s/m] KRADP + K_zt = params["K_zt"] # vertical spring rate of tire [N/m] TSPRINGR + + h_cg = params["h_cg"] + # center of gravity height of total mass [m] HCG (mainly required for conversion to other vehicle models) + h_raf = params["h_raf"] # height of roll axis above ground (front) [m] HRAF + h_rar = params["h_rar"] # height of roll axis above ground (rear) [m] HRAR + + h_s = params["h_s"] # M_s center of gravity above ground [m] HS + + I_uf = params["I_uf"] + # moment of inertia for unsprung mass about x-axis (front) [kg m^2] IXUF + I_ur = params["I_ur"] + # moment of inertia for unsprung mass about x-axis (rear) [kg m^2] IXUR + I_y_w = params["I_y_w"] + # wheel inertia, from internet forum for 235/65 R 17 [kg m^2] + + K_lt = params["K_lt"] + # lateral compliance rate of tire, wheel, and suspension, per tire [m/N] KLT + R_w = params["R_w"] + # effective wheel/tire radius chosen as tire rolling radius RR taken from ADAMS documentation [m] + + # split of brake and engine torque + T_sb = params["T_sb"] + T_se = params["T_se"] + + # suspension parameters + D_f = params["D_f"] # [rad/m] DF + D_r = params["D_r"] # [rad/m] DR + E_f = params["E_f"] # [needs conversion if nonzero] EF + E_r = params["E_r"] # [needs conversion if nonzero] ER + + use_kinematic = True if x[3] < 0.1 else False + + # consider steering and acceleration constraints - outside of the integration + # u = np.array([steering_constraint(x[2], u_init[0], s_min, s_max, sv_min, sv_max), accl_constraints(x[3], u_init[1], v_switch, a_max, v_min, v_max)]) + + # compute slip angle at cg - outside of the integration + # switch to kinematic model for small velocities handeled outside + if use_kinematic: + beta = 0.0 + else: + beta = np.arctan(x[10] / x[3]) + vel = np.sqrt(x[3] ** 2 + x[10] ** 2) + + # vertical tire forces + F_z_LF = (x[16] + R_w * (np.cos(x[13]) - 1) - 0.5 * T_f * np.sin(x[13])) * K_zt + F_z_RF = (x[16] + R_w * (np.cos(x[13]) - 1) + 0.5 * T_f * np.sin(x[13])) * K_zt + F_z_LR = (x[21] + R_w * (np.cos(x[18]) - 1) - 0.5 * T_r * np.sin(x[18])) * K_zt + F_z_RR = (x[21] + R_w * (np.cos(x[18]) - 1) + 0.5 * T_r * np.sin(x[18])) * K_zt + + # obtain individual tire speeds + u_w_lf = (x[3] + 0.5 * T_f * x[5]) * np.cos(x[2]) + (x[10] + lf * x[5]) * np.sin( + x[2] + ) + u_w_rf = (x[3] - 0.5 * T_f * x[5]) * np.cos(x[2]) + (x[10] + lf * x[5]) * np.sin( + x[2] + ) + u_w_lr = x[3] + 0.5 * T_r * x[5] + u_w_rr = x[3] - 0.5 * T_r * x[5] + + # negative wheel spin forbidden + u_w_lf = np.maximum(u_w_lf, 0.0) + u_w_rf = np.maximum(u_w_rf, 0.0) + u_w_lr = np.maximum(u_w_lr, 0.0) + u_w_rr = np.maximum(u_w_rr, 0.0) + # if u_w_lf < 0.0: + # u_w_lf *= 0 + # + # if u_w_rf < 0.0: + # u_w_rf *= 0 + # + # if u_w_lr < 0.0: + # u_w_lr *= 0 + # + # if u_w_rr < 0.0: + # u_w_rr *= 0 + + # compute longitudinal slip + # switch to kinematic model for small velocities + if use_kinematic: + s_lf = 0.0 + s_rf = 0.0 + s_lr = 0.0 + s_rr = 0.0 + else: + s_lf = 1 - R_w * x[23] / u_w_lf + s_rf = 1 - R_w * x[24] / u_w_rf + s_lr = 1 - R_w * x[25] / u_w_lr + s_rr = 1 - R_w * x[26] / u_w_rr + + # lateral slip angles + # switch to kinematic model for small velocities + if use_kinematic: + alpha_LF = 0.0 + alpha_RF = 0.0 + alpha_LR = 0.0 + alpha_RR = 0.0 + else: + alpha_LF = ( + np.arctan( + (x[10] + lf * x[5] - x[14] * (R_w - x[16])) / (x[3] + 0.5 * T_f * x[5]) + ) + - x[2] + ) + alpha_RF = ( + np.arctan( + (x[10] + lf * x[5] - x[14] * (R_w - x[16])) / (x[3] - 0.5 * T_f * x[5]) + ) + - x[2] + ) + alpha_LR = np.arctan( + (x[10] - lr * x[5] - x[19] * (R_w - x[21])) / (x[3] + 0.5 * T_r * x[5]) + ) + alpha_RR = np.arctan( + (x[10] - lr * x[5] - x[19] * (R_w - x[21])) / (x[3] - 0.5 * T_r * x[5]) + ) + + # auxiliary suspension movement + z_SLF = ( + (h_s - R_w + x[16] - x[11]) / np.cos(x[6]) + - h_s + + R_w + + lf * x[8] + + 0.5 * (x[6] - x[13]) * T_f + ) + z_SRF = ( + (h_s - R_w + x[16] - x[11]) / np.cos(x[6]) + - h_s + + R_w + + lf * x[8] + - 0.5 * (x[6] - x[13]) * T_f + ) + z_SLR = ( + (h_s - R_w + x[21] - x[11]) / np.cos(x[6]) + - h_s + + R_w + - lr * x[8] + + 0.5 * (x[6] - x[18]) * T_r + ) + z_SRR = ( + (h_s - R_w + x[21] - x[11]) / np.cos(x[6]) + - h_s + + R_w + - lr * x[8] + - 0.5 * (x[6] - x[18]) * T_r + ) + + dz_SLF = x[17] - x[12] + lf * x[9] + 0.5 * (x[7] - x[14]) * T_f + dz_SRF = x[17] - x[12] + lf * x[9] - 0.5 * (x[7] - x[14]) * T_f + dz_SLR = x[22] - x[12] - lr * x[9] + 0.5 * (x[7] - x[19]) * T_r + dz_SRR = x[22] - x[12] - lr * x[9] - 0.5 * (x[7] - x[19]) * T_r + + # camber angles + gamma_LF = x[6] + D_f * z_SLF + E_f * (z_SLF) ** 2 + gamma_RF = x[6] - D_f * z_SRF - E_f * (z_SRF) ** 2 + gamma_LR = x[6] + D_r * z_SLR + E_r * (z_SLR) ** 2 + gamma_RR = x[6] - D_r * z_SRR - E_r * (z_SRR) ** 2 + + # compute longitudinal tire forces using the magic formula for pure slip + F0_x_LF = formula_longitudinal(s_lf, gamma_LF, F_z_LF, params) + F0_x_RF = formula_longitudinal(s_rf, gamma_RF, F_z_RF, params) + F0_x_LR = formula_longitudinal(s_lr, gamma_LR, F_z_LR, params) + F0_x_RR = formula_longitudinal(s_rr, gamma_RR, F_z_RR, params) + + # compute lateral tire forces using the magic formula for pure slip + res = formula_lateral(alpha_LF, gamma_LF, F_z_LF, params) + F0_y_LF = res[0] + mu_y_LF = res[1] + res = formula_lateral(alpha_RF, gamma_RF, F_z_RF, params) + F0_y_RF = res[0] + mu_y_RF = res[1] + res = formula_lateral(alpha_LR, gamma_LR, F_z_LR, params) + F0_y_LR = res[0] + mu_y_LR = res[1] + res = formula_lateral(alpha_RR, gamma_RR, F_z_RR, params) + F0_y_RR = res[0] + mu_y_RR = res[1] + + # compute longitudinal tire forces using the magic formula for combined slip + F_x_LF = formula_longitudinal_comb(s_lf, alpha_LF, F0_x_LF, params) + F_x_RF = formula_longitudinal_comb(s_rf, alpha_RF, F0_x_RF, params) + F_x_LR = formula_longitudinal_comb(s_lr, alpha_LR, F0_x_LR, params) + F_x_RR = formula_longitudinal_comb(s_rr, alpha_RR, F0_x_RR, params) + + # compute lateral tire forces using the magic formula for combined slip + F_y_LF = formula_lateral_comb( + s_lf, alpha_LF, gamma_LF, mu_y_LF, F_z_LF, F0_y_LF, params + ) + F_y_RF = formula_lateral_comb( + s_rf, alpha_RF, gamma_RF, mu_y_RF, F_z_RF, F0_y_RF, params + ) + F_y_LR = formula_lateral_comb( + s_lr, alpha_LR, gamma_LR, mu_y_LR, F_z_LR, F0_y_LR, params + ) + F_y_RR = formula_lateral_comb( + s_rr, alpha_RR, gamma_RR, mu_y_RR, F_z_RR, F0_y_RR, params + ) + + # auxiliary movements for compliant joint equations + delta_z_f = h_s - R_w + x[16] - x[11] + delta_z_r = h_s - R_w + x[21] - x[11] + + delta_phi_f = x[6] - x[13] + delta_phi_r = x[6] - x[18] + + dot_delta_phi_f = x[7] - x[14] + dot_delta_phi_r = x[7] - x[19] + + dot_delta_z_f = x[17] - x[12] + dot_delta_z_r = x[22] - x[12] + + dot_delta_y_f = x[10] + lf * x[5] - x[15] + dot_delta_y_r = x[10] - lr * x[5] - x[20] + + delta_f = ( + delta_z_f * np.sin(x[6]) + - x[27] * np.cos(x[6]) + - (h_raf - R_w) * np.sin(delta_phi_f) + ) + delta_r = ( + delta_z_r * np.sin(x[6]) + - x[28] * np.cos(x[6]) + - (h_rar - R_w) * np.sin(delta_phi_r) + ) + + dot_delta_f = ( + (delta_z_f * np.cos(x[6]) + x[27] * np.sin(x[6])) * x[7] + + dot_delta_z_f * np.sin(x[6]) + - dot_delta_y_f * np.cos(x[6]) + - (h_raf - R_w) * np.cos(delta_phi_f) * dot_delta_phi_f + ) + dot_delta_r = ( + (delta_z_r * np.cos(x[6]) + x[28] * np.sin(x[6])) * x[7] + + dot_delta_z_r * np.sin(x[6]) + - dot_delta_y_r * np.cos(x[6]) + - (h_rar - R_w) * np.cos(delta_phi_r) * dot_delta_phi_r + ) + + # compliant joint forces + F_RAF = delta_f * K_ras + dot_delta_f * K_rad + F_RAR = delta_r * K_ras + dot_delta_r * K_rad + + # auxiliary suspension forces (bump stop neglected squat/lift forces neglected) + F_SLF = ( + m_s * g * lr / (2 * (lf + lr)) + - z_SLF * K_sf + - dz_SLF * K_sdf + + (x[6] - x[13]) * K_tsf / T_f + ) + + F_SRF = ( + m_s * g * lr / (2 * (lf + lr)) + - z_SRF * K_sf + - dz_SRF * K_sdf + - (x[6] - x[13]) * K_tsf / T_f + ) + + F_SLR = ( + m_s * g * lf / (2 * (lf + lr)) + - z_SLR * K_sr + - dz_SLR * K_sdr + + (x[6] - x[18]) * K_tsr / T_r + ) + + F_SRR = ( + m_s * g * lf / (2 * (lf + lr)) + - z_SRR * K_sr + - dz_SRR * K_sdr + - (x[6] - x[18]) * K_tsr / T_r + ) + + # auxiliary variables sprung mass + sumX = ( + F_x_LR + + F_x_RR + + (F_x_LF + F_x_RF) * np.cos(x[2]) + - (F_y_LF + F_y_RF) * np.sin(x[2]) + ) + + sumN = ( + (F_y_LF + F_y_RF) * lf * np.cos(x[2]) + + (F_x_LF + F_x_RF) * lf * np.sin(x[2]) + + (F_y_RF - F_y_LF) * 0.5 * T_f * np.sin(x[2]) + + (F_x_LF - F_x_RF) * 0.5 * T_f * np.cos(x[2]) + + (F_x_LR - F_x_RR) * 0.5 * T_r + - (F_y_LR + F_y_RR) * lr + ) + + sumY_s = (F_RAF + F_RAR) * np.cos(x[6]) + (F_SLF + F_SLR + F_SRF + F_SRR) * np.sin( + x[6] + ) + + sumL = ( + 0.5 * F_SLF * T_f + + 0.5 * F_SLR * T_r + - 0.5 * F_SRF * T_f + - 0.5 * F_SRR * T_r + - F_RAF + / np.cos(x[6]) + * (h_s - x[11] - R_w + x[16] - (h_raf - R_w) * np.cos(x[13])) + - F_RAR + / np.cos(x[6]) + * (h_s - x[11] - R_w + x[21] - (h_rar - R_w) * np.cos(x[18])) + ) + + sumZ_s = (F_SLF + F_SLR + F_SRF + F_SRR) * np.cos(x[6]) - (F_RAF + F_RAR) * np.sin( + x[6] + ) + + sumM_s = ( + lf * (F_SLF + F_SRF) + - lr * (F_SLR + F_SRR) + + ( + (F_x_LF + F_x_RF) * np.cos(x[2]) + - (F_y_LF + F_y_RF) * np.sin(x[2]) + + F_x_LR + + F_x_RR + ) + * (h_s - x[11]) + ) + + # auxiliary variables unsprung mass + sumL_uf = ( + 0.5 * F_SRF * T_f + - 0.5 * F_SLF * T_f + - F_RAF * (h_raf - R_w) + + F_z_LF * (R_w * np.sin(x[13]) + 0.5 * T_f * np.cos(x[13]) - K_lt * F_y_LF) + - F_z_RF * (-R_w * np.sin(x[13]) + 0.5 * T_f * np.cos(x[13]) + K_lt * F_y_RF) + - ((F_y_LF + F_y_RF) * np.cos(x[2]) + (F_x_LF + F_x_RF) * np.sin(x[2])) + * (R_w - x[16]) + ) + + sumL_ur = ( + 0.5 * F_SRR * T_r + - 0.5 * F_SLR * T_r + - F_RAR * (h_rar - R_w) + + F_z_LR * (R_w * np.sin(x[18]) + 0.5 * T_r * np.cos(x[18]) - K_lt * F_y_LR) + - F_z_RR * (-R_w * np.sin(x[18]) + 0.5 * T_r * np.cos(x[18]) + K_lt * F_y_RR) + - (F_y_LR + F_y_RR) * (R_w - x[21]) + ) + + sumZ_uf = F_z_LF + F_z_RF + F_RAF * np.sin(x[6]) - (F_SLF + F_SRF) * np.cos(x[6]) + + sumZ_ur = F_z_LR + F_z_RR + F_RAR * np.sin(x[6]) - (F_SLR + F_SRR) * np.cos(x[6]) + + sumY_uf = ( + (F_y_LF + F_y_RF) * np.cos(x[2]) + + (F_x_LF + F_x_RF) * np.sin(x[2]) + - F_RAF * np.cos(x[6]) + - (F_SLF + F_SRF) * np.sin(x[6]) + ) + + sumY_ur = (F_y_LR + F_y_RR) - F_RAR * np.cos(x[6]) - (F_SLR + F_SRR) * np.sin(x[6]) + + # dynamics common with single-track model + f = np.zeros((29,)) # init 'right hand side' + # switch to kinematic model for small velocities + if use_kinematic: + # wheelbase + # lwb = lf + lr + + # system dynamics + # x_ks = [x[0], x[1], x[2], x[3], x[4]] + # f_ks = vehicle_dynamics_ks(x_ks, u, p) + # f.extend(f_ks) + # f.append(u[1]*lwb*np.tan(x[2]) + x[3]/(lwb*np.cos(x[2])**2)*u[0]) + + # Use kinematic model with reference point at center of mass + # wheelbase + lwb = lf + lr + + # system dynamics + x_ks = x[0:5] + # kinematic model + # f_ks = vehicle_dynamics_ks_cog(x_ks, u, p) + # f = [f_ks[0], f_ks[1], f_ks[2], f_ks[3], f_ks[4]] # 1 2 3 4 5 + f_ks = vehicle_dynamics_ks(x_ks, u, params) + f[0:5] = f_ks + # derivative of slip angle and yaw rate + d_beta = (lr * u[0]) / ( + lwb * np.cos(x[2]) ** 2 * (1 + (np.tan(x[2]) ** 2 * lr / lwb) ** 2) + ) + dd_psi = ( + 1 + / lwb + * ( + u[1] * np.cos(x[6]) * np.tan(x[2]) + - x[3] * np.sin(x[6]) * d_beta * np.tan(x[2]) + + x[3] * np.cos(x[6]) * u[0] / np.cos(x[2]) ** 2 + ) + ) + # f.append(dd_psi) # 6 + f[5] = dd_psi + + else: + f[0] = np.cos(beta + x[4]) * vel # 1 + f[1] = np.sin(beta + x[4]) * vel # 2 + f[2] = u[0] # 3 + f[3] = 1 / m * sumX + x[5] * x[10] # 4 + f[4] = x[5] # 5 + f[5] = ( + 1 / (I_z - (I_xz_s) ** 2 / I_Phi_s) * (sumN + I_xz_s / I_Phi_s * sumL) + ) # 6 + + # remaining sprung mass dynamics + f[6] = x[7] # 7 + f[7] = 1 / (I_Phi_s - (I_xz_s) ** 2 / I_z) * (I_xz_s / I_z * sumN + sumL) # 8 + f[8] = x[9] # 9 + f[9] = 1 / I_y_s * sumM_s # 10 + f[10] = 1 / m_s * sumY_s - x[5] * x[3] # 11 + f[11] = x[12] # 12 + f[12] = g - 1 / m_s * sumZ_s # 13 + + # unsprung mass dynamics (front) + f[13] = x[14] # 14 + f[14] = 1 / I_uf * sumL_uf # 15 + f[15] = 1 / m_uf * sumY_uf - x[5] * x[3] # 16 + f[16] = x[17] # 17 + f[17] = g - 1 / m_uf * sumZ_uf # 18 + + # unsprung mass dynamics (rear) + f[18] = x[19] # 19 + f[19] = 1 / I_ur * sumL_ur # 20 + f[20] = 1 / m_ur * sumY_ur - x[5] * x[3] # 21 + f[21] = x[22] # 22 + f[22] = g - 1 / m_ur * sumZ_ur # 23 + + # convert acceleration input to brake and engine torque - splitting for brake/drive torque is outside of the integration + if u[0] > 0: + T_B = 0.0 + T_E = m * R_w * u[0] + else: + T_B = m * R_w * u[0] + T_E = 0.0 + + # wheel dynamics (p.T new parameter for torque splitting) - splitting for brake/drive torque is outside of the integration + f[23] = 1 / I_y_w * (-R_w * F_x_LF + 0.5 * T_sb * T_B + 0.5 * T_se * T_E) # 24 + f[24] = 1 / I_y_w * (-R_w * F_x_RF + 0.5 * T_sb * T_B + 0.5 * T_se * T_E) # 25 + f[25] = ( + 1 / I_y_w * (-R_w * F_x_LR + 0.5 * (1 - T_sb) * T_B + 0.5 * (1 - T_se) * T_E) + ) # 26 + f[26] = ( + 1 / I_y_w * (-R_w * F_x_RR + 0.5 * (1 - T_sb) * T_B + 0.5 * (1 - T_se) * T_E) + ) # 27 + + # negative wheel spin forbidden handeled outside of this function + for iState in range(23, 27): + if x[iState] < 0.0: + x[iState] = 0.0 + f[iState] = 0.0 + + # compliant joint equations + f[27] = dot_delta_y_f # 28 + f[28] = dot_delta_y_r # 29 + + # longitudinal slip + # s_lf + # s_rf + # s_lr + # s_rr + + # lateral slip + # alpha_LF + # alpha_RF + # alpha_LR + # alpha_RR + + return f diff --git a/f1tenth_gym/envs/dynamic_models/multi_body/tire_model.py b/f1tenth_gym/envs/dynamic_models/multi_body/tire_model.py new file mode 100644 index 00000000..6fc6f704 --- /dev/null +++ b/f1tenth_gym/envs/dynamic_models/multi_body/tire_model.py @@ -0,0 +1,184 @@ +import numpy as np +from numba import njit + + +# longitudinal tire forces +def formula_longitudinal(kappa, gamma, F_z, params: dict): + # longitudinal coefficients + tire_p_cx1 = params["tire_p_cx1"] # Shape factor Cfx for longitudinal force + tire_p_dx1 = params["tire_p_dx1"] # Longitudinal friction Mux at Fznom + tire_p_dx3 = params["tire_p_dx3"] # Variation of friction Mux with camber + tire_p_ex1 = params["tire_p_ex1"] # Longitudinal curvature Efx at Fznom + tire_p_kx1 = params["tire_p_kx1"] # Longitudinal slip stiffness Kfx/Fz at Fznom + tire_p_hx1 = params["tire_p_hx1"] # Horizontal shift Shx at Fznom + tire_p_vx1 = params["tire_p_vx1"] # Vertical shift Svx/Fz at Fznom + + # turn slip is neglected, so xi_i=1 + # all scaling factors lambda = 1 + + # coordinate system transformation + kappa = -kappa + + S_hx = tire_p_hx1 + S_vx = F_z * tire_p_vx1 + + kappa_x = kappa + S_hx + mu_x = tire_p_dx1 * (1 - tire_p_dx3 * gamma**2) + + C_x = tire_p_cx1 + D_x = mu_x * F_z + E_x = tire_p_ex1 + K_x = F_z * tire_p_kx1 + B_x = K_x / (C_x * D_x) + + # magic tire formula + return D_x * np.sin( + C_x + * np.arctan(B_x * kappa_x - E_x * (B_x * kappa_x - np.arctan(B_x * kappa_x))) + + S_vx + ) + + +# lateral tire forces +def formula_lateral(alpha, gamma, F_z, params: dict): + # lateral coefficients + tire_p_cy1 = params["tire_p_cy1"] # Shape factor Cfy for lateral forces + tire_p_dy1 = params["tire_p_dy1"] # Lateral friction Muy + tire_p_dy3 = params["tire_p_dy3"] # Variation of friction Muy with squared camber + tire_p_ey1 = params["tire_p_ey1"] # Lateral curvature Efy at Fznom + tire_p_ky1 = params["tire_p_ky1"] # Maximum value of stiffness Kfy/Fznom + tire_p_hy1 = params["tire_p_hy1"] # Horizontal shift Shy at Fznom + tire_p_hy3 = params["tire_p_hy3"] # Variation of shift Shy with camber + tire_p_vy1 = params["tire_p_vy1"] # Vertical shift in Svy/Fz at Fznom + tire_p_vy3 = params["tire_p_vy3"] # Variation of shift Svy/Fz with camber + + # turn slip is neglected, so xi_i=1 + # all scaling factors lambda = 1 + + # coordinate system transformation + # alpha = -alpha + + S_hy = np.sign(gamma) * (tire_p_hy1 + tire_p_hy3 * np.fabs(gamma)) + S_vy = np.sign(gamma) * F_z * (tire_p_vy1 + tire_p_vy3 * np.fabs(gamma)) + + alpha_y = alpha + S_hy + mu_y = tire_p_dy1 * (1 - tire_p_dy3 * gamma**2) + + C_y = tire_p_cy1 + D_y = mu_y * F_z + E_y = tire_p_ey1 + K_y = F_z * tire_p_ky1 # simplify K_y0 to tire_p_ky1*F_z + B_y = K_y / (C_y * D_y) + + # magic tire formula + F_y = ( + D_y + * np.sin( + C_y + * np.arctan( + B_y * alpha_y - E_y * (B_y * alpha_y - np.arctan(B_y * alpha_y)) + ) + ) + + S_vy + ) + + res = [] + res.append(F_y) + res.append(mu_y) + return res + + +# longitudinal tire forces for combined slip +def formula_longitudinal_comb(kappa, alpha, F0_x, params: dict): + # longitudinal coefficients + tire_r_bx1 = params["tire_r_bx1"] # Slope factor for combined slip Fx reduction + tire_r_bx2 = params["tire_r_bx2"] # Variation of slope Fx reduction with kappa + tire_r_cx1 = params["tire_r_cx1"] # Shape factor for combined slip Fx reduction + tire_r_ex1 = params["tire_r_ex1"] # Curvature factor of combined Fx + tire_r_hx1 = params["tire_r_hx1"] # Shift factor for combined slip Fx reduction + + # turn slip '' neglected, so xi_i=1 + # all scaling factors lambda = 1 + + S_hxalpha = tire_r_hx1 + + alpha_s = alpha + S_hxalpha + + B_xalpha = tire_r_bx1 * np.cos(np.arctan(tire_r_bx2 * kappa)) + C_xalpha = tire_r_cx1 + E_xalpha = tire_r_ex1 + D_xalpha = F0_x / ( + np.cos( + C_xalpha + * np.arctan( + B_xalpha * S_hxalpha + - E_xalpha * (B_xalpha * S_hxalpha - np.arctan(B_xalpha * S_hxalpha)) + ) + ) + ) + + # magic tire formula + return D_xalpha * np.cos( + C_xalpha + * np.arctan( + B_xalpha * alpha_s + - E_xalpha * (B_xalpha * alpha_s - np.arctan(B_xalpha * alpha_s)) + ) + ) + + +# lateral tire forces for combined slip +def formula_lateral_comb(kappa, alpha, gamma, mu_y, F_z, F0_y, params: dict): + # lateral coefficients + tire_r_by1 = params["tire_r_by1"] # Slope factor for combined Fy reduction + tire_r_by2 = params["tire_r_by2"] # Variation of slope Fy reduction with alpha + tire_r_by3 = params["tire_r_by3"] # Shift term for alpha in slope Fy reduction + tire_r_cy1 = params["tire_r_cy1"] # Shape factor for combined Fy reduction + tire_r_ey1 = params["tire_r_ey1"] # Curvature factor of combined Fy + tire_r_hy1 = params["tire_r_hy1"] # Shift factor for combined Fy reduction + tire_r_vy1 = params["tire_r_vy1"] # Kappa induced side force Svyk/Muy*Fz at Fznom + tire_r_vy3 = params["tire_r_vy3"] # Variation of Svyk/Muy*Fz with camber + tire_r_vy4 = params["tire_r_vy4"] # Variation of Svyk/Muy*Fz with alpha + tire_r_vy5 = params["tire_r_vy5"] # Variation of Svyk/Muy*Fz with kappa + tire_r_vy6 = params["tire_r_vy6"] # Variation of Svyk/Muy*Fz with atan(kappa) + + # turn slip is neglected, so xi_i=1 + # all scaling factors lambda = 1 + + S_hykappa = tire_r_hy1 + + kappa_s = kappa + S_hykappa + + B_ykappa = tire_r_by1 * np.cos(np.arctan(tire_r_by2 * (alpha - tire_r_by3))) + C_ykappa = tire_r_cy1 + E_ykappa = tire_r_ey1 + D_ykappa = F0_y / ( + np.cos( + C_ykappa + * np.arctan( + B_ykappa * S_hykappa + - E_ykappa * (B_ykappa * S_hykappa - np.arctan(B_ykappa * S_hykappa)) + ) + ) + ) + + D_vykappa = ( + mu_y + * F_z + * (tire_r_vy1 + tire_r_vy3 * gamma) + * np.cos(np.arctan(tire_r_vy4 * alpha)) + ) + S_vykappa = D_vykappa * np.sin(tire_r_vy5 * np.arctan(tire_r_vy6 * kappa)) + + # magic tire formula + return ( + D_ykappa + * np.cos( + C_ykappa + * np.arctan( + B_ykappa * kappa_s + - E_ykappa * (B_ykappa * kappa_s - np.arctan(B_ykappa * kappa_s)) + ) + ) + + S_vykappa + ) diff --git a/f1tenth_gym/envs/dynamic_models/single_track.py b/f1tenth_gym/envs/dynamic_models/single_track.py index 16b53b7a..8ce62d0e 100644 --- a/f1tenth_gym/envs/dynamic_models/single_track.py +++ b/f1tenth_gym/envs/dynamic_models/single_track.py @@ -3,27 +3,8 @@ from .utils import steering_constraint, accl_constraints -@njit(cache=True) -def vehicle_dynamics_st( - x, - u_init, - mu, - C_Sf, - C_Sr, - lf, - lr, - h, - m, - I, - s_min, - s_max, - sv_min, - sv_max, - v_switch, - a_max, - v_min, - v_max, -): + +def vehicle_dynamics_st(x: np.ndarray, u_init: np.ndarray, params: dict): """ Single Track Vehicle Dynamics. From https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/-/blob/master/vehicleModels_commonRoad.pdf, section 7 @@ -40,22 +21,23 @@ def vehicle_dynamics_st( u (numpy.ndarray (2, )): control input vector (u1, u2) u1: steering angle velocity of front wheels u2: longitudinal acceleration - mu (float): friction coefficient - C_Sf (float): cornering stiffness of front wheels - C_Sr (float): cornering stiffness of rear wheels - lf (float): distance from center of gravity to front axle - lr (float): distance from center of gravity to rear axle - h (float): height of center of gravity - m (float): mass of vehicle - I (float): moment of inertia of vehicle, about Z axis - s_min (float): minimum steering angle - s_max (float): maximum steering angle - sv_min (float): minimum steering velocity - sv_max (float): maximum steering velocity - v_switch (float): velocity above which the acceleration is no longer able to create wheel spin - a_max (float): maximum allowed acceleration - v_min (float): minimum allowed velocity - v_max (float): maximum allowed velocity + params (dict): dictionary containing the following parameters: + mu (float): friction coefficient + C_Sf (float): cornering stiffness of front wheels + C_Sr (float): cornering stiffness of rear wheels + lf (float): distance from center of gravity to front axle + lr (float): distance from center of gravity to rear axle + h (float): height of center of gravity + m (float): mass of vehicle + I (float): moment of inertia of vehicle, about Z axis + s_min (float): minimum steering angle + s_max (float): maximum steering angle + sv_min (float): minimum steering velocity + sv_max (float): maximum steering velocity + v_switch (float): velocity above which the acceleration is no longer able to create wheel spin + a_max (float): maximum allowed acceleration + v_min (float): minimum allowed velocity + v_max (float): maximum allowed velocity Returns: f (numpy.ndarray): right hand side of differential equations @@ -77,8 +59,22 @@ def vehicle_dynamics_st( # constraints u = np.array( [ - steering_constraint(DELTA, u_init[0], s_min, s_max, sv_min, sv_max), - accl_constraints(V, u_init[1], v_switch, a_max, v_min, v_max), + steering_constraint( + DELTA, + u_init[0], + params["s_min"], + params["s_max"], + params["sv_min"], + params["sv_max"], + ), + accl_constraints( + V, + u_init[1], + params["v_switch"], + params["a_max"], + params["v_min"], + params["v_max"], + ), ] ) # Controls @@ -88,11 +84,11 @@ def vehicle_dynamics_st( # switch to kinematic model for small velocities if V < 0.1: # wheelbase - lwb = lf + lr - BETA_HAT = np.arctan(np.tan(DELTA) * lr / lwb) + lwb = params["lf"] + params["lr"] + BETA_HAT = np.arctan(np.tan(DELTA) * params["lr"] / lwb) BETA_DOT = ( - (1 / (1 + (np.tan(DELTA) * (lr / lwb)) ** 2)) - * (lr / (lwb * np.cos(DELTA) ** 2)) + (1 / (1 + (np.tan(DELTA) * (params["lr"] / lwb)) ** 2)) + * (params["lr"] / (lwb * np.cos(DELTA) ** 2)) * STEER_VEL ) f = np.array( @@ -113,8 +109,8 @@ def vehicle_dynamics_st( ) else: # system dynamics - glr = (g * lr - ACCL * h) - glf = (g * lf + ACCL * h) + glr = g * params["lr"] - ACCL * params["h"] + glf = g * params["lf"] + ACCL * params["h"] f = np.array( [ V * np.cos(PSI + BETA), # X_DOT @@ -122,22 +118,34 @@ def vehicle_dynamics_st( STEER_VEL, # DELTA_DOT ACCL, # V_DOT PSI_DOT, # PSI_DOT - ((mu * m) / (I * (lf + lr))) * ( - lf * C_Sf * (g * lr - ACCL * h) * DELTA - + ( - lr * C_Sr * (g * lf + ACCL * h) - lf * C_Sf * (g * lr - ACCL * h) - ) * BETA - - ( - lf * lf * C_Sf * (g * lr - ACCL * h) + lr * lr * C_Sr * (g * lf + ACCL * h) - ) * (PSI_DOT / V) - ), # PSI_DOT_DOT - (mu / (V * (lr + lf))) * ( - C_Sf * (g * lr - ACCL * h) * DELTA \ - - (C_Sr * (g * lf + ACCL * h) + C_Sf * (g * lr - ACCL * h)) * BETA - + ( - C_Sr * (g * lf + ACCL * h) * lr - C_Sf * (g * lr - ACCL * h) * lf - ) * (PSI_DOT / V) - ) - PSI_DOT, # BETA_DOT + ( + (params["mu"] * params["m"]) + / (params["I"] * (params["lf"] + params["lr"])) + ) + * ( + params["lf"] * params["C_Sf"] * (glr) * DELTA + + ( + params["lr"] * params["C_Sr"] * (glf) + - params["lf"] * params["C_Sf"] * (glr) + ) + * BETA + - ( + params["lf"] * params["lf"] * params["C_Sf"] * (glr) + + params["lr"] * params["lr"] * params["C_Sr"] * (glf) + ) + * (PSI_DOT / V) + ), # PSI_DOT_DOT + (params["mu"] / (V * (params["lr"] + params["lf"]))) + * ( + params["C_Sf"] * (glr) * DELTA + - (params["C_Sr"] * (glf) + params["C_Sf"] * (glr)) * BETA + + ( + params["C_Sr"] * (glf) * params["lr"] + - params["C_Sf"] * (glr) * params["lf"] + ) + * (PSI_DOT / V) + ) + - PSI_DOT, # BETA_DOT ] ) diff --git a/f1tenth_gym/envs/dynamic_models/tire_models.py b/f1tenth_gym/envs/dynamic_models/tire_models.py deleted file mode 100644 index e69de29b..00000000 diff --git a/f1tenth_gym/envs/dynamic_models/utils.py b/f1tenth_gym/envs/dynamic_models/utils.py index 2044e3c5..6115bc77 100644 --- a/f1tenth_gym/envs/dynamic_models/utils.py +++ b/f1tenth_gym/envs/dynamic_models/utils.py @@ -1,6 +1,7 @@ import numpy as np from numba import njit + @njit(cache=True) def upper_accel_limit(vel, a_max, v_switch): """ diff --git a/f1tenth_gym/envs/f110_env.py b/f1tenth_gym/envs/f110_env.py index f6b62916..ba446614 100644 --- a/f1tenth_gym/envs/f110_env.py +++ b/f1tenth_gym/envs/f110_env.py @@ -286,6 +286,41 @@ def fullscale_vehicle_params(cls) -> dict: "E_f": 0, # [needs conversion if nonzero] ER "E_r": 0, + # tire parameters from ADAMS handbook + # longitudinal coefficients + "tire_p_cx1": 1.6411, # Shape factor Cfx for longitudinal force + "tire_p_dx1": 1.1739, # Longitudinal friction Mux at Fznom + "tire_p_dx3": 0, # Variation of friction Mux with camber + "tire_p_ex1": 0.46403, # Longitudinal curvature Efx at Fznom + "tire_p_kx1": 22.303, # Longitudinal slip stiffness Kfx/Fz at Fznom + "tire_p_hx1": 0.0012297, # Horizontal shift Shx at Fznom + "tire_p_vx1": -8.8098e-006, # Vertical shift Svx/Fz at Fznom + "tire_r_bx1": 13.276, # Slope factor for combined slip Fx reduction + "tire_r_bx2": -13.778, # Variation of slope Fx reduction with kappa + "tire_r_cx1": 1.2568, # Shape factor for combined slip Fx reduction + "tire_r_ex1": 0.65225, # Curvature factor of combined Fx + "tire_r_hx1": 0.0050722, # Shift factor for combined slip Fx reduction + # lateral coefficients + "tire_p_cy1": 1.3507, # Shape factor Cfy for lateral forces + "tire_p_dy1": 1.0489, # Lateral friction Muy + "tire_p_dy3": -2.8821, # Variation of friction Muy with squared camber + "tire_p_ey1": -0.0074722, # Lateral curvature Efy at Fznom + "tire_p_ky1": -21.92, # Maximum value of stiffness Kfy/Fznom + "tire_p_hy1": 0.0026747, # Horizontal shift Shy at Fznom + "tire_p_hy3": 0.031415, # Variation of shift Shy with camber + "tire_p_vy1": 0.037318, # Vertical shift in Svy/Fz at Fznom + "tire_p_vy3": -0.32931, # Variation of shift Svy/Fz with camber + "tire_r_by1": 7.1433, # Slope factor for combined Fy reduction + "tire_r_by2": 9.1916, # Variation of slope Fy reduction with alpha + "tire_r_by3": -0.027856, # Shift term for alpha in slope Fy reduction + "tire_r_cy1": 1.0719, # Shape factor for combined Fy reduction + "tire_r_ey1": -0.27572, # Curvature factor of combined Fy + "tire_r_hy1": 5.7448e-006, # Shift factor for combined Fy reduction + "tire_r_vy1": -0.027825, # Kappa induced side force Svyk/Muy*Fz at Fznom + "tire_r_vy3": -0.27568, # Variation of Svyk/Muy*Fz with camber + "tire_r_vy4": 12.12, # Variation of Svyk/Muy*Fz with alpha + "tire_r_vy5": 1.9, # Variation of Svyk/Muy*Fz with kappa + "tire_r_vy6": -10.704, # Variation of Svyk/Muy*Fz with atan(kappa) } return params diff --git a/f1tenth_gym/envs/integrator.py b/f1tenth_gym/envs/integrator.py index bfc796be..0987fbfe 100644 --- a/f1tenth_gym/envs/integrator.py +++ b/f1tenth_gym/envs/integrator.py @@ -35,95 +35,16 @@ def __init__(self) -> None: self._integrator_type = "rk4" def integrate(self, f, x, u, dt, params): - k1 = f( - x, - u, - params["mu"], - params["C_Sf"], - params["C_Sr"], - params["lf"], - params["lr"], - params["h"], - params["m"], - params["I"], - params["s_min"], - params["s_max"], - params["sv_min"], - params["sv_max"], - params["v_switch"], - params["a_max"], - params["v_min"], - params["v_max"], - ) + k1 = f(x, u, params) k2_state = x + dt * (k1 / 2) - - k2 = f( - k2_state, - u, - params["mu"], - params["C_Sf"], - params["C_Sr"], - params["lf"], - params["lr"], - params["h"], - params["m"], - params["I"], - params["s_min"], - params["s_max"], - params["sv_min"], - params["sv_max"], - params["v_switch"], - params["a_max"], - params["v_min"], - params["v_max"], - ) + k2 = f(k2_state, u, params) k3_state = x + dt * (k2 / 2) - - k3 = f( - k3_state, - u, - params["mu"], - params["C_Sf"], - params["C_Sr"], - params["lf"], - params["lr"], - params["h"], - params["m"], - params["I"], - params["s_min"], - params["s_max"], - params["sv_min"], - params["sv_max"], - params["v_switch"], - params["a_max"], - params["v_min"], - params["v_max"], - ) + k3 = f(k3_state, u, params) k4_state = x + dt * k3 - - k4 = f( - k4_state, - u, - params["mu"], - params["C_Sf"], - params["C_Sr"], - params["lf"], - params["lr"], - params["h"], - params["m"], - params["I"], - params["s_min"], - params["s_max"], - params["sv_min"], - params["sv_max"], - params["v_switch"], - params["a_max"], - params["v_min"], - params["v_max"], - ) + k4 = f(k4_state, u, params) # dynamics integration x = x + dt * (1 / 6) * (k1 + 2 * k2 + 2 * k3 + k4) @@ -136,25 +57,6 @@ def __init__(self) -> None: self._integrator_type = "euler" def integrate(self, f, x, u, dt, params): - dstate = f( - x, - u, - params["mu"], - params["C_Sf"], - params["C_Sr"], - params["lf"], - params["lr"], - params["h"], - params["m"], - params["I"], - params["s_min"], - params["s_max"], - params["sv_min"], - params["sv_max"], - params["v_switch"], - params["a_max"], - params["v_min"], - params["v_max"], - ) + dstate = f(x, u, params) x = x + dt * dstate return x