diff --git a/gym/f110_gym/__init__.py b/gym/f110_gym/__init__.py index a993eb05..20052799 100644 --- a/gym/f110_gym/__init__.py +++ b/gym/f110_gym/__init__.py @@ -1,5 +1,6 @@ import gymnasium as gym gym.register( - id="f110-v0", entry_point="f110_gym.envs:F110Env", + id="f110-v0", + entry_point="f110_gym.envs:F110Env", ) diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py index 4deffbe9..c9754d50 100644 --- a/gym/f110_gym/envs/base_classes.py +++ b/gym/f110_gym/envs/base_classes.py @@ -26,6 +26,7 @@ Replacement of the old RaceCar, Simulator classes in C++ Author: Hongrui Zheng """ +from __future__ import annotations import numpy as np from f110_gym.envs.dynamic_models import DynamicModel from f110_gym.envs.action import CarAction @@ -407,7 +408,7 @@ def __init__( self.params = params self.agent_poses = np.empty((self.num_agents, 3)) self.agent_steerings = np.empty((self.num_agents,)) - self.agents = [] + self.agents: list[RaceCar] = [] self.collisions = np.zeros((self.num_agents,)) self.collision_idx = -1 * np.ones((self.num_agents,)) self.model = model @@ -512,7 +513,7 @@ def step(self, control_inputs): for i, agent in enumerate(self.agents): # update agent's information on other agents opp_poses = np.concatenate( - (self.agent_poses[0:i, :], self.agent_poses[i + 1:, :]), axis=0 + (self.agent_poses[0:i, :], self.agent_poses[i + 1 :, :]), axis=0 ) agent.update_opp_poses(opp_poses) diff --git a/gym/f110_gym/envs/cubic_spline.py b/gym/f110_gym/envs/cubic_spline.py index 5ed193d6..8b1a0fab 100644 --- a/gym/f110_gym/envs/cubic_spline.py +++ b/gym/f110_gym/envs/cubic_spline.py @@ -64,7 +64,7 @@ def calc_position(self, x): i = self.__search_index(x) dx = x - self.x[i] position = ( - self.a[i] + self.b[i] * dx + self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 + self.a[i] + self.b[i] * dx + self.c[i] * dx**2.0 + self.d[i] * dx**3.0 ) return position @@ -86,7 +86,7 @@ def calc_first_derivative(self, x): i = self.__search_index(x) dx = x - self.x[i] - dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 + dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx**2.0 return dy def calc_second_derivative(self, x): @@ -205,7 +205,7 @@ def calc_curvature(self, s): ddx = self.sx.calc_second_derivative(s) dy = self.sy.calc_first_derivative(s) ddy = self.sy.calc_second_derivative(s) - k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2) ** (3 / 2)) + k = (ddy * dx - ddx * dy) / ((dx**2 + dy**2) ** (3 / 2)) return k def calc_yaw(self, s): diff --git a/gym/f110_gym/envs/dynamic_models.py b/gym/f110_gym/envs/dynamic_models.py index b4a436d1..248360e2 100644 --- a/gym/f110_gym/envs/dynamic_models.py +++ b/gym/f110_gym/envs/dynamic_models.py @@ -24,7 +24,7 @@ Track dynamic model Following the implementation of commanroad's Single Track Dynamics model Original implementation: https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/ -Author: Hongrui Zheng +Author: Hongrui Zheng, Renukanandan Tumu """ import warnings from enum import Enum @@ -78,15 +78,36 @@ def f_dynamics(self): @njit(cache=True) -def accl_constraints(vel, accl, v_switch, a_max, v_min, v_max): +def upper_accel_limit(vel, a_max, v_switch): + """ + Upper acceleration limit, adjusts the acceleration based on constraints + + Args: + vel (float): current velocity of the vehicle + a_max (float): maximum allowed acceleration, symmetrical + v_switch (float): switching velocity (velocity at which the acceleration is no longer able to create wheel spin) + + Returns: + positive_accel_limit (float): adjusted acceleration + """ + if vel > v_switch: + pos_limit = a_max * (v_switch / vel) + else: + pos_limit = a_max + + return pos_limit + + +@njit(cache=True) +def accl_constraints(vel, a_long_d, v_switch, a_max, v_min, v_max): """ Acceleration constraints, adjusts the acceleration based on constraints Args: vel (float): current velocity of the vehicle - accl (float): unconstraint desired acceleration + a_long_d (float): unconstrained desired acceleration in the direction of travel. v_switch (float): switching velocity (velocity at which the acceleration is no longer able to create wheel spin) - a_max (float): maximum allowed acceleration + a_max (float): maximum allowed acceleration, symmetrical v_min (float): minimum allowed velocity v_max (float): maximum allowed velocity @@ -94,21 +115,18 @@ def accl_constraints(vel, accl, v_switch, a_max, v_min, v_max): accl (float): adjusted acceleration """ - # positive accl limit - if vel > v_switch: - pos_limit = a_max * v_switch / vel - else: - pos_limit = a_max + uac = upper_accel_limit(vel, a_max, v_switch) - # accl limit reached? - if (vel <= v_min and accl <= 0) or (vel >= v_max and accl >= 0): - accl = 0.0 - elif accl <= -a_max: - accl = -a_max - elif accl >= pos_limit: - accl = pos_limit + if (vel <= v_min and a_long_d <= 0) or (vel >= v_max and a_long_d >= 0): + a_long = 0.0 + elif a_long_d <= -a_max: + a_long = -a_max + elif a_long_d >= uac: + a_long = uac + else: + a_long = a_long_d - return accl + return a_long @njit(cache=True) @@ -166,103 +184,69 @@ def vehicle_dynamics_ks( ): """ Single Track Kinematic Vehicle Dynamics. + Follows https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/-/blob/master/vehicleModels_commonRoad.pdf, section 5 Args: - x (numpy.ndarray (3, )): vehicle state vector (x1, x2, x3, x4, x5) - x1: x position in global coordinates - x2: y position in global coordinates - x3: steering angle of front wheels - x4: velocity in x direction - x5: yaw angle - u (numpy.ndarray (2, )): control input vector (u1, u2) - u1: steering angle velocity of front wheels - u2: longitudinal acceleration - - Returns: - f (numpy.ndarray): right hand side of differential equations - """ - # wheelbase - lwb = lf + lr - - # constraints - 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), - ] - ) - - # system dynamics - f = np.array( - [ - x[3] * np.cos(x[4]), - x[3] * np.sin(x[4]), - u[0], - u[1], - x[3] / lwb * np.tan(x[2]), - ] - ) - return f - - -@njit(cache=True) -def vehicle_dynamics_ks_cog( - 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, -): - """ - Single Track Kinematic Vehicle Dynamics at CoG. - - Args: - x (numpy.ndarray (3, )): vehicle state vector (x1, x2, x3, x4, x5) - x1: x position in global coordinates - x2: y position in global coordinates - x3: steering angle of front wheels - x4: velocity in x direction - x5: yaw angle + x (numpy.ndarray (5, )): vehicle state vector (x0, x1, x2, x3, x4) + x0: x position in global coordinates + x1: y position in global coordinates + x2: steering angle of front wheels + x3: velocity in x direction + x4: yaw angle 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 Returns: f (numpy.ndarray): right hand side of differential equations """ + # Controls + X = x[0] + Y = x[1] + DELTA = x[2] + V = x[3] + PSI = x[4] + # Raw Actions + RAW_STEER_VEL = u_init[0] + RAW_ACCL = u_init[1] # wheelbase lwb = lf + lr # constraints 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), + 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), ] ) + # Corrected Actions + STEER_VEL = u[0] + ACCL = u[1] # system dynamics - beta = np.arctan(np.tan(x[2]) * lr / lwb) f = np.array( [ - x[3] * np.cos(beta + x[4]), - x[3] * np.sin(beta + x[4]), - u[0], - u[1], - x[3] * np.cos(beta) * np.tan(x[2]) / lwb, + V * np.cos(PSI), # X_DOT + V * np.sin(PSI), # Y_DOT + STEER_VEL, # DELTA_DOT + ACCL, # V_DOT + (V / lwb) * np.tan(DELTA), # PSI_DOT ] ) return f @@ -290,24 +274,51 @@ def vehicle_dynamics_st( v_max, ): """ - Single Track Dynamic Vehicle Dynamics. + Single Track Vehicle Dynamics. + From https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/-/blob/master/vehicleModels_commonRoad.pdf, section 7 Args: - x (numpy.ndarray (3, )): vehicle state vector (x1, x2, x3, x4, x5, x6, x7) - x1: x position in global coordinates - x2: y position in global coordinates - x3: steering angle of front wheels - x4: velocity in x direction - x5: yaw angle - x6: yaw rate - x7: slip angle at vehicle center + x (numpy.ndarray (7, )): vehicle state vector (x0, x1, x2, x3, x4, x5, x6) + x0: x position in global coordinates + x1: y position in global coordinates + x2: steering angle of front wheels + x3: velocity in x direction + x4:yaw angle + x5: yaw rate + x6: slip angle at vehicle center 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 Returns: f (numpy.ndarray): right hand side of differential equations """ + # States + X = x[0] + Y = x[1] + DELTA = x[2] + V = x[3] + PSI = x[4] + PSI_DOT = x[5] + BETA = x[6] + # We have to wrap the slip angle to [-pi, pi] + # BETA = np.arctan2(np.sin(BETA), np.cos(BETA)) # gravity constant m/s^2 g = 9.81 @@ -315,90 +326,74 @@ def vehicle_dynamics_st( # constraints 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), + 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), ] ) + # Controls + STEER_VEL = u[0] + ACCL = u[1] # switch to kinematic model for small velocities - if abs(x[3]) < 0.5: + if V < 0.5: # wheelbase lwb = lf + lr - - # system dynamics - x_ks = x[0:5] - f_ks = vehicle_dynamics_ks_cog( - x_ks, - u, - 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, + BETA_HAT = np.arctan(np.tan(DELTA) * lr / lwb) + BETA_DOT = ( + (1 / (1 + (np.tan(DELTA) * (lr / lwb)) ** 2)) + * (lr / (lwb * np.cos(DELTA) ** 2)) + * STEER_VEL ) - 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 = np.array( + [ + V * np.cos(PSI + BETA_HAT), # X_DOT + V * np.sin(PSI + BETA_HAT), # Y_DOT + STEER_VEL, # DELTA_DOT + ACCL, # V_DOT + V * np.cos(BETA_HAT) * np.tan(DELTA) / lwb, # PSI_DOT + (1 / lwb) + * ( + ACCL * np.cos(BETA) * np.tan(DELTA) + - V * np.sin(BETA) * np.tan(DELTA) * BETA_DOT + + ((V * np.cos(BETA) * STEER_VEL) / (np.cos(DELTA) ** 2)) + ), # PSI_DOT_DOT + BETA_DOT, # BETA_DOT + ] ) - f = np.hstack((f_ks, np.array([dd_psi, d_beta,]),)) - else: # system dynamics f = np.array( [ - x[3] * np.cos(x[6] + x[4]), - x[3] * np.sin(x[6] + x[4]), - u[0], - u[1], - x[5], - -mu - * m - / (x[3] * I * (lr + lf)) + V * np.cos(PSI + BETA), # X_DOT + V * np.sin(PSI + BETA), # Y_DOT + STEER_VEL, # DELTA_DOT + ACCL, # V_DOT + PSI_DOT, # PSI_DOT + ((mu * m) / (I * (lf + lr))) * ( - lf ** 2 * C_Sf * (g * lr - u[1] * h) - + lr ** 2 * C_Sr * (g * lf + u[1] * h) - ) - * x[5] - + mu - * m - / (I * (lr + lf)) - * (lr * C_Sr * (g * lf + u[1] * h) - lf * C_Sf * (g * lr - u[1] * h)) - * x[6] - + mu * m / (I * (lr + lf)) * lf * C_Sf * (g * lr - u[1] * h) * x[2], - ( - mu - / (x[3] ** 2 * (lr + lf)) - * ( - C_Sr * (g * lf + u[1] * h) * lr - - C_Sf * (g * lr - u[1] * h) * lf + 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 ) - - 1 + * (PSI_DOT / V) ) - * x[5] - - mu - / (x[3] * (lr + lf)) - * (C_Sr * (g * lf + u[1] * h) + C_Sf * (g * lr - u[1] * h)) - * x[6] - + mu / (x[3] * (lr + lf)) * (C_Sf * (g * lr - u[1] * h)) * x[2], + - PSI_DOT, # BETA_DOT ] ) diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py index 51e7c54f..71d048b4 100644 --- a/gym/f110_gym/envs/f110_env.py +++ b/gym/f110_gym/envs/f110_env.py @@ -169,13 +169,15 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs): # match render_fps to integration timestep self.metadata["render_fps"] = int(1.0 / self.timestep) if self.render_mode == "human_fast": - self.metadata["render_fps"] *= 10 # boost fps by 10x + self.metadata["render_fps"] *= 10 # boost fps by 10x self.renderer, self.render_spec = make_renderer( - params=self.params, track=self.track, agent_ids=self.agent_ids, - render_mode=render_mode, render_fps=self.metadata["render_fps"] + params=self.params, + track=self.track, + agent_ids=self.agent_ids, + render_mode=render_mode, + render_fps=self.metadata["render_fps"], ) - @classmethod def default_config(cls) -> dict: """ @@ -264,7 +266,7 @@ def _check_done(self): temp_y[idx2] = -right_t - temp_y[idx2] temp_y[np.invert(np.logical_or(idx1, idx2))] = 0 - dist2 = delta_pt[0, :] ** 2 + temp_y ** 2 + dist2 = delta_pt[0, :] ** 2 + temp_y**2 closes = dist2 <= 0.1 for i in range(self.num_agents): if closes[i] and not self.near_starts[i]: diff --git a/gym/f110_gym/envs/f110_env_backup.py b/gym/f110_gym/envs/f110_env_backup.py index e0acc38d..b488492d 100644 --- a/gym/f110_gym/envs/f110_env_backup.py +++ b/gym/f110_gym/envs/f110_env_backup.py @@ -31,9 +31,11 @@ # others import numpy as np + # protobuf import import sim_requests_pb2 import yaml + # zmq imports import zmq from PIL import Image @@ -261,7 +263,7 @@ def _check_done(self): temp_y[idx2] = -right_t - temp_y[idx2] temp_y[np.invert(np.logical_or(idx1, idx2))] = 0 - dist2 = delta_pt[0, :] ** 2 + temp_y ** 2 + dist2 = delta_pt[0, :] ** 2 + temp_y**2 closes = dist2 <= 0.1 for i in range(self.num_agents): if closes[i] and not self.near_starts[i]: @@ -289,7 +291,7 @@ def _check_done(self): temp_y = -right_t - delta_pt[1] else: temp_y = 0 - dist2 = delta_pt[0] ** 2 + temp_y ** 2 + dist2 = delta_pt[0] ** 2 + temp_y**2 close = dist2 <= 0.1 # close = dist_to_start <= self.start_thresh if close and not self.near_start: diff --git a/gym/f110_gym/envs/integrator.py b/gym/f110_gym/envs/integrator.py index 57eb9cb5..f708aa82 100644 --- a/gym/f110_gym/envs/integrator.py +++ b/gym/f110_gym/envs/integrator.py @@ -10,9 +10,6 @@ class IntegratorType(Enum): @staticmethod def from_string(integrator: str): if integrator == "rk4": - warnings.warn( - "Chosen integrator is RK4. This is different from previous versions of the gym." - ) return RK4Integrator() elif integrator == "euler": return EulerIntegrator() diff --git a/gym/f110_gym/test/benchmark_renderer.py b/gym/f110_gym/test/benchmark_renderer.py index 90a1634b..c32ebd32 100644 --- a/gym/f110_gym/test/benchmark_renderer.py +++ b/gym/f110_gym/test/benchmark_renderer.py @@ -55,7 +55,11 @@ def _make_env(config={}, render_mode=None) -> F110Env: } config = deep_update(base_config, config) - env = gym.make("f110_gym:f110-v0", config=config, render_mode=render_mode,) + env = gym.make( + "f110_gym:f110-v0", + config=config, + render_mode=render_mode, + ) return env diff --git a/gym/f110_gym/test/test_renderer.py b/gym/f110_gym/test/test_renderer.py index f1645608..a9069684 100644 --- a/gym/f110_gym/test/test_renderer.py +++ b/gym/f110_gym/test/test_renderer.py @@ -24,7 +24,11 @@ def _make_env(config={}, render_mode=None) -> F110Env: } config = deep_update(base_config, config) - env = gym.make("f110_gym:f110-v0", config=config, render_mode=render_mode,) + env = gym.make( + "f110_gym:f110-v0", + config=config, + render_mode=render_mode, + ) return env diff --git a/setup.py b/setup.py index 778b4d5e..8d5d5b50 100644 --- a/setup.py +++ b/setup.py @@ -22,14 +22,14 @@ "opencv-python", ], extras_require={ - 'dev': [ - 'pytest', - 'flake8', - 'black', - 'ipykernel', - 'isort', - 'autoflake', - 'matplotlib' + "dev": [ + "pytest", + "flake8", + "black", + "ipykernel", + "isort", + "autoflake", + "matplotlib", ] - } + }, ) diff --git a/tests/test_collision_checks.py b/tests/test_collision_checks.py index 88e4b4fc..2608d2c1 100644 --- a/tests/test_collision_checks.py +++ b/tests/test_collision_checks.py @@ -82,4 +82,3 @@ def test_fps(self): fps = 1000 / elapsed print("gjk fps:", fps) # self.assertGreater(fps, 500) This is a platform dependent test, not ideal. - diff --git a/tests/test_dynamics.py b/tests/test_dynamics.py index 03ddfc1e..3a910165 100644 --- a/tests/test_dynamics.py +++ b/tests/test_dynamics.py @@ -23,7 +23,12 @@ import unittest import numpy as np -from f110_gym.envs.dynamic_models import vehicle_dynamics_ks, vehicle_dynamics_st, func_KS, func_ST +from f110_gym.envs.dynamic_models import ( + vehicle_dynamics_ks, + vehicle_dynamics_st, + func_KS, + func_ST, +) class DynamicsTest(unittest.TestCase): @@ -553,7 +558,7 @@ def test_zeroinit_rollleft_singletrack(self): 0.0000000000000000, 0.0000000000000000, 0.0000000000000000, - 0.0000000000000000, + 0.0833661500000000, ] np.testing.assert_array_almost_equal(x_left_st[-1], x_left_st_gt, decimal=2) diff --git a/tests/test_f110_env.py b/tests/test_f110_env.py index faccb97e..a0ed29ca 100644 --- a/tests/test_f110_env.py +++ b/tests/test_f110_env.py @@ -7,7 +7,6 @@ class TestEnvInterface(unittest.TestCase): def _make_env(self, config={}): - conf = { "map": "Example", "num_agents": 1, @@ -18,7 +17,10 @@ def _make_env(self, config={}): } conf = deep_update(conf, config) - env = gym.make("f110_gym:f110-v0", config=conf,) + env = gym.make( + "f110_gym:f110-v0", + config=conf, + ) return env def test_gymnasium_api(self): diff --git a/tests/test_observation.py b/tests/test_observation.py index d88b345b..5874a461 100644 --- a/tests/test_observation.py +++ b/tests/test_observation.py @@ -11,7 +11,6 @@ class TestObservationInterface(unittest.TestCase): @staticmethod def _make_env(config={}) -> F110Env: - conf = { "map": "Example", "num_agents": 1, @@ -228,5 +227,7 @@ def test_gymnasium_api(self): for obs_type_id in obs_type_ids: env = self._make_env(config={"observation_config": {"type": obs_type_id}}) check_env( - env.unwrapped, f"Observation {obs_type_id} breaks the gymnasium API", skip_render_check=True + env.unwrapped, + f"Observation {obs_type_id} breaks the gymnasium API", + skip_render_check=True, )