From 6de3c1e0e6582e1e4e6f0abcab9ae543bcc955a6 Mon Sep 17 00:00:00 2001 From: Teodor Ilie Date: Tue, 14 Oct 2025 14:35:04 -0400 Subject: [PATCH 1/2] add missing params, change I to I_z, revert tire params to full-scale --- .../envs/dynamic_models/single_track.py | 2 +- .../envs/dynamic_models/single_track_drift.py | 6 +- f1tenth_gym/envs/f110_env.py | 68 ++++++++++--------- 3 files changed, 39 insertions(+), 37 deletions(-) diff --git a/f1tenth_gym/envs/dynamic_models/single_track.py b/f1tenth_gym/envs/dynamic_models/single_track.py index 7a1e3516..e9e27880 100644 --- a/f1tenth_gym/envs/dynamic_models/single_track.py +++ b/f1tenth_gym/envs/dynamic_models/single_track.py @@ -121,7 +121,7 @@ def vehicle_dynamics_st(x: np.ndarray, u_init: np.ndarray, params: dict): PSI_DOT, # PSI_DOT ( (params["mu"] * params["m"]) - / (params["I"] * (params["lf"] + params["lr"])) + / (params["I_z"] * (params["lf"] + params["lr"])) ) * ( params["lf"] * params["C_Sf"] * (glr) * DELTA diff --git a/f1tenth_gym/envs/dynamic_models/single_track_drift.py b/f1tenth_gym/envs/dynamic_models/single_track_drift.py index 2d64cf59..8b8649d4 100644 --- a/f1tenth_gym/envs/dynamic_models/single_track_drift.py +++ b/f1tenth_gym/envs/dynamic_models/single_track_drift.py @@ -159,14 +159,14 @@ def vehicle_dynamics_std(x: np.ndarray, u_init: np.ndarray, params: dict): f_ks = vehicle_dynamics_ks_cog(np.array(x_ks), u, params) # derivative of slip angle and yaw rate (kinematic) d_beta_ks = (params["lr"] * STEER_VEL) / ( - lwb * np.cos(DELTA) ** 2 * (1 + (np.arctan(DELTA) ** 2 * params["lr"] / lwb) ** 2) + lwb * np.cos(DELTA) ** 2 * (1 + (np.tan(DELTA) ** 2 * params["lr"] / lwb) ** 2) ) dd_psi_ks = ( 1 / lwb * ( - ACCL * np.cos(BETA) * np.arctan(DELTA) - - V * np.sin(BETA) * d_beta_ks * np.arctan(DELTA) + ACCL * np.cos(BETA) * np.tan(DELTA) + - V * np.sin(BETA) * d_beta_ks * np.tan(DELTA) + V * np.cos(BETA) * STEER_VEL / np.cos(DELTA) ** 2 ) ) diff --git a/f1tenth_gym/envs/f110_env.py b/f1tenth_gym/envs/f110_env.py index 2e52bc64..d1844c23 100644 --- a/f1tenth_gym/envs/f110_env.py +++ b/f1tenth_gym/envs/f110_env.py @@ -360,7 +360,7 @@ def f1tenth_vehicle_params(cls) -> dict: "lr": 0.17145, "h": 0.074, "m": 3.74, - "I": 0.04712, + "I_z": 0.04712, "s_min": -0.4189, "s_max": 0.4189, "sv_min": -3.2, @@ -374,40 +374,42 @@ def f1tenth_vehicle_params(cls) -> dict: "R_w": 0.049, # effective tire radius [m] "T_sb": 0.5, # torque split of brakes (percent of torque sent to front axle) [no units] "T_se": 0.5, # torque split of engine (percent of torque sent to front axle) [no units] + "I_y_w": 0.00017, # wheel inertia in [kg m^2] + "h_s": 0.074, # height of center of gravity [m] # longitudinal coefficients - "tire_p_cx1": 1.3, - "tire_p_dx1": 9.99, - "tire_p_dx3": 0, - "tire_p_ex1": 0.46403, - "tire_p_kx1": 99.9, - "tire_p_hx1": 0, - "tire_p_vx1": -8.809800e-06, - "tire_r_bx1": 13.276, - "tire_r_bx2": -13.778, - "tire_r_cx1": 1.2568, - "tire_r_ex1": 0.65225, - "tire_r_hx1": 0.005072, + "tire_p_cx1": 1.6411, + "tire_p_dx1": 1.1739, + "tire_p_dx3": 0, + "tire_p_ex1": 0.46403, + "tire_p_kx1": 22.303, + "tire_p_hx1": 0.0012297, + "tire_p_vx1": -8.8098e-006, + "tire_r_bx1": 13.276, + "tire_r_bx2": -13.778, + "tire_r_cx1": 1.2568, + "tire_r_ex1": 0.65225, + "tire_r_hx1": 0.0050722, # lateral coefficients - "tire_p_cy1": 1.329112, - "tire_p_dy1": 818.578752, - "tire_p_dy3": -2.8821, - "tire_p_ey1": -0.007472, - "tire_p_ky1": 4700.458358, - "tire_p_hy1": -0.000146, - "tire_p_hy3": 0.031415, - "tire_p_vy1": 0.037318, - "tire_p_vy3": -0.32931, - "tire_r_by1": 7.1433, - "tire_r_by2": 9.1916, - "tire_r_by3": -0.027856, - "tire_r_cy1": 1.0719, - "tire_r_ey1": -0.27572, - "tire_r_hy1": 5.744800e-06, - "tire_r_vy1": -0.027825, - "tire_r_vy3": -0.27568, - "tire_r_vy4": 12.12, - "tire_r_vy5": 1.9, - "tire_r_vy6": -10.704 + "tire_p_cy1": 1.3507, + "tire_p_dy1": 1.0489, + "tire_p_dy3": -2.8821, + "tire_p_ey1": -0.0074722, + "tire_p_ky1": -21.92, + "tire_p_hy1": 0.0026747, + "tire_p_hy3": 0.031415, + "tire_p_vy1": 0.037318, + "tire_p_vy3": -0.32931, + "tire_r_by1": 7.1433, + "tire_r_by2": 9.1916, + "tire_r_by3": -0.027856, + "tire_r_cy1": 1.0719, + "tire_r_ey1": -0.27572, + "tire_r_hy1": 5.7448e-006, + "tire_r_vy1": -0.027825, + "tire_r_vy3": -0.27568, + "tire_r_vy4": 12.12, + "tire_r_vy5": 1.9, + "tire_r_vy6": -10.704, } return params From 822dd309b46b3d038e73c68fb1c26dcea3a5389a Mon Sep 17 00:00:00 2001 From: TeoIlie Date: Tue, 21 Oct 2025 09:42:26 -0400 Subject: [PATCH 2/2] added init_std method, changed std model to packag similar to mb --- f1tenth_gym/envs/dynamic_models/__init__.py | 5 +- .../single_track_drift/__init__.py | 67 +++++++++++++++++++ .../single_track_drift.py | 40 +++++++---- 3 files changed, 97 insertions(+), 15 deletions(-) create mode 100644 f1tenth_gym/envs/dynamic_models/single_track_drift/__init__.py rename f1tenth_gym/envs/dynamic_models/{ => single_track_drift}/single_track_drift.py (87%) diff --git a/f1tenth_gym/envs/dynamic_models/__init__.py b/f1tenth_gym/envs/dynamic_models/__init__.py index d7ff85d4..00ee6cb0 100644 --- a/f1tenth_gym/envs/dynamic_models/__init__.py +++ b/f1tenth_gym/envs/dynamic_models/__init__.py @@ -9,7 +9,7 @@ from .kinematic import vehicle_dynamics_ks, get_standardized_state_ks from .single_track import vehicle_dynamics_st, get_standardized_state_st -from .single_track_drift import vehicle_dynamics_std, get_standardized_state_std +from .single_track_drift import init_std, vehicle_dynamics_std, get_standardized_state_std from .multi_body import init_mb, vehicle_dynamics_mb, get_standardized_state_mb from .utils import pid_steer, pid_accl from typing import Optional @@ -65,6 +65,9 @@ def get_initial_state(self, pose=None, params: Optional[dict] = None): # If state is MultiBody, we must inflate the state to 29D if self == DynamicModel.MB: state = init_mb(state, params) + # If state is SingleTrackDrift, we must inflate to 9D + elif self == DynamicModel.STD: + state = init_std(state, params) return state @property diff --git a/f1tenth_gym/envs/dynamic_models/single_track_drift/__init__.py b/f1tenth_gym/envs/dynamic_models/single_track_drift/__init__.py new file mode 100644 index 00000000..b2824089 --- /dev/null +++ b/f1tenth_gym/envs/dynamic_models/single_track_drift/__init__.py @@ -0,0 +1,67 @@ +""" +Single-track drift model initialization function +""" +import numpy as np +from numba import njit + +from .single_track_drift import vehicle_dynamics_std, get_standardized_state_std + + +def init_std(init_state, params: dict) -> np.ndarray: + """ + init_std generates the initial state vector for the drift single track model + + Syntax: + x0 = init_std(init_state, p) + + Inputs: + :param init_state: core initial states + :param p: parameter vector + + Outputs: + :return x0: initial state vector + + Author: Teodor Ilie + Written: 18-October-2025 + """ + x0 = np.zeros((9,)) + + # Steering, vel constraints + + delta0 = init_state[2] # steering angle of front wheels + vel0 = init_state[3] # speed of the car + + ## 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] + + 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 + + # Copy first 7 states as-is with constraints on steering, velocity + x0[0] = init_state[0] + x0[1] = init_state[1] + x0[2] = delta0 + x0[3] = vel0 + x0[4] = init_state[4] + x0[5] = init_state[5] + x0[6] = init_state[6] + + # Additional 2 states calculations + x0[7] = x0[3] * np.cos(x0[6]) * np.cos(x0[2]) / params["R_w"] # init front wheel angular speed + x0[8] = x0[3] * np.cos(x0[6]) / params["R_w"] # init rear wheel angular speed + + return x0 diff --git a/f1tenth_gym/envs/dynamic_models/single_track_drift.py b/f1tenth_gym/envs/dynamic_models/single_track_drift/single_track_drift.py similarity index 87% rename from f1tenth_gym/envs/dynamic_models/single_track_drift.py rename to f1tenth_gym/envs/dynamic_models/single_track_drift/single_track_drift.py index 8b8649d4..dfe5a162 100644 --- a/f1tenth_gym/envs/dynamic_models/single_track_drift.py +++ b/f1tenth_gym/envs/dynamic_models/single_track_drift/single_track_drift.py @@ -1,9 +1,10 @@ import numpy as np from numba import njit +from numba.typed import Dict -from .utils import steering_constraint, accl_constraints -from .tire_model import formula_longitudinal, formula_lateral, formula_longitudinal_comb, formula_lateral_comb -from .kinematic import vehicle_dynamics_ks_cog +from ..utils import steering_constraint, accl_constraints +from ..tire_model import formula_longitudinal, formula_lateral, formula_longitudinal_comb, formula_lateral_comb +from ..kinematic import vehicle_dynamics_ks_cog def vehicle_dynamics_std(x: np.ndarray, u_init: np.ndarray, params: dict): @@ -11,8 +12,11 @@ def vehicle_dynamics_std(x: np.ndarray, u_init: np.ndarray, params: dict): Single Track Drift model. From: https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/-/blob/master/PYTHON/vehiclemodels/vehicle_dynamics_std.py?ref_type=heads - Args: - x (numpy.ndarray (9,)): vehicle state vector (x0, x1, x2, x3, x4, x5, x6, x7, x8) + Syntax: + f = vehicle_dynamics_std(x,u,p) + + Inputs: + :param x: (numpy.ndarray (9,)): vehicle state vector (x0, x1, x2, x3, x4, x5, x6, x7, x8) x[0]: x position in global coordinates x[1]: y position in global coordinates x[2]: steering angle of front wheels @@ -22,14 +26,16 @@ def vehicle_dynamics_std(x: np.ndarray, u_init: np.ndarray, params: dict): x[6]: slip angle at vehicle center x[7]: angular speed of the front wheel x[8]: angular speed of the rear wheel - u_init (numpy.ndarray (2,)): control input vector (u1, u2) + :param u_init: (numpy.ndarray (2,)): control input vector (u1, u2) u_init[0]: steering angle velocity of front wheels u_init[1]: longitudinal acceleration - params (dict): dictionary containing necessary parameters: - see f110_env.py:f1tenth_std_vehicle_params + :param params: (dict): dictionary containing necessary parameters: - Returns: - f (numpy.ndarray): right hand side of differential equations + Outputs: + :return f: (numpy.ndarray): right hand side of differential equations + + Author: Teodor Ilie + Written: 18-October-2025 """ # Get states from state vector X = x[0] @@ -125,9 +131,15 @@ def vehicle_dynamics_std(x: np.ndarray, u_init: np.ndarray, params: dict): # system dynamics d_v = ( - 1 / params["m"] * (-F_yf * np.sin(DELTA - BETA) + F_yr * np.sin(BETA) + F_xr * np.cos(BETA) + F_xf * np.cos(DELTA - BETA)) + 1 + / params["m"] + * (-F_yf * np.sin(DELTA - BETA) + F_yr * np.sin(BETA) + F_xr * np.cos(BETA) + F_xf * np.cos(DELTA - BETA)) + ) + dd_psi = ( + 1 + / params["I_z"] + * (F_yf * np.cos(DELTA) * params["lf"] - F_yr * params["lr"] + F_xf * np.sin(DELTA) * params["lf"]) ) - dd_psi = 1 / params["I_z"] * (F_yf * np.cos(DELTA) * params["lf"] - F_yr * params["lr"] + F_xf * np.sin(DELTA) * params["lf"]) d_beta = ( -PSI_DOT + 1 @@ -179,7 +191,7 @@ def vehicle_dynamics_std(x: np.ndarray, u_init: np.ndarray, params: dict): w_ks = 1 - w_std # output vector: mix results of dynamic and kinematic model - f = np.array ( + f = np.array( [ V * np.cos(BETA + PSI), V * np.sin(BETA + PSI), @@ -192,7 +204,7 @@ def vehicle_dynamics_std(x: np.ndarray, u_init: np.ndarray, params: dict): w_std * d_omega_r + w_ks * d_omega_r_ks, ] ) - + # return f, the time derivatives of the input state x return f