Bugfix: Add missing params, change I to I_z, revert tire params to full-scale

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

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

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

f1tenth_gym/envs/dynamic_models/single_track_drift.py → f1tenth_gym/envs/dynamic_models/single_track_drift/single_track_drift.py

@@ -1,18 +1,22 @@
 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):
     """
     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
@@ -159,14 +171,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
         )
     )
@@ -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
 

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