Working MB commit

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

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}")

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

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