From 3ab2dc244f761a50c8eaec696463f52959a1b1d9 Mon Sep 17 00:00:00 2001
From: Hongrui Zheng <billyzheng.bz@gmail.com>
Date: Tue, 20 Jun 2023 16:15:46 -0400
Subject: [PATCH 01/42] add python gitignore

---
 .gitignore | 161 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 161 insertions(+)

diff --git a/.gitignore b/.gitignore
index c91fe84a..3641cd67 100644
--- a/.gitignore
+++ b/.gitignore
@@ -9,3 +9,164 @@ gym/f110_gym/unittest/centerline/
 docs/_build/
 *.mypy_cache/
 flycheck_*
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# poetry
+#   Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
+#   This is especially recommended for binary packages to ensure reproducibility, and is more
+#   commonly ignored for libraries.
+#   https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
+#poetry.lock
+
+# pdm
+#   Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
+#pdm.lock
+#   pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
+#   in version control.
+#   https://pdm.fming.dev/#use-with-ide
+.pdm.toml
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+venv*
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+# PyCharm
+#  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
+#  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
+#  and can be added to the global gitignore or merged into this file.  For a more nuclear
+#  option (not recommended) you can uncomment the following to ignore the entire idea folder.
+#.idea/
\ No newline at end of file

From 632d529968cfaf338af2a92f9eb459cc70d7d528 Mon Sep 17 00:00:00 2001
From: Hongrui Zheng <billyzheng.bz@gmail.com>
Date: Tue, 20 Jun 2023 16:17:21 -0400
Subject: [PATCH 02/42] change env config to dict, linting, update example

---
 examples/waypoint_follow.py       | 149 ++++++++---
 gym/f110_gym/envs/base_classes.py | 405 ++++++++++++++++++------------
 gym/f110_gym/envs/f110_env.py     | 341 ++++++++++++++++---------
 3 files changed, 573 insertions(+), 322 deletions(-)

diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py
index 71a40cbf..9a70c998 100644
--- a/examples/waypoint_follow.py
+++ b/examples/waypoint_follow.py
@@ -46,11 +46,18 @@ def nearest_point_on_trajectory(point, trajectory):
         temp = point - projections[i]
         dists[i] = np.sqrt(np.sum(temp * temp))
     min_dist_segment = np.argmin(dists)
-    return projections[min_dist_segment], dists[min_dist_segment], t[min_dist_segment], min_dist_segment
+    return (
+        projections[min_dist_segment],
+        dists[min_dist_segment],
+        t[min_dist_segment],
+        min_dist_segment,
+    )
 
 
 @njit(fastmath=False, cache=True)
-def first_point_on_trajectory_intersecting_circle(point, radius, trajectory, t=0.0, wrap=False):
+def first_point_on_trajectory_intersecting_circle(
+    point, radius, trajectory, t=0.0, wrap=False
+):
     """
     starts at beginning of trajectory, and find the first point one radius away from the given point along the trajectory.
 
@@ -67,11 +74,20 @@ def first_point_on_trajectory_intersecting_circle(point, radius, trajectory, t=0
     for i in range(start_i, trajectory.shape[0] - 1):
         start = trajectory[i, :]
         end = trajectory[i + 1, :] + 1e-6
-        V = np.ascontiguousarray(end - start).astype(np.float32)  # NOTE: specify type or numba complains
+        V = np.ascontiguousarray(end - start).astype(
+            np.float32
+        )  # NOTE: specify type or numba complains
 
         a = np.dot(V, V)
-        b = np.float32(2.0) * np.dot(V, start - point)  # NOTE: specify type or numba complains
-        c = np.dot(start, start) + np.dot(point, point) - np.float32(2.0) * np.dot(start, point) - radius * radius
+        b = np.float32(2.0) * np.dot(
+            V, start - point
+        )  # NOTE: specify type or numba complains
+        c = (
+            np.dot(start, start)
+            + np.dot(point, point)
+            - np.float32(2.0) * np.dot(start, point)
+            - radius * radius
+        )
         discriminant = b * b - 4 * a * c
 
         if discriminant < 0:
@@ -111,8 +127,15 @@ def first_point_on_trajectory_intersecting_circle(point, radius, trajectory, t=0
             V = (end - start).astype(np.float32)
 
             a = np.dot(V, V)
-            b = np.float32(2.0) * np.dot(V, start - point)  # NOTE: specify type or numba complains
-            c = np.dot(start, start) + np.dot(point, point) - np.float32(2.0) * np.dot(start, point) - radius * radius
+            b = np.float32(2.0) * np.dot(
+                V, start - point
+            )  # NOTE: specify type or numba complains
+            c = (
+                np.dot(start, start)
+                + np.dot(point, point)
+                - np.float32(2.0) * np.dot(start, point)
+                - radius * radius
+            )
             discriminant = b * b - 4 * a * c
 
             if discriminant < 0:
@@ -139,12 +162,14 @@ def get_actuation(pose_theta, lookahead_point, position, lookahead_distance, whe
     """
     Returns actuation
     """
-    waypoint_y = np.dot(np.array([np.sin(-pose_theta), np.cos(-pose_theta)], dtype=np.float32),
-                        lookahead_point[0:2] - position)
+    waypoint_y = np.dot(
+        np.array([np.sin(-pose_theta), np.cos(-pose_theta)], dtype=np.float32),
+        lookahead_point[0:2] - position,
+    )
     speed = lookahead_point[2]
     if np.abs(waypoint_y) < 1e-6:
-        return speed, 0.
-    radius = 1 / (2.0 * waypoint_y / lookahead_distance ** 2)
+        return speed, 0.0
+    radius = 1 / (2.0 * waypoint_y / lookahead_distance**2)
     steering_angle = np.arctan(wheelbase / radius)
     return speed, steering_angle
 
@@ -158,7 +183,7 @@ def __init__(self, conf, wb):
         self.wheelbase = wb
         self.conf = conf
         self.load_waypoints(conf)
-        self.max_reacquire = 20.
+        self.max_reacquire = 20.0
 
         self.drawn_waypoints = []
 
@@ -167,8 +192,9 @@ def load_waypoints(self, conf):
         loads waypoints
         """
         # NOTE: specify type or numba complains
-        self.waypoints = np.loadtxt(conf.wpt_path, delimiter=conf.wpt_delim,
-                                    skiprows=conf.wpt_rowskip).astype(np.float32)
+        self.waypoints = np.loadtxt(
+            conf.wpt_path, delimiter=conf.wpt_delim, skiprows=conf.wpt_rowskip
+        ).astype(np.float32)
 
     def render_waypoints(self, e):
         """
@@ -177,29 +203,49 @@ def render_waypoints(self, e):
 
         # points = self.waypoints
 
-        points = np.vstack((self.waypoints[:, self.conf.wpt_xind], self.waypoints[:, self.conf.wpt_yind])).T
+        points = np.vstack(
+            (
+                self.waypoints[:, self.conf.wpt_xind],
+                self.waypoints[:, self.conf.wpt_yind],
+            )
+        ).T
 
-        scaled_points = 50. * points
+        scaled_points = 50.0 * points
 
         for i in range(points.shape[0]):
             if len(self.drawn_waypoints) < points.shape[0]:
-                b = e.batch.add(1, GL_POINTS, None, ('v3f/stream', [scaled_points[i, 0], scaled_points[i, 1], 0.]),
-                                ('c3B/stream', [183, 193, 222]))
+                b = e.batch.add(
+                    1,
+                    GL_POINTS,
+                    None,
+                    ("v3f/stream", [scaled_points[i, 0], scaled_points[i, 1], 0.0]),
+                    ("c3B/stream", [183, 193, 222]),
+                )
                 self.drawn_waypoints.append(b)
             else:
-                self.drawn_waypoints[i].vertices = [scaled_points[i, 0], scaled_points[i, 1], 0.]
+                self.drawn_waypoints[i].vertices = [
+                    scaled_points[i, 0],
+                    scaled_points[i, 1],
+                    0.0,
+                ]
 
     def _get_current_waypoint(self, waypoints, lookahead_distance, position, theta):
         """
         gets the current waypoint to follow
         """
-        wpts = np.vstack((self.waypoints[:, self.conf.wpt_xind], self.waypoints[:, self.conf.wpt_yind])).T
+        wpts = np.vstack(
+            (
+                self.waypoints[:, self.conf.wpt_xind],
+                self.waypoints[:, self.conf.wpt_yind],
+            )
+        ).T
         lookahead_distance = np.float32(lookahead_distance)
         nearest_point, nearest_dist, t, i = nearest_point_on_trajectory(position, wpts)
         if nearest_dist < lookahead_distance:
             t1 = np.float32(i + t)
-            lookahead_point, i2, t2 = first_point_on_trajectory_intersecting_circle(position, lookahead_distance, wpts,
-                                                                                    t1, wrap=True)
+            lookahead_point, i2, t2 = first_point_on_trajectory_intersecting_circle(
+                position, lookahead_distance, wpts, t1, wrap=True
+            )
             if i2 == None:
                 return None
             current_waypoint = np.empty((3,), dtype=np.float32)
@@ -210,7 +256,9 @@ def _get_current_waypoint(self, waypoints, lookahead_distance, position, theta):
             return current_waypoint
         elif nearest_dist < self.max_reacquire:
             # NOTE: specify type or numba complains
-            return np.append(wpts[i, :], waypoints[i, self.conf.wpt_vind]).astype(np.float32)
+            return np.append(wpts[i, :], waypoints[i, self.conf.wpt_vind]).astype(
+                np.float32
+            )
         else:
             return None
 
@@ -219,12 +267,16 @@ def plan(self, pose_x, pose_y, pose_theta, lookahead_distance, vgain):
         gives actuation given observation
         """
         position = np.array([pose_x, pose_y])
-        lookahead_point = self._get_current_waypoint(self.waypoints, lookahead_distance, position, pose_theta)
+        lookahead_point = self._get_current_waypoint(
+            self.waypoints, lookahead_distance, position, pose_theta
+        )
 
         if lookahead_point is None:
             return 4.0, 0.0
 
-        speed, steering_angle = get_actuation(pose_theta, lookahead_point, position, lookahead_distance, self.wheelbase)
+        speed, steering_angle = get_actuation(
+            pose_theta, lookahead_point, position, lookahead_distance, self.wheelbase
+        )
         speed = vgain * speed
 
         return speed, steering_angle
@@ -257,14 +309,20 @@ def main():
     main entry point
     """
 
-    work = {'mass': 3.463388126201571, 'lf': 0.15597534362552312, 'tlad': 0.82461887897713965,
-            'vgain': 1.375}  # 0.90338203837889}
+    work = {
+        "mass": 3.463388126201571,
+        "lf": 0.15597534362552312,
+        "tlad": 0.82461887897713965,
+        "vgain": 1.375,
+    }  # 0.90338203837889}
 
-    with open('config_example_map.yaml') as file:
+    with open("config_example_map.yaml") as file:
         conf_dict = yaml.load(file, Loader=yaml.FullLoader)
     conf = Namespace(**conf_dict)
 
-    planner = PurePursuitPlanner(conf, (0.17145 + 0.15875))  # FlippyPlanner(speed=0.2, flip_every=1, steer=10)
+    planner = PurePursuitPlanner(
+        conf, (0.17145 + 0.15875)
+    )  # FlippyPlanner(speed=0.2, flip_every=1, steer=10)
 
     def render_callback(env_renderer):
         # custom extra drawing function
@@ -284,9 +342,23 @@ def render_callback(env_renderer):
 
         planner.render_waypoints(env_renderer)
 
-    env = gym.make('f110_gym:f110-v0', map=conf.map_path, map_ext=conf.map_ext,
-                   num_agents=1, timestep=0.01, integrator=Integrator.RK4,
-                   render_mode='human')
+    # old API
+    # env = gym.make('f110_gym:f110-v0', map=conf.map_path, map_ext=conf.map_ext,
+    #                num_agents=1, timestep=0.01, integrator=Integrator.RK4,
+    #                render_mode='human')
+
+    # new API
+    env = gym.make(
+        "f110_gym:f110-v0",
+        config={
+            "map": conf.map_path,
+            "map_ext": conf.map_ext,
+            "num_agents": 1,
+            "timestep": 0.01,
+            "integrator": "rk4",
+        },
+        render_mode="human",
+    )
     env.add_render_callback(render_callback)
 
     poses = np.array([[conf.sx, conf.sy, conf.stheta]])
@@ -298,14 +370,19 @@ def render_callback(env_renderer):
     start = time.time()
 
     while not done:
-        speed, steer = planner.plan(obs['poses_x'][0], obs['poses_y'][0], obs['poses_theta'][0], work['tlad'],
-                                    work['vgain'])
+        speed, steer = planner.plan(
+            obs["poses_x"][0],
+            obs["poses_y"][0],
+            obs["poses_theta"][0],
+            work["tlad"],
+            work["vgain"],
+        )
         obs, step_reward, done, truncated, info = env.step(np.array([[steer, speed]]))
         laptime += step_reward
         env.render()
 
-    print('Sim elapsed time:', laptime, 'Real elapsed time:', time.time() - start)
+    print("Sim elapsed time:", laptime, "Real elapsed time:", time.time() - start)
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()
diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index 192e32eb..6284a314 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -21,7 +21,6 @@
 # SOFTWARE.
 
 
-
 """
 Prototype of base classes
 Replacement of the old RaceCar, Simulator classes in C++
@@ -37,6 +36,7 @@
 from f110_gym.envs.laser_models import ScanSimulator2D, check_ttc_jit, ray_cast
 from f110_gym.envs.collision_models import get_vertices, collision_multiple
 
+
 class Integrator(Enum):
     RK4 = 1
     Euler = 2
@@ -66,7 +66,17 @@ class RaceCar(object):
     scan_angles = None
     side_distances = None
 
-    def __init__(self, params, seed, is_ego=False, time_step=0.01, num_beams=1080, fov=4.7, integrator=Integrator.Euler):
+    def __init__(
+        self,
+        params,
+        seed,
+        is_ego=False,
+        time_step=0.01,
+        num_beams=1080,
+        fov=4.7,
+        integrator=Integrator.Euler,
+        control_input="speed",
+    ):
         """
         Init function
 
@@ -90,10 +100,13 @@ def __init__(self, params, seed, is_ego=False, time_step=0.01, num_beams=1080, f
         self.fov = fov
         self.integrator = integrator
         if self.integrator is Integrator.RK4:
-            warnings.warn(f"Chosen integrator is RK4. This is different from previous versions of the gym.")
+            warnings.warn(
+                f"Chosen integrator is RK4. This is different from previous versions of the gym."
+            )
+        self.control_input = control_input
 
         # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
-        self.state = np.zeros((7, ))
+        self.state = np.zeros((7,))
 
         # pose of opponents in the world
         self.opp_poses = None
@@ -103,7 +116,7 @@ def __init__(self, params, seed, is_ego=False, time_step=0.01, num_beams=1080, f
         self.steer_angle_vel = 0.0
 
         # steering delay buffer
-        self.steer_buffer = np.empty((0, ))
+        self.steer_buffer = np.empty((0,))
         self.steer_buffer_size = 2
 
         # collision identifier
@@ -120,39 +133,39 @@ def __init__(self, params, seed, is_ego=False, time_step=0.01, num_beams=1080, f
             scan_ang_incr = RaceCar.scan_simulator.get_increment()
 
             # angles of each scan beam, distance from lidar to edge of car at each beam, and precomputed cosines of each angle
-            RaceCar.cosines = np.zeros((num_beams, ))
-            RaceCar.scan_angles = np.zeros((num_beams, ))
-            RaceCar.side_distances = np.zeros((num_beams, ))
+            RaceCar.cosines = np.zeros((num_beams,))
+            RaceCar.scan_angles = np.zeros((num_beams,))
+            RaceCar.side_distances = np.zeros((num_beams,))
 
-            dist_sides = params['width']/2.
-            dist_fr = (params['lf']+params['lr'])/2.
+            dist_sides = params["width"] / 2.0
+            dist_fr = (params["lf"] + params["lr"]) / 2.0
 
             for i in range(num_beams):
-                angle = -fov/2. + i*scan_ang_incr
+                angle = -fov / 2.0 + i * scan_ang_incr
                 RaceCar.scan_angles[i] = angle
                 RaceCar.cosines[i] = np.cos(angle)
 
                 if angle > 0:
-                    if angle < np.pi/2:
+                    if angle < np.pi / 2:
                         # between 0 and pi/2
                         to_side = dist_sides / np.sin(angle)
                         to_fr = dist_fr / np.cos(angle)
                         RaceCar.side_distances[i] = min(to_side, to_fr)
                     else:
                         # between pi/2 and pi
-                        to_side = dist_sides / np.cos(angle - np.pi/2.)
-                        to_fr = dist_fr / np.sin(angle - np.pi/2.)
+                        to_side = dist_sides / np.cos(angle - np.pi / 2.0)
+                        to_fr = dist_fr / np.sin(angle - np.pi / 2.0)
                         RaceCar.side_distances[i] = min(to_side, to_fr)
                 else:
-                    if angle > -np.pi/2:
+                    if angle > -np.pi / 2:
                         # between 0 and -pi/2
                         to_side = dist_sides / np.sin(-angle)
                         to_fr = dist_fr / np.cos(-angle)
                         RaceCar.side_distances[i] = min(to_side, to_fr)
                     else:
                         # between -pi/2 and -pi
-                        to_side = dist_sides / np.cos(-angle - np.pi/2)
-                        to_fr = dist_fr / np.sin(-angle - np.pi/2)
+                        to_side = dist_sides / np.cos(-angle - np.pi / 2)
+                        to_fr = dist_fr / np.sin(-angle - np.pi / 2)
                         RaceCar.side_distances[i] = min(to_side, to_fr)
 
     def update_params(self, params):
@@ -167,11 +180,11 @@ def update_params(self, params):
             None
         """
         self.params = params
-    
+
     def set_map(self, map_path, map_ext):
         """
         Sets the map for scan simulator
-        
+
         Args:
             map_path (str): absolute path to the map yaml file
             map_ext (str): extension of the map image file
@@ -181,7 +194,7 @@ def set_map(self, map_path, map_ext):
     def reset(self, pose):
         """
         Resets the vehicle to a pose
-        
+
         Args:
             pose (np.ndarray (3, )): pose to reset the vehicle to
 
@@ -194,10 +207,10 @@ def reset(self, pose):
         # clear collision indicator
         self.in_collision = False
         # clear state
-        self.state = np.zeros((7, ))
+        self.state = np.zeros((7,))
         self.state[0:2] = pose[0:2]
         self.state[4] = pose[2]
-        self.steer_buffer = np.empty((0, ))
+        self.steer_buffer = np.empty((0,))
         # reset scan random generator
         self.scan_rng = np.random.default_rng(seed=self.seed)
 
@@ -214,13 +227,20 @@ def ray_cast_agents(self, scan):
 
         # starting from original scan
         new_scan = scan
-        
+
         # loop over all opponent vehicle poses
         for opp_pose in self.opp_poses:
             # get vertices of current oppoenent
-            opp_vertices = get_vertices(opp_pose, self.params['length'], self.params['width'])
+            opp_vertices = get_vertices(
+                opp_pose, self.params["length"], self.params["width"]
+            )
 
-            new_scan = ray_cast(np.append(self.state[0:2], self.state[4]), new_scan, self.scan_angles, opp_vertices)
+            new_scan = ray_cast(
+                np.append(self.state[0:2], self.state[4]),
+                new_scan,
+                self.scan_angles,
+                opp_vertices,
+            )
 
         return new_scan
 
@@ -237,12 +257,19 @@ def check_ttc(self, current_scan):
         Returns:
             None
         """
-        
-        in_collision = check_ttc_jit(current_scan, self.state[3], self.scan_angles, self.cosines, self.side_distances, self.ttc_thresh)
+
+        in_collision = check_ttc_jit(
+            current_scan,
+            self.state[3],
+            self.scan_angles,
+            self.cosines,
+            self.side_distances,
+            self.ttc_thresh,
+        )
 
         # if in collision stop vehicle
         if in_collision:
-            self.state[3:] = 0.
+            self.state[3:] = 0.0
             self.accel = 0.0
             self.steer_angle_vel = 0.0
 
@@ -256,8 +283,8 @@ def update_pose(self, raw_steer, vel):
         Steps the vehicle's physical simulation
 
         Args:
-            steer (float): desired steering angle
-            vel (float): desired longitudinal velocity
+            steer (float): desired steering angle, or desired steering velocity
+            vel (float): desired longitudinal velocity, or desired longitudinal acceleration
 
         Returns:
             current_scan
@@ -266,143 +293,169 @@ def update_pose(self, raw_steer, vel):
         # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
 
         # steering delay
-        steer = 0.
+        steer = 0.0
         if self.steer_buffer.shape[0] < self.steer_buffer_size:
-            steer = 0.
+            steer = 0.0
             self.steer_buffer = np.append(raw_steer, self.steer_buffer)
         else:
             steer = self.steer_buffer[-1]
             self.steer_buffer = self.steer_buffer[:-1]
             self.steer_buffer = np.append(raw_steer, self.steer_buffer)
 
+        if self.control_input == "speed":
+            # steering angle velocity input to steering velocity acceleration input
+            accl, sv = pid(
+                vel,
+                steer,
+                self.state[3],
+                self.state[2],
+                self.params["sv_max"],
+                self.params["a_max"],
+                self.params["v_max"],
+                self.params["v_min"],
+            )
+        elif self.control_input == "acceleration":
+            accl, sv = vel, steer
+        else:
+            raise SyntaxError(
+                f"Invalid Control Input Type Specified. Provided {self.control_input}. Please choose speed or acceleration"
+            )
 
-        # steering angle velocity input to steering velocity acceleration input
-        accl, sv = pid(vel, steer, self.state[3], self.state[2], self.params['sv_max'], self.params['a_max'], self.params['v_max'], self.params['v_min'])
-        
         if self.integrator is Integrator.RK4:
             # RK4 integration
             k1 = vehicle_dynamics_st(
                 self.state,
                 np.array([sv, accl]),
-                self.params['mu'],
-                self.params['C_Sf'],
-                self.params['C_Sr'],
-                self.params['lf'],
-                self.params['lr'],
-                self.params['h'],
-                self.params['m'],
-                self.params['I'],
-                self.params['s_min'],
-                self.params['s_max'],
-                self.params['sv_min'],
-                self.params['sv_max'],
-                self.params['v_switch'],
-                self.params['a_max'],
-                self.params['v_min'],
-                self.params['v_max'])
-
-            k2_state = self.state + self.time_step*(k1/2)
+                self.params["mu"],
+                self.params["C_Sf"],
+                self.params["C_Sr"],
+                self.params["lf"],
+                self.params["lr"],
+                self.params["h"],
+                self.params["m"],
+                self.params["I"],
+                self.params["s_min"],
+                self.params["s_max"],
+                self.params["sv_min"],
+                self.params["sv_max"],
+                self.params["v_switch"],
+                self.params["a_max"],
+                self.params["v_min"],
+                self.params["v_max"],
+            )
+
+            k2_state = self.state + self.time_step * (k1 / 2)
 
             k2 = vehicle_dynamics_st(
                 k2_state,
                 np.array([sv, accl]),
-                self.params['mu'],
-                self.params['C_Sf'],
-                self.params['C_Sr'],
-                self.params['lf'],
-                self.params['lr'],
-                self.params['h'],
-                self.params['m'],
-                self.params['I'],
-                self.params['s_min'],
-                self.params['s_max'],
-                self.params['sv_min'],
-                self.params['sv_max'],
-                self.params['v_switch'],
-                self.params['a_max'],
-                self.params['v_min'],
-                self.params['v_max'])
-
-            k3_state = self.state + self.time_step*(k2/2)
+                self.params["mu"],
+                self.params["C_Sf"],
+                self.params["C_Sr"],
+                self.params["lf"],
+                self.params["lr"],
+                self.params["h"],
+                self.params["m"],
+                self.params["I"],
+                self.params["s_min"],
+                self.params["s_max"],
+                self.params["sv_min"],
+                self.params["sv_max"],
+                self.params["v_switch"],
+                self.params["a_max"],
+                self.params["v_min"],
+                self.params["v_max"],
+            )
+
+            k3_state = self.state + self.time_step * (k2 / 2)
 
             k3 = vehicle_dynamics_st(
                 k3_state,
                 np.array([sv, accl]),
-                self.params['mu'],
-                self.params['C_Sf'],
-                self.params['C_Sr'],
-                self.params['lf'],
-                self.params['lr'],
-                self.params['h'],
-                self.params['m'],
-                self.params['I'],
-                self.params['s_min'],
-                self.params['s_max'],
-                self.params['sv_min'],
-                self.params['sv_max'],
-                self.params['v_switch'],
-                self.params['a_max'],
-                self.params['v_min'],
-                self.params['v_max'])
-
-            k4_state = self.state + self.time_step*k3
+                self.params["mu"],
+                self.params["C_Sf"],
+                self.params["C_Sr"],
+                self.params["lf"],
+                self.params["lr"],
+                self.params["h"],
+                self.params["m"],
+                self.params["I"],
+                self.params["s_min"],
+                self.params["s_max"],
+                self.params["sv_min"],
+                self.params["sv_max"],
+                self.params["v_switch"],
+                self.params["a_max"],
+                self.params["v_min"],
+                self.params["v_max"],
+            )
+
+            k4_state = self.state + self.time_step * k3
 
             k4 = vehicle_dynamics_st(
                 k4_state,
                 np.array([sv, accl]),
-                self.params['mu'],
-                self.params['C_Sf'],
-                self.params['C_Sr'],
-                self.params['lf'],
-                self.params['lr'],
-                self.params['h'],
-                self.params['m'],
-                self.params['I'],
-                self.params['s_min'],
-                self.params['s_max'],
-                self.params['sv_min'],
-                self.params['sv_max'],
-                self.params['v_switch'],
-                self.params['a_max'],
-                self.params['v_min'],
-                self.params['v_max'])
+                self.params["mu"],
+                self.params["C_Sf"],
+                self.params["C_Sr"],
+                self.params["lf"],
+                self.params["lr"],
+                self.params["h"],
+                self.params["m"],
+                self.params["I"],
+                self.params["s_min"],
+                self.params["s_max"],
+                self.params["sv_min"],
+                self.params["sv_max"],
+                self.params["v_switch"],
+                self.params["a_max"],
+                self.params["v_min"],
+                self.params["v_max"],
+            )
 
             # dynamics integration
-            self.state = self.state + self.time_step*(1/6)*(k1 + 2*k2 + 2*k3 + k4)
-        
+            self.state = self.state + self.time_step * (1 / 6) * (
+                k1 + 2 * k2 + 2 * k3 + k4
+            )
+
         elif self.integrator is Integrator.Euler:
             f = vehicle_dynamics_st(
                 self.state,
                 np.array([sv, accl]),
-                self.params['mu'],
-                self.params['C_Sf'],
-                self.params['C_Sr'],
-                self.params['lf'],
-                self.params['lr'],
-                self.params['h'],
-                self.params['m'],
-                self.params['I'],
-                self.params['s_min'],
-                self.params['s_max'],
-                self.params['sv_min'],
-                self.params['sv_max'],
-                self.params['v_switch'],
-                self.params['a_max'],
-                self.params['v_min'],
-                self.params['v_max'])
+                self.params["mu"],
+                self.params["C_Sf"],
+                self.params["C_Sr"],
+                self.params["lf"],
+                self.params["lr"],
+                self.params["h"],
+                self.params["m"],
+                self.params["I"],
+                self.params["s_min"],
+                self.params["s_max"],
+                self.params["sv_min"],
+                self.params["sv_max"],
+                self.params["v_switch"],
+                self.params["a_max"],
+                self.params["v_min"],
+                self.params["v_max"],
+            )
             self.state = self.state + self.time_step * f
-        
+
         else:
-            raise SyntaxError(f"Invalid Integrator Specified. Provided {self.integrator.name}. Please choose RK4 or Euler")
+            raise SyntaxError(
+                f"Invalid Integrator Specified. Provided {self.integrator.name}. Please choose RK4 or Euler"
+            )
 
         # bound yaw angle
-        if self.state[4] > 2*np.pi:
-            self.state[4] = self.state[4] - 2*np.pi
+        if self.state[4] > 2 * np.pi:
+            self.state[4] = self.state[4] - 2 * np.pi
         elif self.state[4] < 0:
-            self.state[4] = self.state[4] + 2*np.pi
+            self.state[4] = self.state[4] + 2 * np.pi
 
         # update scan
-        current_scan = RaceCar.scan_simulator.scan(np.append(self.state[0:2], self.state[4]), self.scan_rng)
+        current_scan = RaceCar.scan_simulator.scan(
+            np.append(self.state[0:2], self.state[4]), self.scan_rng
+        )
 
         return current_scan
 
@@ -418,7 +471,6 @@ def update_opp_poses(self, opp_poses):
         """
         self.opp_poses = opp_poses
 
-
     def update_scan(self, agent_scans, agent_index):
         """
         Steps the vehicle's laser scan simulation
@@ -442,6 +494,7 @@ def update_scan(self, agent_scans, agent_index):
 
         agent_scans[agent_index] = new_scan
 
+
 class Simulator(object):
     """
     Simulator class, handles the interaction and update of all vehicles in the environment
@@ -456,7 +509,16 @@ class Simulator(object):
 
     """
 
-    def __init__(self, params, num_agents, seed, time_step=0.01, ego_idx=0, integrator=Integrator.RK4):
+    def __init__(
+        self,
+        params,
+        num_agents,
+        seed,
+        time_step=0.01,
+        ego_idx=0,
+        integrator=Integrator.RK4,
+        control_input="speed",
+    ):
         """
         Init function
 
@@ -477,16 +539,30 @@ def __init__(self, params, num_agents, seed, time_step=0.01, ego_idx=0, integrat
         self.params = params
         self.agent_poses = np.empty((self.num_agents, 3))
         self.agents = []
-        self.collisions = np.zeros((self.num_agents, ))
-        self.collision_idx = -1 * np.ones((self.num_agents, ))
+        self.collisions = np.zeros((self.num_agents,))
+        self.collision_idx = -1 * np.ones((self.num_agents,))
 
         # initializing agents
         for i in range(self.num_agents):
             if i == ego_idx:
-                ego_car = RaceCar(params, self.seed, is_ego=True, time_step=self.time_step, integrator=integrator)
+                ego_car = RaceCar(
+                    params,
+                    self.seed,
+                    is_ego=True,
+                    time_step=self.time_step,
+                    integrator=integrator,
+                    control_input=control_input,
+                )
                 self.agents.append(ego_car)
             else:
-                agent = RaceCar(params, self.seed, is_ego=False, time_step=self.time_step, integrator=integrator)
+                agent = RaceCar(
+                    params,
+                    self.seed,
+                    is_ego=False,
+                    time_step=self.time_step,
+                    integrator=integrator,
+                    control_input=control_input,
+                )
                 self.agents.append(agent)
 
     def set_map(self, map_path, map_ext):
@@ -503,7 +579,6 @@ def set_map(self, map_path, map_ext):
         for agent in self.agents:
             agent.set_map(map_path, map_ext)
 
-
     def update_params(self, params, agent_idx=-1):
         """
         Updates the params of agents, if an index of an agent is given, update only that agent's params
@@ -524,7 +599,7 @@ def update_params(self, params, agent_idx=-1):
             self.agents[agent_idx].update_params(params)
         else:
             # index out of bounds, throw error
-            raise IndexError('Index given is out of bounds for list of agents.')
+            raise IndexError("Index given is out of bounds for list of agents.")
 
     def check_collision(self):
         """
@@ -539,22 +614,24 @@ def check_collision(self):
         # get vertices of all agents
         all_vertices = np.empty((self.num_agents, 4, 2))
         for i in range(self.num_agents):
-            all_vertices[i, :, :] = get_vertices(np.append(self.agents[i].state[0:2],self.agents[i].state[4]), self.params['length'], self.params['width'])
+            all_vertices[i, :, :] = get_vertices(
+                np.append(self.agents[i].state[0:2], self.agents[i].state[4]),
+                self.params["length"],
+                self.params["width"],
+            )
         self.collisions, self.collision_idx = collision_multiple(all_vertices)
 
-
     def step(self, control_inputs):
         """
         Steps the simulation environment
 
         Args:
             control_inputs (np.ndarray (num_agents, 2)): control inputs of all agents, first column is desired steering angle, second column is desired velocity
-        
+
         Returns:
             observations (dict): dictionary for observations: poses of agents, current laser scan of each agent, collision indicators, etc.
         """
 
-
         agent_scans = []
 
         # looping over agents
@@ -571,7 +648,9 @@ 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)
+            opp_poses = np.concatenate(
+                (self.agent_poses[0:i, :], self.agent_poses[i + 1 :, :]), axis=0
+            )
             agent.update_opp_poses(opp_poses)
 
             # update each agent's current scan based on other agents
@@ -579,28 +658,30 @@ def step(self, control_inputs):
 
             # update agent collision with environment
             if agent.in_collision:
-                self.collisions[i] = 1.
+                self.collisions[i] = 1.0
 
         # fill in observations
         # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
         # collision_angles is removed from observations
-        observations = {'ego_idx': self.ego_idx,
-            'scans': [],
-            'poses_x': [],
-            'poses_y': [],
-            'poses_theta': [],
-            'linear_vels_x': [],
-            'linear_vels_y': [],
-            'ang_vels_z': [],
-            'collisions': self.collisions}
+        observations = {
+            "ego_idx": self.ego_idx,
+            "scans": [],
+            "poses_x": [],
+            "poses_y": [],
+            "poses_theta": [],
+            "linear_vels_x": [],
+            "linear_vels_y": [],
+            "ang_vels_z": [],
+            "collisions": self.collisions,
+        }
         for i, agent in enumerate(self.agents):
-            observations['scans'].append(agent_scans[i])
-            observations['poses_x'].append(agent.state[0])
-            observations['poses_y'].append(agent.state[1])
-            observations['poses_theta'].append(agent.state[4])
-            observations['linear_vels_x'].append(agent.state[3])
-            observations['linear_vels_y'].append(0.)
-            observations['ang_vels_z'].append(agent.state[5])
+            observations["scans"].append(agent_scans[i])
+            observations["poses_x"].append(agent.state[0])
+            observations["poses_y"].append(agent.state[1])
+            observations["poses_theta"].append(agent.state[4])
+            observations["linear_vels_x"].append(agent.state[3])
+            observations["linear_vels_y"].append(0.0)
+            observations["ang_vels_z"].append(agent.state[5])
 
         return observations
 
@@ -614,9 +695,11 @@ def reset(self, poses):
         Returns:
             None
         """
-        
+
         if poses.shape[0] != self.num_agents:
-            raise ValueError('Number of poses for reset does not match number of agents.')
+            raise ValueError(
+                "Number of poses for reset does not match number of agents."
+            )
 
         # loop over poses to reset
         for i in range(self.num_agents):
diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 66b728c3..3b044791 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -20,9 +20,9 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 
-'''
+"""
 Author: Hongrui Zheng
-'''
+"""
 
 # gym imports
 import gymnasium as gym
@@ -38,32 +38,36 @@
 # gl
 import pyglet
 
-pyglet.options['debug_gl'] = False
+pyglet.options["debug_gl"] = False
 from pyglet import gl
 
-# constants
-
 # rendering
 VIDEO_W = 600
 VIDEO_H = 400
 WINDOW_W = 1000
 WINDOW_H = 800
 
+# config map
+CONFIG_MAP = {
+    "rk4": Integrator.RK4,
+    "euler": Integrator.Euler,
+}
+
 
 class F110Env(gym.Env):
     """
     OpenAI gym environment for F1TENTH
-    
+
     Env should be initialized by calling gym.make('f110_gym:f110-v0', **kwargs)
 
     Args:
         kwargs:
             seed (int, default=12345): seed for random state and reproducibility
-            
+
             map (str, default='vegas'): name of the map used for the environment. Currently, available environments include: 'berlin', 'vegas', 'skirk'. You could use a string of the absolute path to the yaml file of your custom map.
-        
+
             map_ext (str, default='png'): image extension of the map image file. For example 'png', 'pgm'
-        
+
             params (dict, default={'mu': 1.0489, 'C_Sf':, 'C_Sr':, 'lf': 0.15875, 'lr': 0.17145, 'h': 0.074, 'm': 3.74, 'I': 0.04712, 's_min': -0.4189, 's_max': 0.4189, 'sv_min': -3.2, 'sv_max': 3.2, 'v_switch':7.319, 'a_max': 9.51, 'v_min':-5.0, 'v_max': 20.0, 'width': 0.31, 'length': 0.58}): dictionary of vehicle parameters.
             mu: surface friction coefficient
             C_Sf: Cornering stiffness coefficient, front
@@ -90,71 +94,32 @@ class F110Env(gym.Env):
 
             ego_idx (int, default=0): ego's index in list of agents
     """
+
     # NOTE: change matadata with default rendering-modes, add definition of render_fps
-    metadata = {'render_modes': ['human', 'human_fast', 'rgb_array'], 'render_fps': 100}
+    metadata = {"render_modes": ["human", "human_fast", "rgb_array"], "render_fps": 100}
 
     # rendering
     renderer = None
     current_obs = None
     render_callbacks = []
 
-    def __init__(self, render_mode=None, **kwargs):
-        # NOTE: change signature, add render_mode
-
-        # kwargs extraction
-        try:
-            self.seed = kwargs['seed']
-        except:
-            self.seed = 12345
-        try:
-            self.map_name = kwargs['map']
-            # different default maps
-            if self.map_name == 'berlin':
-                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/berlin.yaml'
-            elif self.map_name == 'skirk':
-                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/skirk.yaml'
-            elif self.map_name == 'levine':
-                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/levine.yaml'
-            else:
-                self.map_path = self.map_name + '.yaml'
-        except:
-            self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/vegas.yaml'
-
-        try:
-            self.map_ext = kwargs['map_ext']
-        except:
-            self.map_ext = '.png'
-
-        try:
-            self.params = kwargs['params']
-        except:
-            self.params = {'mu': 1.0489, 'C_Sf': 4.718, 'C_Sr': 5.4562, 'lf': 0.15875, 'lr': 0.17145, 'h': 0.074,
-                           'm': 3.74, 'I': 0.04712, 's_min': -0.4189, 's_max': 0.4189, 'sv_min': -3.2, 'sv_max': 3.2,
-                           'v_switch': 7.319, 'a_max': 9.51, 'v_min': -5.0, 'v_max': 20.0, 'width': 0.31,
-                           'length': 0.58}
-
-        # simulation parameters
-        try:
-            self.num_agents = kwargs['num_agents']
-        except:
-            self.num_agents = 2
-
-        try:
-            self.timestep = kwargs['timestep']
-        except:
-            self.timestep = 0.01
-
-        # default ego index
-        try:
-            self.ego_idx = kwargs['ego_idx']
-        except:
-            self.ego_idx = 0
-
-        # default integrator
-        try:
-            self.integrator = kwargs['integrator']
-        except:
-            self.integrator = Integrator.RK4
+    def __init__(self, config: dict = None, render_mode=None, **kwargs):
+        super().__init__()
+
+        # Configuration
+        self.config = self.default_config()
+        self.configure(config)
+
+        self.seed = self.config["seed"]
+        self.map_name = self.config["map"]
+        self.map_path = self.map_name + ".yaml"
+        self.map_ext = self.config["map_ext"]
+        self.params = self.config["params"]
+        self.num_agents = self.config["num_agents"]
+        self.timestep = self.config["timestep"]
+        self.ego_idx = self.config["ego_idx"]
+        self.integrator = CONFIG_MAP[self.config["integrator"]]
+        self.control_input = self.config["control_input"]
 
         # radius to consider done
         self.start_thresh = 0.5  # 10cm
@@ -187,37 +152,97 @@ def __init__(self, render_mode=None, **kwargs):
         self.start_rot = np.eye(2)
 
         # initiate stuff
-        self.sim = Simulator(self.params, self.num_agents, self.seed, time_step=self.timestep,
-                             integrator=self.integrator)
+        self.sim = Simulator(
+            self.params,
+            self.num_agents,
+            self.seed,
+            time_step=self.timestep,
+            integrator=self.integrator,
+            control_input=self.control_input,
+        )
         self.sim.set_map(self.map_path, self.map_ext)
 
         # observation space
         # NOTE: keep original structure of observation space (dict). just define it as a dict space and define bounds
-        scan_size, scan_range = self.sim.agents[0].scan_simulator.num_beams, self.sim.agents[0].scan_simulator.max_range
+        scan_size, scan_range = (
+            self.sim.agents[0].scan_simulator.num_beams,
+            self.sim.agents[0].scan_simulator.max_range,
+        )
         large_num = 1e30  # large number to avoid unbounded obs space (ie., low=-inf or high=inf)
-        self.observation_space = gym.spaces.Dict({
-            'ego_idx': gym.spaces.Discrete(self.num_agents),
-            'scans': gym.spaces.Box(low=0.0, high=scan_range, shape=(self.num_agents, scan_size), dtype=np.float32),
-            'poses_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'poses_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'poses_theta': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'linear_vels_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'linear_vels_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'ang_vels_z': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'collisions': gym.spaces.Box(low=0.0, high=1.0, shape=(self.num_agents,), dtype=np.float32),
-            'lap_times': gym.spaces.Box(low=0.0, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'lap_counts': gym.spaces.Box(low=0.0, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-        })
+        self.observation_space = gym.spaces.Dict(
+            {
+                "ego_idx": gym.spaces.Discrete(self.num_agents),
+                "scans": gym.spaces.Box(
+                    low=0.0,
+                    high=scan_range,
+                    shape=(self.num_agents, scan_size),
+                    dtype=np.float32,
+                ),
+                "poses_x": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(self.num_agents,),
+                    dtype=np.float32,
+                ),
+                "poses_y": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(self.num_agents,),
+                    dtype=np.float32,
+                ),
+                "poses_theta": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(self.num_agents,),
+                    dtype=np.float32,
+                ),
+                "linear_vels_x": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(self.num_agents,),
+                    dtype=np.float32,
+                ),
+                "linear_vels_y": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(self.num_agents,),
+                    dtype=np.float32,
+                ),
+                "ang_vels_z": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(self.num_agents,),
+                    dtype=np.float32,
+                ),
+                "collisions": gym.spaces.Box(
+                    low=0.0, high=1.0, shape=(self.num_agents,), dtype=np.float32
+                ),
+                "lap_times": gym.spaces.Box(
+                    low=0.0, high=large_num, shape=(self.num_agents,), dtype=np.float32
+                ),
+                "lap_counts": gym.spaces.Box(
+                    low=0.0, high=large_num, shape=(self.num_agents,), dtype=np.float32
+                ),
+            }
+        )
 
         # action space
         # NOTE: keep original structure of action space (box space), just define bounds
-        steering_low, steering_high = self.sim.params['s_min'], self.sim.params['s_max']
-        velocity_low, velocity_high = self.sim.params['v_min'], self.sim.params['v_max']
-        low = np.array([[steering_low, velocity_low]]).repeat(self.num_agents, 0).astype(np.float32)
-        high = np.array([[steering_high, velocity_high]]).repeat(self.num_agents, 0).astype(np.float32)
-        self.action_space = gym.spaces.Box(low=low, high=high,
-                                           shape=(self.num_agents, 2),
-                                           dtype=np.float32)
+        steering_low, steering_high = self.sim.params["s_min"], self.sim.params["s_max"]
+        velocity_low, velocity_high = self.sim.params["v_min"], self.sim.params["v_max"]
+        low = (
+            np.array([[steering_low, velocity_low]])
+            .repeat(self.num_agents, 0)
+            .astype(np.float32)
+        )
+        high = (
+            np.array([[steering_high, velocity_high]])
+            .repeat(self.num_agents, 0)
+            .astype(np.float32)
+        )
+        self.action_space = gym.spaces.Box(
+            low=low, high=high, shape=(self.num_agents, 2), dtype=np.float32
+        )
 
         # stateful observations for rendering
         self.render_obs = None
@@ -229,10 +254,58 @@ def __del__(self):
         """
         pass
 
+    @classmethod
+    def default_config(cls) -> dict:
+        """
+        Default environment configuration.
+
+        Can be overloaded in environment implementations, or by calling configure().
+
+        Args:
+            None
+
+        Returns:
+            a configuration dict
+        """
+        return {
+            "seed": 12345,
+            "map_name": "vegas.yaml",
+            "map_ext": ".png",
+            "params": {
+                "mu": 1.0489,
+                "C_Sf": 4.718,
+                "C_Sr": 5.4562,
+                "lf": 0.15875,
+                "lr": 0.17145,
+                "h": 0.074,
+                "m": 3.74,
+                "I": 0.04712,
+                "s_min": -0.4189,
+                "s_max": 0.4189,
+                "sv_min": -3.2,
+                "sv_max": 3.2,
+                "v_switch": 7.319,
+                "a_max": 9.51,
+                "v_min": -5.0,
+                "v_max": 20.0,
+                "width": 0.31,
+                "length": 0.58,
+            },
+            "num_agents": 2,
+            "timestep": 0.01,
+            "ego_idx": 0,
+            "integrator": "rk4",
+            "control_input": "speed",
+        }
+
+    def configure(self, config: dict) -> None:
+        if config:
+            self.config.update(config)
+
     def _check_done(self):
         """
         Check if the current rollout is done
-        
+
         Args:
             None
 
@@ -256,7 +329,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]:
@@ -276,17 +349,17 @@ def _check_done(self):
     def _update_state(self, obs_dict):
         """
         Update the env's states according to observations
-        
+
         Args:
             obs_dict (dict): dictionary of observation
 
         Returns:
             None
         """
-        self.poses_x = obs_dict['poses_x']
-        self.poses_y = obs_dict['poses_y']
-        self.poses_theta = obs_dict['poses_theta']
-        self.collisions = obs_dict['collisions']
+        self.poses_x = obs_dict["poses_x"]
+        self.poses_y = obs_dict["poses_y"]
+        self.poses_theta = obs_dict["poses_theta"]
+        self.collisions = obs_dict["collisions"]
 
     def step(self, action):
         """
@@ -304,8 +377,8 @@ def step(self, action):
 
         # call simulation step
         obs = self.sim.step(action)
-        obs['lap_times'] = self.lap_times
-        obs['lap_counts'] = self.lap_counts
+        obs["lap_times"] = self.lap_times
+        obs["lap_counts"] = self.lap_counts
 
         # cast to match observation space
         for key in obs.keys():
@@ -315,12 +388,12 @@ def step(self, action):
         F110Env.current_obs = obs
 
         self.render_obs = {
-            'ego_idx': obs['ego_idx'],
-            'poses_x': obs['poses_x'],
-            'poses_y': obs['poses_y'],
-            'poses_theta': obs['poses_theta'],
-            'lap_times': obs['lap_times'],
-            'lap_counts': obs['lap_counts']
+            "ego_idx": obs["ego_idx"],
+            "poses_x": obs["poses_x"],
+            "poses_y": obs["poses_y"],
+            "poses_theta": obs["poses_theta"],
+            "lap_times": obs["lap_times"],
+            "lap_counts": obs["lap_counts"],
         }
 
         # times
@@ -333,7 +406,7 @@ def step(self, action):
         # check done
         done, toggle_list = self._check_done()
         truncated = False
-        info = {'checkpoint_done': toggle_list}
+        info = {"checkpoint_done": toggle_list}
 
         return obs, reward, done, truncated, info
 
@@ -367,15 +440,26 @@ def reset(self, seed=None, options=None):
         if options is None or "poses" not in options:
             options = {"poses": np.zeros((self.num_agents, 3))}
         assert "poses" in options, "Must provide initial poses for reset"
-        assert isinstance(options["poses"], np.ndarray) and options["poses"].shape == (self.num_agents, 3), \
-            "Initial poses must be a numpy array of shape (num_agents, 3)"
+        assert isinstance(options["poses"], np.ndarray) and options["poses"].shape == (
+            self.num_agents,
+            3,
+        ), "Initial poses must be a numpy array of shape (num_agents, 3)"
         poses = options["poses"]
         self.start_xs = poses[:, 0]
         self.start_ys = poses[:, 1]
         self.start_thetas = poses[:, 2]
         self.start_rot = np.array(
-            [[np.cos(-self.start_thetas[self.ego_idx]), -np.sin(-self.start_thetas[self.ego_idx])],
-             [np.sin(-self.start_thetas[self.ego_idx]), np.cos(-self.start_thetas[self.ego_idx])]])
+            [
+                [
+                    np.cos(-self.start_thetas[self.ego_idx]),
+                    -np.sin(-self.start_thetas[self.ego_idx]),
+                ],
+                [
+                    np.sin(-self.start_thetas[self.ego_idx]),
+                    np.cos(-self.start_thetas[self.ego_idx]),
+                ],
+            ]
+        )
 
         # call reset to simulator
         self.sim.reset(poses)
@@ -385,12 +469,12 @@ def reset(self, seed=None, options=None):
         obs, _, _, _, info = self.step(action)
 
         self.render_obs = {
-            'ego_idx': obs['ego_idx'],
-            'poses_x': obs['poses_x'],
-            'poses_y': obs['poses_y'],
-            'poses_theta': obs['poses_theta'],
-            'lap_times': obs['lap_times'],
-            'lap_counts': obs['lap_counts']
+            "ego_idx": obs["ego_idx"],
+            "poses_x": obs["poses_x"],
+            "poses_y": obs["poses_y"],
+            "poses_theta": obs["poses_theta"],
+            "lap_times": obs["lap_times"],
+            "lap_counts": obs["lap_counts"],
         }
 
         return obs, info
@@ -411,7 +495,7 @@ def update_map(self, map_path, map_ext):
     def update_params(self, params, index=-1):
         """
         Updates the parameters used by simulation for vehicles
-        
+
         Args:
             params (dict): dictionary of parameters
             index (int, default=-1): if >= 0 then only update a specific agent's params
@@ -431,7 +515,7 @@ def add_render_callback(self, callback_func):
 
         F110Env.render_callbacks.append(callback_func)
 
-    def render(self, mode='human'):
+    def render(self, mode="human"):
         """
         Renders the environment with pyglet. Use mouse scroll in the window to zoom in/out, use mouse click drag to pan. Shows the agents, the map, current fps (bottom left corner), and the race information near as text.
 
@@ -445,20 +529,26 @@ def render(self, mode='human'):
         """
         # NOTE: separate render (manage render-mode) from render_frame (actual rendering with pyglet)
 
-        if self.render_mode not in self.metadata['render_modes']:
+        if self.render_mode not in self.metadata["render_modes"]:
             return
-        if self.render_mode in ['human', 'human_fast']:
+        if self.render_mode in ["human", "human_fast"]:
             self.render_frame(mode=self.render_mode)
-        elif self.render_mode == 'rgb_array':
+        elif self.render_mode == "rgb_array":
             # NOTE: this is extremely slow and should be changed to use pygame
             import PIL
             from PIL.Image import Transpose
 
             self.render_frame(mode="human_fast")
-            image_data = pyglet.image.get_buffer_manager().get_color_buffer().get_image_data()
+            image_data = (
+                pyglet.image.get_buffer_manager().get_color_buffer().get_image_data()
+            )
             fmt = "RGB"
             pitch = image_data.width * len(fmt)
-            pil_image = PIL.Image.frombytes(fmt, (image_data.width, image_data.height), image_data.get_data(fmt, pitch))
+            pil_image = PIL.Image.frombytes(
+                fmt,
+                (image_data.width, image_data.height),
+                image_data.get_data(fmt, pitch),
+            )
             pil_image = pil_image.transpose(Transpose.FLIP_TOP_BOTTOM)
             return np.array(pil_image)
         else:
@@ -468,6 +558,7 @@ def render_frame(self, mode):
         if F110Env.renderer is None:
             # first call, initialize everything
             from f110_gym.envs.rendering import EnvRenderer
+
             F110Env.renderer = EnvRenderer(WINDOW_W, WINDOW_H)
             F110Env.renderer.update_map(self.map_name, self.map_ext)
 
@@ -479,7 +570,7 @@ def render_frame(self, mode):
         F110Env.renderer.dispatch_events()
         F110Env.renderer.on_draw()
         F110Env.renderer.flip()
-        if mode == 'human':
+        if mode == "human":
             time.sleep(0.005)
-        elif mode == 'human_fast':
+        elif mode == "human_fast":
             pass

From 779543f570dcbccda8dcbcda1882dac285f4c2c0 Mon Sep 17 00:00:00 2001
From: Hongrui Zheng <billyzheng.bz@gmail.com>
Date: Tue, 20 Jun 2023 16:35:14 -0400
Subject: [PATCH 03/42] fix default value error

---
 gym/f110_gym/envs/f110_env.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 3b044791..a0ff01ec 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -269,7 +269,7 @@ def default_config(cls) -> dict:
         """
         return {
             "seed": 12345,
-            "map_name": "vegas.yaml",
+            "map": os.path.dirname(os.path.abspath(__file__)) + "/maps/skirk",
             "map_ext": ".png",
             "params": {
                 "mu": 1.0489,

From 0b1ea1703d91e2837be18e751cce6b27942f2bad Mon Sep 17 00:00:00 2001
From: ahmadamine998 <ahmadamine998@gmail.com>
Date: Thu, 22 Jun 2023 15:10:14 -0400
Subject: [PATCH 04/42] Added model as kwargs and parameter

---
 gym/f110_gym/envs/base_classes.py | 18 +++++++++++++-----
 gym/f110_gym/envs/f110_env.py     | 10 ++++++++--
 2 files changed, 21 insertions(+), 7 deletions(-)

diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index 192e32eb..7ab72d8b 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -41,6 +41,9 @@ class Integrator(Enum):
     RK4 = 1
     Euler = 2
 
+class Model(Enum):
+    KS = 1      # Kinematic Single Track
+    ST = 2      # Single Track
 
 class RaceCar(object):
     """
@@ -66,7 +69,7 @@ class RaceCar(object):
     scan_angles = None
     side_distances = None
 
-    def __init__(self, params, seed, is_ego=False, time_step=0.01, num_beams=1080, fov=4.7, integrator=Integrator.Euler):
+    def __init__(self, params, seed, is_ego=False, time_step=0.01, num_beams=1080, fov=4.7, integrator=Integrator.Euler, model=Model.ST):
         """
         Init function
 
@@ -92,6 +95,10 @@ def __init__(self, params, seed, is_ego=False, time_step=0.01, num_beams=1080, f
         if self.integrator is Integrator.RK4:
             warnings.warn(f"Chosen integrator is RK4. This is different from previous versions of the gym.")
 
+        self.model = model
+        if self.model is not Model.ST:
+            warnings.warn(f"Chosen model is not ST. This is different from previous versions of the gym.")
+            
         # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
         self.state = np.zeros((7, ))
 
@@ -456,7 +463,7 @@ class Simulator(object):
 
     """
 
-    def __init__(self, params, num_agents, seed, time_step=0.01, ego_idx=0, integrator=Integrator.RK4):
+    def __init__(self, params, num_agents, seed, time_step=0.01, ego_idx=0, integrator=Integrator.RK4, model=Model.ST):
         """
         Init function
 
@@ -466,7 +473,8 @@ def __init__(self, params, num_agents, seed, time_step=0.01, ego_idx=0, integrat
             seed (int): seed of the rng in scan simulation
             time_step (float, default=0.01): physics time step
             ego_idx (int, default=0): ego vehicle's index in list of agents
-
+            integrator (Integrator, default=Integrator.RK4): integrator to use for vehicle dynamics
+            model (Model, default=Model.ST): vehicle dynamics model to use
         Returns:
             None
         """
@@ -483,10 +491,10 @@ def __init__(self, params, num_agents, seed, time_step=0.01, ego_idx=0, integrat
         # initializing agents
         for i in range(self.num_agents):
             if i == ego_idx:
-                ego_car = RaceCar(params, self.seed, is_ego=True, time_step=self.time_step, integrator=integrator)
+                ego_car = RaceCar(params, self.seed, is_ego=True, time_step=self.time_step, integrator=integrator, model=model)
                 self.agents.append(ego_car)
             else:
-                agent = RaceCar(params, self.seed, is_ego=False, time_step=self.time_step, integrator=integrator)
+                agent = RaceCar(params, self.seed, is_ego=False, time_step=self.time_step, integrator=integrator, model=model)
                 self.agents.append(agent)
 
     def set_map(self, map_path, map_ext):
diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 8491baab..534e8e12 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -30,7 +30,7 @@
 from gym.utils import seeding
 
 # base classes
-from f110_gym.envs.base_classes import Simulator, Integrator
+from f110_gym.envs.base_classes import Simulator, Integrator, Model
 
 # others
 import numpy as np
@@ -150,6 +150,12 @@ def __init__(self, **kwargs):
         except:
             self.integrator = Integrator.RK4
 
+        # default dynamics model
+        try:
+            self.model = kwargs['model']
+        except:
+            self.model = Model.KS
+
         # radius to consider done
         self.start_thresh = 0.5  # 10cm
 
@@ -181,7 +187,7 @@ def __init__(self, **kwargs):
         self.start_rot = np.eye(2)
 
         # initiate stuff
-        self.sim = Simulator(self.params, self.num_agents, self.seed, time_step=self.timestep, integrator=self.integrator)
+        self.sim = Simulator(self.params, self.num_agents, self.seed, time_step=self.timestep, integrator=self.integrator, model=self.model)
         self.sim.set_map(self.map_path, self.map_ext)
 
         # stateful observations for rendering

From 2c170d073395c0d2a4bb5284afb0677b053ab8ef Mon Sep 17 00:00:00 2001
From: ahmadamine998 <ahmadamine998@gmail.com>
Date: Thu, 22 Jun 2023 15:30:29 -0400
Subject: [PATCH 05/42] Implemented and tested for KS model

---
 gym/f110_gym/envs/base_classes.py | 49 +++++++++++++++++++++++--------
 gym/f110_gym/envs/f110_env.py     |  2 +-
 2 files changed, 38 insertions(+), 13 deletions(-)

diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index 7ab72d8b..8f2498df 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -33,7 +33,7 @@
 import numpy as np
 from numba import njit
 
-from f110_gym.envs.dynamic_models import vehicle_dynamics_st, pid
+from f110_gym.envs.dynamic_models import vehicle_dynamics_st, vehicle_dynamics_ks, pid
 from f110_gym.envs.laser_models import ScanSimulator2D, check_ttc_jit, ray_cast
 from f110_gym.envs.collision_models import get_vertices, collision_multiple
 
@@ -98,10 +98,16 @@ def __init__(self, params, seed, is_ego=False, time_step=0.01, num_beams=1080, f
         self.model = model
         if self.model is not Model.ST:
             warnings.warn(f"Chosen model is not ST. This is different from previous versions of the gym.")
-            
-        # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
-        self.state = np.zeros((7, ))
 
+        if self.model is Model.ST:
+            # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
+            self.state = np.zeros((7, ))
+        elif self.model is Model.KS:
+            # state is [x, y, steer_angle, vel, yaw_angle]
+            self.state = np.zeros((5, ))
+        else:
+            raise NotImplementedError(f"Model {self.model} is not implemented.")
+        
         # pose of opponents in the world
         self.opp_poses = None
 
@@ -201,7 +207,13 @@ def reset(self, pose):
         # clear collision indicator
         self.in_collision = False
         # clear state
-        self.state = np.zeros((7, ))
+        if self.model is Model.ST:
+            self.state = np.zeros((7, ))
+        elif self.model is Model.KS:
+            self.state = np.zeros((5, ))
+        else:
+            raise NotImplementedError(f"Model {self.model} is not implemented.")
+        
         self.state[0:2] = pose[0:2]
         self.state[4] = pose[2]
         self.steer_buffer = np.empty((0, ))
@@ -286,9 +298,16 @@ def update_pose(self, raw_steer, vel):
         # steering angle velocity input to steering velocity acceleration input
         accl, sv = pid(vel, steer, self.state[3], self.state[2], self.params['sv_max'], self.params['a_max'], self.params['v_max'], self.params['v_min'])
         
+        if self.model == Model.KS:
+            f_dynamics = vehicle_dynamics_ks
+        elif self.model == Model.ST:
+            f_dynamics = vehicle_dynamics_st
+        else:
+            raise ValueError('Invalid vehicle model')
+        
         if self.integrator is Integrator.RK4:
             # RK4 integration
-            k1 = vehicle_dynamics_st(
+            k1 = f_dynamics(
                 self.state,
                 np.array([sv, accl]),
                 self.params['mu'],
@@ -310,7 +329,7 @@ def update_pose(self, raw_steer, vel):
 
             k2_state = self.state + self.time_step*(k1/2)
 
-            k2 = vehicle_dynamics_st(
+            k2 = f_dynamics(
                 k2_state,
                 np.array([sv, accl]),
                 self.params['mu'],
@@ -332,7 +351,7 @@ def update_pose(self, raw_steer, vel):
 
             k3_state = self.state + self.time_step*(k2/2)
 
-            k3 = vehicle_dynamics_st(
+            k3 = f_dynamics(
                 k3_state,
                 np.array([sv, accl]),
                 self.params['mu'],
@@ -354,7 +373,7 @@ def update_pose(self, raw_steer, vel):
 
             k4_state = self.state + self.time_step*k3
 
-            k4 = vehicle_dynamics_st(
+            k4 = f_dynamics(
                 k4_state,
                 np.array([sv, accl]),
                 self.params['mu'],
@@ -378,7 +397,7 @@ def update_pose(self, raw_steer, vel):
             self.state = self.state + self.time_step*(1/6)*(k1 + 2*k2 + 2*k3 + k4)
         
         elif self.integrator is Integrator.Euler:
-            f = vehicle_dynamics_st(
+            f = f_dynamics(
                 self.state,
                 np.array([sv, accl]),
                 self.params['mu'],
@@ -460,7 +479,8 @@ class Simulator(object):
         agents (list[RaceCar]): container for RaceCar objects
         collisions (np.ndarray(num_agents, )): array of collision indicator for each agent
         collision_idx (np.ndarray(num_agents, )): which agent is each agent in collision with
-
+        integrator (Integrator): integrator to use for vehicle dynamics
+        model (Model): model to use for vehicle dynamics
     """
 
     def __init__(self, params, num_agents, seed, time_step=0.01, ego_idx=0, integrator=Integrator.RK4, model=Model.ST):
@@ -487,6 +507,7 @@ def __init__(self, params, num_agents, seed, time_step=0.01, ego_idx=0, integrat
         self.agents = []
         self.collisions = np.zeros((self.num_agents, ))
         self.collision_idx = -1 * np.ones((self.num_agents, ))
+        self.model = model
 
         # initializing agents
         for i in range(self.num_agents):
@@ -608,7 +629,11 @@ def step(self, control_inputs):
             observations['poses_theta'].append(agent.state[4])
             observations['linear_vels_x'].append(agent.state[3])
             observations['linear_vels_y'].append(0.)
-            observations['ang_vels_z'].append(agent.state[5])
+
+            if self.model == Model.ST:
+                observations['ang_vels_z'].append(agent.state[5])
+            elif self.model == Model.KS:
+                observations['ang_vels_z'].append(0.)
 
         return observations
 
diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 534e8e12..6edb48ef 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -154,7 +154,7 @@ def __init__(self, **kwargs):
         try:
             self.model = kwargs['model']
         except:
-            self.model = Model.KS
+            self.model = Model.ST
 
         # radius to consider done
         self.start_thresh = 0.5  # 10cm

From 774ec429dbe9af627feeb9d867adf81d24fd9984 Mon Sep 17 00:00:00 2001
From: ahmadamine998 <ahmadamine998@gmail.com>
Date: Thu, 22 Jun 2023 15:35:39 -0400
Subject: [PATCH 06/42] Minor Cleanup

---
 gym/f110_gym/envs/base_classes.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index 8f2498df..39f09945 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -298,9 +298,9 @@ def update_pose(self, raw_steer, vel):
         # steering angle velocity input to steering velocity acceleration input
         accl, sv = pid(vel, steer, self.state[3], self.state[2], self.params['sv_max'], self.params['a_max'], self.params['v_max'], self.params['v_min'])
         
-        if self.model == Model.KS:
+        if self.model is Model.KS:
             f_dynamics = vehicle_dynamics_ks
-        elif self.model == Model.ST:
+        elif self.model is Model.ST:
             f_dynamics = vehicle_dynamics_st
         else:
             raise ValueError('Invalid vehicle model')
@@ -634,6 +634,8 @@ def step(self, control_inputs):
                 observations['ang_vels_z'].append(agent.state[5])
             elif self.model == Model.KS:
                 observations['ang_vels_z'].append(0.)
+            else:
+                raise NotImplementedError(f"Model {self.model} not implemented")
 
         return observations
 

From 73c501a2798720dcbb20e2ee9a88e96d1f531640 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Fri, 23 Jun 2023 00:10:37 +0200
Subject: [PATCH 07/42] implement multiple observations, add test. base
 classes: add agent_scan for making it accessible from observation-types
 f110-env: refactoring to remove dependency on observation for state-update,
 remove unnecessary code for render_obs in reset (already done in step)

---
 gym/f110_gym/envs/base_classes.py         |  27 +---
 gym/f110_gym/envs/f110_env.py             |  85 ++++-------
 gym/f110_gym/envs/observation.py          | 176 ++++++++++++++++++++++
 gym/f110_gym/unittest/observation_test.py | 150 ++++++++++++++++++
 4 files changed, 362 insertions(+), 76 deletions(-)
 create mode 100644 gym/f110_gym/envs/observation.py
 create mode 100644 gym/f110_gym/unittest/observation_test.py

diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index 192e32eb..ac07cb8f 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -489,6 +489,10 @@ def __init__(self, params, num_agents, seed, time_step=0.01, ego_idx=0, integrat
                 agent = RaceCar(params, self.seed, is_ego=False, time_step=self.time_step, integrator=integrator)
                 self.agents.append(agent)
 
+        # initialize agents scan, to be accessed from observation types
+        num_beams = self.agents[0].scan_simulator.num_beams
+        self.agent_scans = np.empty((self.num_agents, num_beams))
+
     def set_map(self, map_path, map_ext):
         """
         Sets the map of the environment and sets the map for scan simulator of each agent
@@ -581,29 +585,6 @@ def step(self, control_inputs):
             if agent.in_collision:
                 self.collisions[i] = 1.
 
-        # fill in observations
-        # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
-        # collision_angles is removed from observations
-        observations = {'ego_idx': self.ego_idx,
-            'scans': [],
-            'poses_x': [],
-            'poses_y': [],
-            'poses_theta': [],
-            'linear_vels_x': [],
-            'linear_vels_y': [],
-            'ang_vels_z': [],
-            'collisions': self.collisions}
-        for i, agent in enumerate(self.agents):
-            observations['scans'].append(agent_scans[i])
-            observations['poses_x'].append(agent.state[0])
-            observations['poses_y'].append(agent.state[1])
-            observations['poses_theta'].append(agent.state[4])
-            observations['linear_vels_x'].append(agent.state[3])
-            observations['linear_vels_y'].append(0.)
-            observations['ang_vels_z'].append(agent.state[5])
-
-        return observations
-
     def reset(self, poses):
         """
         Resets the simulation environment by given poses
diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 66b728c3..8f7264f4 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -38,6 +38,8 @@
 # gl
 import pyglet
 
+from f110_gym.envs.observation import observation_factory
+
 pyglet.options['debug_gl'] = False
 from pyglet import gl
 
@@ -156,6 +158,13 @@ def __init__(self, render_mode=None, **kwargs):
         except:
             self.integrator = Integrator.RK4
 
+        # observation
+        try:
+            assert isinstance(kwargs['observation_config'], dict), "expected 'observation_config' to be a dictionary"
+            self.observation_config = kwargs['observation_config']
+        except:
+            self.observation_config = {"type": "original"}
+
         # radius to consider done
         self.start_thresh = 0.5  # 10cm
 
@@ -191,23 +200,12 @@ def __init__(self, render_mode=None, **kwargs):
                              integrator=self.integrator)
         self.sim.set_map(self.map_path, self.map_ext)
 
-        # observation space
-        # NOTE: keep original structure of observation space (dict). just define it as a dict space and define bounds
-        scan_size, scan_range = self.sim.agents[0].scan_simulator.num_beams, self.sim.agents[0].scan_simulator.max_range
-        large_num = 1e30  # large number to avoid unbounded obs space (ie., low=-inf or high=inf)
-        self.observation_space = gym.spaces.Dict({
-            'ego_idx': gym.spaces.Discrete(self.num_agents),
-            'scans': gym.spaces.Box(low=0.0, high=scan_range, shape=(self.num_agents, scan_size), dtype=np.float32),
-            'poses_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'poses_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'poses_theta': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'linear_vels_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'linear_vels_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'ang_vels_z': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'collisions': gym.spaces.Box(low=0.0, high=1.0, shape=(self.num_agents,), dtype=np.float32),
-            'lap_times': gym.spaces.Box(low=0.0, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'lap_counts': gym.spaces.Box(low=0.0, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-        })
+        # observations
+        self.agent_ids = [f"agent_{i}" for i in range(self.num_agents)]
+
+        assert "type" in self.observation_config, "observation_config must contain 'type' key"
+        self.observation_type = observation_factory(env=self, **self.observation_config)
+        self.observation_space = self.observation_type.space()
 
         # action space
         # NOTE: keep original structure of action space (box space), just define bounds
@@ -273,20 +271,14 @@ def _check_done(self):
 
         return bool(done), self.toggle_list >= 4
 
-    def _update_state(self, obs_dict):
+    def _update_state(self):
         """
-        Update the env's states according to observations
-        
-        Args:
-            obs_dict (dict): dictionary of observation
-
-        Returns:
-            None
+        Update the env's states according to observations.
         """
-        self.poses_x = obs_dict['poses_x']
-        self.poses_y = obs_dict['poses_y']
-        self.poses_theta = obs_dict['poses_theta']
-        self.collisions = obs_dict['collisions']
+        self.poses_x = self.sim.agent_poses[:, 0]
+        self.poses_y = self.sim.agent_poses[:, 1]
+        self.poses_theta = self.sim.agent_poses[:, 2]
+        self.collisions = self.sim.collisions
 
     def step(self, action):
         """
@@ -303,24 +295,20 @@ def step(self, action):
         """
 
         # call simulation step
-        obs = self.sim.step(action)
-        obs['lap_times'] = self.lap_times
-        obs['lap_counts'] = self.lap_counts
+        self.sim.step(action)
 
-        # cast to match observation space
-        for key in obs.keys():
-            if isinstance(obs[key], np.ndarray) or isinstance(obs[key], list):
-                obs[key] = np.array(obs[key], dtype=np.float32)
+        # observation
+        obs = self.observation_type.observe()
 
+        # rendering observation
         F110Env.current_obs = obs
-
         self.render_obs = {
-            'ego_idx': obs['ego_idx'],
-            'poses_x': obs['poses_x'],
-            'poses_y': obs['poses_y'],
-            'poses_theta': obs['poses_theta'],
-            'lap_times': obs['lap_times'],
-            'lap_counts': obs['lap_counts']
+            'ego_idx': self.sim.ego_idx,
+            'poses_x': self.sim.agent_poses[:, 0],
+            'poses_y': self.sim.agent_poses[:, 1],
+            'poses_theta': self.sim.agent_poses[:, 2],
+            'lap_times': self.lap_times,
+            'lap_counts': self.lap_counts,
         }
 
         # times
@@ -328,7 +316,7 @@ def step(self, action):
         self.current_time = self.current_time + self.timestep
 
         # update data member
-        self._update_state(obs)
+        self._update_state()  # note: remove dependency on observation because it could contains different features
 
         # check done
         done, toggle_list = self._check_done()
@@ -384,15 +372,6 @@ def reset(self, seed=None, options=None):
         action = np.zeros((self.num_agents, 2))
         obs, _, _, _, info = self.step(action)
 
-        self.render_obs = {
-            'ego_idx': obs['ego_idx'],
-            'poses_x': obs['poses_x'],
-            'poses_y': obs['poses_y'],
-            'poses_theta': obs['poses_theta'],
-            'lap_times': obs['lap_times'],
-            'lap_counts': obs['lap_counts']
-        }
-
         return obs, info
 
     def update_map(self, map_path, map_ext):
diff --git a/gym/f110_gym/envs/observation.py b/gym/f110_gym/envs/observation.py
new file mode 100644
index 00000000..59a31aea
--- /dev/null
+++ b/gym/f110_gym/envs/observation.py
@@ -0,0 +1,176 @@
+"""
+Author: Luigi Berducci
+"""
+from abc import abstractmethod
+from typing import List
+
+import gymnasium as gym
+import numpy as np
+
+
+class Observation:
+    """
+    Abstract class for observations. Each observation must implement the space and observe methods.
+
+    :param env: The environment.
+    :param vehicle_id: The id of the observer vehicle.
+    :param kwargs: Additional arguments.
+    """
+
+    def __init__(self, env: 'F110Env'):
+        self.env = env
+
+    @abstractmethod
+    def space(self):
+        raise NotImplementedError()
+
+    @abstractmethod
+    def observe(self):
+        raise NotImplementedError()
+
+
+class OriginalObservation(Observation):
+    def __init__(self, env: 'F110Env'):
+        super().__init__(env)
+
+    def space(self):
+        num_agents = self.env.num_agents
+        scan_size = self.env.sim.agents[0].scan_simulator.num_beams
+        scan_range = self.env.sim.agents[0].scan_simulator.max_range
+        large_num = 1e30  # large number to avoid unbounded obs space (ie., low=-inf or high=inf)
+
+        obs_space = gym.spaces.Dict({
+            'ego_idx': gym.spaces.Discrete(num_agents),
+            'scans': gym.spaces.Box(low=0.0, high=scan_range, shape=(num_agents, scan_size), dtype=np.float32),
+            'poses_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'poses_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'poses_theta': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'linear_vels_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'linear_vels_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'ang_vels_z': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'collisions': gym.spaces.Box(low=0.0, high=1.0, shape=(num_agents,), dtype=np.float32),
+            'lap_times': gym.spaces.Box(low=0.0, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'lap_counts': gym.spaces.Box(low=0.0, high=large_num, shape=(num_agents,), dtype=np.float32),
+        })
+
+        return obs_space
+
+    def observe(self):
+        # state indices
+        agent = self.env.sim.agents[0]
+        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(len(agent.state))
+
+        observations = {'ego_idx': self.env.sim.ego_idx,
+                        'scans': [],
+                        'poses_x': [],
+                        'poses_y': [],
+                        'poses_theta': [],
+                        'linear_vels_x': [],
+                        'linear_vels_y': [],
+                        'ang_vels_z': [],
+                        'collisions': [],
+                        'lap_times': [],
+                        'lap_counts': []}
+
+        for i, agent in enumerate(self.env.sim.agents):
+            agent_scan = self.env.sim.agent_scans[i]
+            lap_time = self.env.lap_times[i]
+            lap_count = self.env.lap_counts[i]
+            collision = self.env.sim.collisions[i]
+
+            observations['scans'].append(agent_scan)
+            observations['poses_x'].append(agent.state[xi])
+            observations['poses_y'].append(agent.state[yi])
+            observations['poses_theta'].append(agent.state[yawi])
+            observations['linear_vels_x'].append(agent.state[vxi])
+            observations['linear_vels_y'].append(0.0)
+            observations['ang_vels_z'].append(agent.state[yaw_ratei])
+            observations['collisions'].append(collision)
+            observations['lap_times'].append(lap_time)
+            observations['lap_counts'].append(lap_count)
+
+        # cast to match observation space
+        for key in observations.keys():
+            if isinstance(observations[key], np.ndarray) or isinstance(observations[key], list):
+                observations[key] = np.array(observations[key], dtype=np.float32)
+
+        return observations
+
+
+class FeaturesObservation(Observation):
+    def __init__(self, env: 'F110Env', features: List[str]):
+        super().__init__(env)
+        self.features = features
+
+    def space(self):
+        scan_size = self.env.sim.agents[0].scan_simulator.num_beams
+        scan_range = self.env.sim.agents[0].scan_simulator.max_range
+        large_num = 1e30  # large number to avoid unbounded obs space (ie., low=-inf or high=inf)
+
+        complete_space = {}
+        for agent_id in self.env.agent_ids:
+            agent_dict = {
+                "scan": gym.spaces.Box(low=0.0, high=scan_range, shape=(scan_size,), dtype=np.float32),
+                "pose_x": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "pose_y": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "pose_theta": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "linear_vel_x": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "linear_vel_y": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "ang_vel_z": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "delta": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "beta": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "collision": gym.spaces.Box(low=0.0, high=1.0, shape=(1,), dtype=np.float32),
+                "lap_time": gym.spaces.Box(low=0.0, high=large_num, shape=(1,), dtype=np.float32),
+                "lap_count": gym.spaces.Box(low=0.0, high=large_num, shape=(1,), dtype=np.float32),
+            }
+            complete_space[agent_id] = gym.spaces.Dict({k: agent_dict[k] for k in self.features})
+
+        obs_space = gym.spaces.Dict(complete_space)
+        return obs_space
+
+    def observe(self):
+        # state indices
+        agent = self.env.sim.agents[0]
+        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(len(agent.state))
+
+        obs = {}  # dictionary agent_id -> observation dict
+
+        for i, agent_id in enumerate(self.env.agent_ids):
+            scan = self.env.sim.agent_scans[i]
+            agent = self.env.sim.agents[i]
+
+            # create agent's observation dict
+            agent_obs = {
+                "scan": scan,
+                "pose_x": agent.state[xi],
+                "pose_y": agent.state[yi],
+                "pose_theta": agent.state[yawi],
+                "linear_vel_x": agent.state[vxi],
+                "linear_vel_y": 0.0,  # note: always set to 0.0? deprecated or not?
+                "ang_vel_z": agent.state[yaw_ratei],
+                "delta": agent.state[deltai],
+                "beta": agent.state[slipi],
+                "collision": int(agent.in_collision),
+                "lap_time": self.env.lap_times[i],
+                "lap_count": self.env.lap_counts[i],
+            }
+
+            # add agent's observation to multi-agent observation
+            obs[agent_id] = {k: agent_obs[k] for k in self.features}
+
+        return obs
+
+
+def observation_factory(env: 'F110Env', type: str, **kwargs) -> Observation:
+    if type == "original":
+        return OriginalObservation(env)
+    elif type == "features":
+        return FeaturesObservation(env, **kwargs)
+    elif type == "kinematic_state":
+        features = ["pose_x", "pose_y", "pose_theta", "linear_vel_x"]
+        return FeaturesObservation(env, features=features)
+    elif type == "dynamic_state":
+        features = ["pose_x", "pose_y", "delta", "linear_vel_x", "pose_theta", "ang_vel_z", "beta"]
+        return FeaturesObservation(env, features=features)
+    else:
+        raise ValueError(f"Invalid observation type {type}.")
diff --git a/gym/f110_gym/unittest/observation_test.py b/gym/f110_gym/unittest/observation_test.py
new file mode 100644
index 00000000..a722fed5
--- /dev/null
+++ b/gym/f110_gym/unittest/observation_test.py
@@ -0,0 +1,150 @@
+import pathlib
+import unittest
+from argparse import Namespace
+
+import numpy as np
+import yaml
+from gymnasium.spaces import Box
+
+from f110_gym.envs import F110Env
+from f110_gym.envs.observation import FeaturesObservation, observation_factory
+import gymnasium as gym
+
+
+class TestObservationInterface(unittest.TestCase):
+
+    @staticmethod
+    def _make_env(**kwargs) -> F110Env:
+        import f110_gym
+
+        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / 'examples'
+
+        with open(example_dir / 'config_example_map.yaml') as file:
+            conf_dict = yaml.load(file, Loader=yaml.FullLoader)
+        conf = Namespace(**conf_dict)
+        conf.map_path = str(example_dir / conf.map_path)
+
+        env = gym.make('f110_gym:f110-v0', map=conf.map_path, map_ext=conf.map_ext,
+                       num_agents=3, **kwargs)
+        return env
+
+    def test_original_obs_space(self):
+        """
+        Check backward compatibility with the original observation space.
+        """
+        env = self._make_env(observation_config={"type": "original"})
+
+        obs, _ = env.reset()
+
+        obs_keys = ['ego_idx', 'scans', 'poses_x', 'poses_y', 'poses_theta', 'linear_vels_x',
+                    'linear_vels_y', 'ang_vels_z', 'collisions', 'lap_times', 'lap_counts']
+
+        # check that the observation space has the correct types
+        self.assertTrue(all([isinstance(env.observation_space.spaces[k], Box) for k in obs_keys if k != 'ego_idx']))
+        self.assertTrue(all([env.observation_space.spaces[k].dtype == np.float32 for k in obs_keys if k != 'ego_idx']))
+
+        # check the observation space is a dict
+        self.assertTrue(isinstance(obs, dict))
+
+        # check that the observation has the correct keys
+        self.assertTrue(all([k in obs for k in obs_keys]))
+        self.assertTrue(all([k in obs_keys for k in obs]))
+        self.assertTrue(env.observation_space.contains(obs))
+
+    def test_features_observation(self):
+        """
+        Check the FeatureObservation allows to select an arbitrary subset of features.
+        """
+        features = ["pose_x", "pose_y", "pose_theta"]
+
+        env = self._make_env(observation_config={"type": "features", "features": features})
+
+        # check the observation space is a dict
+        self.assertTrue(isinstance(env.observation_space, gym.spaces.Dict))
+
+        # check that the observation space has the correct keys
+        for agent_id in env.agent_ids:
+            space = env.observation_space.spaces[agent_id].spaces
+            self.assertTrue(all([k in space for k in features]))
+            self.assertTrue(all([k in features for k in space]))
+
+        # check that the observation space has the correct types
+        for agent_id in env.agent_ids:
+            space = env.observation_space.spaces[agent_id].spaces
+            self.assertTrue(all([isinstance(space[k], Box) for k in features]))
+            self.assertTrue(all([space[k].dtype == np.float32 for k in features]))
+
+        # check the actual observation
+        obs, _ = env.reset()
+        obs, _, _, _, _ = env.step(env.action_space.sample())
+
+        for i, agent_id in enumerate(env.agent_ids):
+            pose_x, pose_y, pose_theta = env.sim.agent_poses[i]
+            obs_x, obs_y, obs_theta = obs[agent_id]["pose_x"], obs[agent_id]["pose_y"], obs[agent_id]["pose_theta"]
+
+            for ground_truth, observation in zip([pose_x, pose_y, pose_theta], [obs_x, obs_y, obs_theta]):
+                self.assertAlmostEqual(ground_truth, observation)
+
+    def test_unexisting_obs_space(self):
+        """
+        Check that an error is raised when an unexisting observation type is requested.
+        """
+        env = self._make_env()
+        with self.assertRaises(ValueError):
+            obs = observation_factory(env, vehicle_id=0, type="unexisting_obs_type")
+
+    def test_kinematic_obs_space(self):
+        """
+        Check the kinematic state observation space contains the correct features [x, y, theta, v].
+        """
+        env = self._make_env(observation_config={"type": "kinematic_state"})
+
+        kinematic_features = ["pose_x", "pose_y", "pose_theta", "linear_vel_x"]
+
+        # check kinematic features are in the observation space
+        for agent_id in env.agent_ids:
+            space = env.observation_space.spaces[agent_id].spaces
+            self.assertTrue(all([k in space for k in kinematic_features]))
+            self.assertTrue(all([k in kinematic_features for k in space]))
+
+        # check the actual observation
+        obs, _ = env.reset()
+        obs, _, _, _, _ = env.step(env.action_space.sample())
+
+        for i, agent_id in enumerate(env.agent_ids):
+            pose_x, pose_y, _, velx, pose_theta, _, _ = env.sim.agents[i].state
+            obs_x, obs_y, obs_theta = obs[agent_id]["pose_x"], obs[agent_id]["pose_y"], obs[agent_id]["pose_theta"]
+            obs_velx = obs[agent_id]["linear_vel_x"]
+
+            for ground_truth, observed in zip([pose_x, pose_y, pose_theta, velx],
+                                              [obs_x, obs_y, obs_theta, obs_velx]):
+                self.assertAlmostEqual(ground_truth, observed)
+
+    def test_dynamic_obs_space(self):
+        """
+        Check the dynamic state observation space contains the correct features.
+        """
+        env = self._make_env(observation_config={"type": "dynamic_state"})
+
+        kinematic_features = ["pose_x", "pose_y", "pose_theta", "linear_vel_x", "ang_vel_z", "delta", "beta"]
+
+        # check kinematic features are in the observation space
+        for agent_id in env.agent_ids:
+            space = env.observation_space.spaces[agent_id].spaces
+            self.assertTrue(all([k in space for k in kinematic_features]))
+            self.assertTrue(all([k in kinematic_features for k in space]))
+
+        # check the actual observation
+        obs, _ = env.reset()
+        obs, _, _, _, _ = env.step(env.action_space.sample())
+
+        for i, agent_id in enumerate(env.agent_ids):
+            pose_x, pose_y, delta, velx, pose_theta, _, beta = env.sim.agents[i].state
+
+            agent_obs = obs[agent_id]
+            obs_x, obs_y, obs_theta = agent_obs["pose_x"], agent_obs["pose_y"], agent_obs["pose_theta"]
+            obs_velx, obs_delta, obs_beta = agent_obs["linear_vel_x"], agent_obs["delta"], agent_obs["beta"]
+
+            for ground_truth, observed in zip([pose_x, pose_y, pose_theta, velx, delta, beta],
+                                              [obs_x, obs_y, obs_theta, obs_velx, obs_delta, obs_beta]):
+                self.assertAlmostEqual(ground_truth, observed)

From 3e2214944702416c8c13aa4c74af1f0b15627ad4 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Fri, 23 Jun 2023 00:28:02 +0200
Subject: [PATCH 08/42] set 0.0 state variables that are not in kinematic
 single track model

---
 gym/f110_gym/envs/observation.py | 50 +++++++++++++++++++-------------
 1 file changed, 30 insertions(+), 20 deletions(-)

diff --git a/gym/f110_gym/envs/observation.py b/gym/f110_gym/envs/observation.py
index 59a31aea..78efd616 100644
--- a/gym/f110_gym/envs/observation.py
+++ b/gym/f110_gym/envs/observation.py
@@ -57,8 +57,7 @@ def space(self):
 
     def observe(self):
         # state indices
-        agent = self.env.sim.agents[0]
-        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(len(agent.state))
+        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(7)  # 7 largest state size (ST Model)
 
         observations = {'ego_idx': self.env.sim.ego_idx,
                         'scans': [],
@@ -78,13 +77,17 @@ def observe(self):
             lap_count = self.env.lap_counts[i]
             collision = self.env.sim.collisions[i]
 
+            x, y, theta = agent.state[xi], agent.state[yi], agent.state[yawi]
+            vx, vy = agent.state[vxi], 0.0
+            angvel = 0.0 if len(agent.state) < 7 else agent.state[yaw_ratei]  # set 0.0 when KST Model
+
             observations['scans'].append(agent_scan)
-            observations['poses_x'].append(agent.state[xi])
-            observations['poses_y'].append(agent.state[yi])
-            observations['poses_theta'].append(agent.state[yawi])
-            observations['linear_vels_x'].append(agent.state[vxi])
-            observations['linear_vels_y'].append(0.0)
-            observations['ang_vels_z'].append(agent.state[yaw_ratei])
+            observations['poses_x'].append(x)
+            observations['poses_y'].append(y)
+            observations['poses_theta'].append(theta)
+            observations['linear_vels_x'].append(vx)
+            observations['linear_vels_y'].append(vy)
+            observations['ang_vels_z'].append(angvel)
             observations['collisions'].append(collision)
             observations['lap_times'].append(lap_time)
             observations['lap_counts'].append(lap_count)
@@ -130,29 +133,36 @@ def space(self):
 
     def observe(self):
         # state indices
-        agent = self.env.sim.agents[0]
-        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(len(agent.state))
+        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(7)  # 7 largest state size (ST Model)
 
         obs = {}  # dictionary agent_id -> observation dict
 
         for i, agent_id in enumerate(self.env.agent_ids):
             scan = self.env.sim.agent_scans[i]
             agent = self.env.sim.agents[i]
+            lap_time = self.env.lap_times[i]
+            lap_count = self.env.lap_counts[i]
+
+            x, y, theta = agent.state[xi], agent.state[yi], agent.state[yawi]
+            vx, vy = agent.state[vxi], 0.0
+            delta = agent.state[deltai]
+            beta = 0.0 if len(agent.state) < 7 else agent.state[slipi]  # set 0.0 when KST Model
+            angvel = 0.0 if len(agent.state) < 7 else agent.state[yaw_ratei]  # set 0.0 when KST Model
 
             # create agent's observation dict
             agent_obs = {
                 "scan": scan,
-                "pose_x": agent.state[xi],
-                "pose_y": agent.state[yi],
-                "pose_theta": agent.state[yawi],
-                "linear_vel_x": agent.state[vxi],
-                "linear_vel_y": 0.0,  # note: always set to 0.0? deprecated or not?
-                "ang_vel_z": agent.state[yaw_ratei],
-                "delta": agent.state[deltai],
-                "beta": agent.state[slipi],
+                "pose_x": x,
+                "pose_y": y,
+                "pose_theta": theta,
+                "linear_vel_x": vx,
+                "linear_vel_y": vy,
+                "ang_vel_z": angvel,
+                "delta": delta,
+                "beta": beta,
                 "collision": int(agent.in_collision),
-                "lap_time": self.env.lap_times[i],
-                "lap_count": self.env.lap_counts[i],
+                "lap_time": lap_time,
+                "lap_count": lap_count,
             }
 
             # add agent's observation to multi-agent observation

From 63c4e1e7b916086ca06810bc053e1bbf6ef042a0 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Fri, 23 Jun 2023 00:10:37 +0200
Subject: [PATCH 09/42] implement multiple observations, add test. base
 classes: add agent_scan for making it accessible from observation-types
 f110-env: refactoring to remove dependency on observation for state-update,
 remove unnecessary code for render_obs in reset (already done in step) set
 0.0 state variables that are not in kinematic single track model

---
 gym/f110_gym/envs/base_classes.py         |  31 +---
 gym/f110_gym/envs/f110_env.py             |  85 ++++------
 gym/f110_gym/envs/observation.py          | 186 ++++++++++++++++++++++
 gym/f110_gym/unittest/observation_test.py | 150 +++++++++++++++++
 4 files changed, 374 insertions(+), 78 deletions(-)
 create mode 100644 gym/f110_gym/envs/observation.py
 create mode 100644 gym/f110_gym/unittest/observation_test.py

diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index 192e32eb..a0945bc1 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -489,6 +489,10 @@ def __init__(self, params, num_agents, seed, time_step=0.01, ego_idx=0, integrat
                 agent = RaceCar(params, self.seed, is_ego=False, time_step=self.time_step, integrator=integrator)
                 self.agents.append(agent)
 
+        # initialize agents scan, to be accessed from observation types
+        num_beams = self.agents[0].scan_simulator.num_beams
+        self.agent_scans = np.empty((self.num_agents, num_beams))
+
     def set_map(self, map_path, map_ext):
         """
         Sets the map of the environment and sets the map for scan simulator of each agent
@@ -561,7 +565,7 @@ def step(self, control_inputs):
         for i, agent in enumerate(self.agents):
             # update each agent's pose
             current_scan = agent.update_pose(control_inputs[i, 0], control_inputs[i, 1])
-            agent_scans.append(current_scan)
+            self.agent_scans[i, :] = current_scan
 
             # update sim's information of agent poses
             self.agent_poses[i, :] = np.append(agent.state[0:2], agent.state[4])
@@ -575,35 +579,12 @@ def step(self, control_inputs):
             agent.update_opp_poses(opp_poses)
 
             # update each agent's current scan based on other agents
-            agent.update_scan(agent_scans, i)
+            agent.update_scan(self.agent_scans, i)
 
             # update agent collision with environment
             if agent.in_collision:
                 self.collisions[i] = 1.
 
-        # fill in observations
-        # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
-        # collision_angles is removed from observations
-        observations = {'ego_idx': self.ego_idx,
-            'scans': [],
-            'poses_x': [],
-            'poses_y': [],
-            'poses_theta': [],
-            'linear_vels_x': [],
-            'linear_vels_y': [],
-            'ang_vels_z': [],
-            'collisions': self.collisions}
-        for i, agent in enumerate(self.agents):
-            observations['scans'].append(agent_scans[i])
-            observations['poses_x'].append(agent.state[0])
-            observations['poses_y'].append(agent.state[1])
-            observations['poses_theta'].append(agent.state[4])
-            observations['linear_vels_x'].append(agent.state[3])
-            observations['linear_vels_y'].append(0.)
-            observations['ang_vels_z'].append(agent.state[5])
-
-        return observations
-
     def reset(self, poses):
         """
         Resets the simulation environment by given poses
diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 66b728c3..8f7264f4 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -38,6 +38,8 @@
 # gl
 import pyglet
 
+from f110_gym.envs.observation import observation_factory
+
 pyglet.options['debug_gl'] = False
 from pyglet import gl
 
@@ -156,6 +158,13 @@ def __init__(self, render_mode=None, **kwargs):
         except:
             self.integrator = Integrator.RK4
 
+        # observation
+        try:
+            assert isinstance(kwargs['observation_config'], dict), "expected 'observation_config' to be a dictionary"
+            self.observation_config = kwargs['observation_config']
+        except:
+            self.observation_config = {"type": "original"}
+
         # radius to consider done
         self.start_thresh = 0.5  # 10cm
 
@@ -191,23 +200,12 @@ def __init__(self, render_mode=None, **kwargs):
                              integrator=self.integrator)
         self.sim.set_map(self.map_path, self.map_ext)
 
-        # observation space
-        # NOTE: keep original structure of observation space (dict). just define it as a dict space and define bounds
-        scan_size, scan_range = self.sim.agents[0].scan_simulator.num_beams, self.sim.agents[0].scan_simulator.max_range
-        large_num = 1e30  # large number to avoid unbounded obs space (ie., low=-inf or high=inf)
-        self.observation_space = gym.spaces.Dict({
-            'ego_idx': gym.spaces.Discrete(self.num_agents),
-            'scans': gym.spaces.Box(low=0.0, high=scan_range, shape=(self.num_agents, scan_size), dtype=np.float32),
-            'poses_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'poses_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'poses_theta': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'linear_vels_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'linear_vels_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'ang_vels_z': gym.spaces.Box(low=-large_num, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'collisions': gym.spaces.Box(low=0.0, high=1.0, shape=(self.num_agents,), dtype=np.float32),
-            'lap_times': gym.spaces.Box(low=0.0, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-            'lap_counts': gym.spaces.Box(low=0.0, high=large_num, shape=(self.num_agents,), dtype=np.float32),
-        })
+        # observations
+        self.agent_ids = [f"agent_{i}" for i in range(self.num_agents)]
+
+        assert "type" in self.observation_config, "observation_config must contain 'type' key"
+        self.observation_type = observation_factory(env=self, **self.observation_config)
+        self.observation_space = self.observation_type.space()
 
         # action space
         # NOTE: keep original structure of action space (box space), just define bounds
@@ -273,20 +271,14 @@ def _check_done(self):
 
         return bool(done), self.toggle_list >= 4
 
-    def _update_state(self, obs_dict):
+    def _update_state(self):
         """
-        Update the env's states according to observations
-        
-        Args:
-            obs_dict (dict): dictionary of observation
-
-        Returns:
-            None
+        Update the env's states according to observations.
         """
-        self.poses_x = obs_dict['poses_x']
-        self.poses_y = obs_dict['poses_y']
-        self.poses_theta = obs_dict['poses_theta']
-        self.collisions = obs_dict['collisions']
+        self.poses_x = self.sim.agent_poses[:, 0]
+        self.poses_y = self.sim.agent_poses[:, 1]
+        self.poses_theta = self.sim.agent_poses[:, 2]
+        self.collisions = self.sim.collisions
 
     def step(self, action):
         """
@@ -303,24 +295,20 @@ def step(self, action):
         """
 
         # call simulation step
-        obs = self.sim.step(action)
-        obs['lap_times'] = self.lap_times
-        obs['lap_counts'] = self.lap_counts
+        self.sim.step(action)
 
-        # cast to match observation space
-        for key in obs.keys():
-            if isinstance(obs[key], np.ndarray) or isinstance(obs[key], list):
-                obs[key] = np.array(obs[key], dtype=np.float32)
+        # observation
+        obs = self.observation_type.observe()
 
+        # rendering observation
         F110Env.current_obs = obs
-
         self.render_obs = {
-            'ego_idx': obs['ego_idx'],
-            'poses_x': obs['poses_x'],
-            'poses_y': obs['poses_y'],
-            'poses_theta': obs['poses_theta'],
-            'lap_times': obs['lap_times'],
-            'lap_counts': obs['lap_counts']
+            'ego_idx': self.sim.ego_idx,
+            'poses_x': self.sim.agent_poses[:, 0],
+            'poses_y': self.sim.agent_poses[:, 1],
+            'poses_theta': self.sim.agent_poses[:, 2],
+            'lap_times': self.lap_times,
+            'lap_counts': self.lap_counts,
         }
 
         # times
@@ -328,7 +316,7 @@ def step(self, action):
         self.current_time = self.current_time + self.timestep
 
         # update data member
-        self._update_state(obs)
+        self._update_state()  # note: remove dependency on observation because it could contains different features
 
         # check done
         done, toggle_list = self._check_done()
@@ -384,15 +372,6 @@ def reset(self, seed=None, options=None):
         action = np.zeros((self.num_agents, 2))
         obs, _, _, _, info = self.step(action)
 
-        self.render_obs = {
-            'ego_idx': obs['ego_idx'],
-            'poses_x': obs['poses_x'],
-            'poses_y': obs['poses_y'],
-            'poses_theta': obs['poses_theta'],
-            'lap_times': obs['lap_times'],
-            'lap_counts': obs['lap_counts']
-        }
-
         return obs, info
 
     def update_map(self, map_path, map_ext):
diff --git a/gym/f110_gym/envs/observation.py b/gym/f110_gym/envs/observation.py
new file mode 100644
index 00000000..09b49d0b
--- /dev/null
+++ b/gym/f110_gym/envs/observation.py
@@ -0,0 +1,186 @@
+"""
+Author: Luigi Berducci
+"""
+from abc import abstractmethod
+from typing import List
+
+import gymnasium as gym
+import numpy as np
+
+
+class Observation:
+    """
+    Abstract class for observations. Each observation must implement the space and observe methods.
+
+    :param env: The environment.
+    :param vehicle_id: The id of the observer vehicle.
+    :param kwargs: Additional arguments.
+    """
+
+    def __init__(self, env: 'F110Env'):
+        self.env = env
+
+    @abstractmethod
+    def space(self):
+        raise NotImplementedError()
+
+    @abstractmethod
+    def observe(self):
+        raise NotImplementedError()
+
+
+class OriginalObservation(Observation):
+    def __init__(self, env: 'F110Env'):
+        super().__init__(env)
+
+    def space(self):
+        num_agents = self.env.num_agents
+        scan_size = self.env.sim.agents[0].scan_simulator.num_beams
+        scan_range = self.env.sim.agents[0].scan_simulator.max_range
+        large_num = 1e30  # large number to avoid unbounded obs space (ie., low=-inf or high=inf)
+
+        obs_space = gym.spaces.Dict({
+            'ego_idx': gym.spaces.Discrete(num_agents),
+            'scans': gym.spaces.Box(low=0.0, high=scan_range, shape=(num_agents, scan_size), dtype=np.float32),
+            'poses_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'poses_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'poses_theta': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'linear_vels_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'linear_vels_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'ang_vels_z': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'collisions': gym.spaces.Box(low=0.0, high=1.0, shape=(num_agents,), dtype=np.float32),
+            'lap_times': gym.spaces.Box(low=0.0, high=large_num, shape=(num_agents,), dtype=np.float32),
+            'lap_counts': gym.spaces.Box(low=0.0, high=large_num, shape=(num_agents,), dtype=np.float32),
+        })
+
+        return obs_space
+
+    def observe(self):
+        # state indices
+        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(7)  # 7 largest state size (ST Model)
+
+        observations = {'ego_idx': self.env.sim.ego_idx,
+                        'scans': [],
+                        'poses_x': [],
+                        'poses_y': [],
+                        'poses_theta': [],
+                        'linear_vels_x': [],
+                        'linear_vels_y': [],
+                        'ang_vels_z': [],
+                        'collisions': [],
+                        'lap_times': [],
+                        'lap_counts': []}
+
+        for i, agent in enumerate(self.env.sim.agents):
+            agent_scan = self.env.sim.agent_scans[i]
+            lap_time = self.env.lap_times[i]
+            lap_count = self.env.lap_counts[i]
+            collision = self.env.sim.collisions[i]
+
+            x, y, theta = agent.state[xi], agent.state[yi], agent.state[yawi]
+            vx, vy = agent.state[vxi], 0.0
+            angvel = 0.0 if len(agent.state) < 7 else agent.state[yaw_ratei]  # set 0.0 when KST Model
+
+            observations['scans'].append(agent_scan)
+            observations['poses_x'].append(x)
+            observations['poses_y'].append(y)
+            observations['poses_theta'].append(theta)
+            observations['linear_vels_x'].append(vx)
+            observations['linear_vels_y'].append(vy)
+            observations['ang_vels_z'].append(angvel)
+            observations['collisions'].append(collision)
+            observations['lap_times'].append(lap_time)
+            observations['lap_counts'].append(lap_count)
+
+        # cast to match observation space
+        for key in observations.keys():
+            if isinstance(observations[key], np.ndarray) or isinstance(observations[key], list):
+                observations[key] = np.array(observations[key], dtype=np.float32)
+
+        return observations
+
+
+class FeaturesObservation(Observation):
+    def __init__(self, env: 'F110Env', features: List[str]):
+        super().__init__(env)
+        self.features = features
+
+    def space(self):
+        scan_size = self.env.sim.agents[0].scan_simulator.num_beams
+        scan_range = self.env.sim.agents[0].scan_simulator.max_range
+        large_num = 1e30  # large number to avoid unbounded obs space (ie., low=-inf or high=inf)
+
+        complete_space = {}
+        for agent_id in self.env.agent_ids:
+            agent_dict = {
+                "scan": gym.spaces.Box(low=0.0, high=scan_range, shape=(scan_size,), dtype=np.float32),
+                "pose_x": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "pose_y": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "pose_theta": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "linear_vel_x": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "linear_vel_y": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "ang_vel_z": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "delta": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "beta": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
+                "collision": gym.spaces.Box(low=0.0, high=1.0, shape=(1,), dtype=np.float32),
+                "lap_time": gym.spaces.Box(low=0.0, high=large_num, shape=(1,), dtype=np.float32),
+                "lap_count": gym.spaces.Box(low=0.0, high=large_num, shape=(1,), dtype=np.float32),
+            }
+            complete_space[agent_id] = gym.spaces.Dict({k: agent_dict[k] for k in self.features})
+
+        obs_space = gym.spaces.Dict(complete_space)
+        return obs_space
+
+    def observe(self):
+        # state indices
+        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(7)  # 7 largest state size (ST Model)
+
+        obs = {}  # dictionary agent_id -> observation dict
+
+        for i, agent_id in enumerate(self.env.agent_ids):
+            scan = self.env.sim.agent_scans[i]
+            agent = self.env.sim.agents[i]
+            lap_time = self.env.lap_times[i]
+            lap_count = self.env.lap_counts[i]
+
+            x, y, theta = agent.state[xi], agent.state[yi], agent.state[yawi]
+            vx, vy = agent.state[vxi], 0.0
+            delta = agent.state[deltai]
+            beta = 0.0 if len(agent.state) < 7 else agent.state[slipi]  # set 0.0 when KST Model
+            angvel = 0.0 if len(agent.state) < 7 else agent.state[yaw_ratei]  # set 0.0 when KST Model
+
+            # create agent's observation dict
+            agent_obs = {
+                "scan": scan,
+                "pose_x": x,
+                "pose_y": y,
+                "pose_theta": theta,
+                "linear_vel_x": vx,
+                "linear_vel_y": vy,
+                "ang_vel_z": angvel,
+                "delta": delta,
+                "beta": beta,
+                "collision": int(agent.in_collision),
+                "lap_time": lap_time,
+                "lap_count": lap_count,
+            }
+
+            # add agent's observation to multi-agent observation
+            obs[agent_id] = {k: agent_obs[k] for k in self.features}
+
+        return obs
+
+
+def observation_factory(env: 'F110Env', type: str, **kwargs) -> Observation:
+    if type == "original":
+        return OriginalObservation(env)
+    elif type == "features":
+        return FeaturesObservation(env, **kwargs)
+    elif type == "kinematic_state":
+        features = ["pose_x", "pose_y", "delta", "linear_vel_x", "pose_theta"]
+        return FeaturesObservation(env, features=features)
+    elif type == "dynamic_state":
+        features = ["pose_x", "pose_y", "delta", "linear_vel_x", "pose_theta", "ang_vel_z", "beta"]
+        return FeaturesObservation(env, features=features)
+    else:
+        raise ValueError(f"Invalid observation type {type}.")
diff --git a/gym/f110_gym/unittest/observation_test.py b/gym/f110_gym/unittest/observation_test.py
new file mode 100644
index 00000000..4a27d29b
--- /dev/null
+++ b/gym/f110_gym/unittest/observation_test.py
@@ -0,0 +1,150 @@
+import pathlib
+import unittest
+from argparse import Namespace
+
+import numpy as np
+import yaml
+from gymnasium.spaces import Box
+
+from f110_gym.envs import F110Env
+from f110_gym.envs.observation import FeaturesObservation, observation_factory
+import gymnasium as gym
+
+
+class TestObservationInterface(unittest.TestCase):
+
+    @staticmethod
+    def _make_env(**kwargs) -> F110Env:
+        import f110_gym
+
+        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / 'examples'
+
+        with open(example_dir / 'config_example_map.yaml') as file:
+            conf_dict = yaml.load(file, Loader=yaml.FullLoader)
+        conf = Namespace(**conf_dict)
+        conf.map_path = str(example_dir / conf.map_path)
+
+        env = gym.make('f110_gym:f110-v0', map=conf.map_path, map_ext=conf.map_ext,
+                       num_agents=3, **kwargs)
+        return env
+
+    def test_original_obs_space(self):
+        """
+        Check backward compatibility with the original observation space.
+        """
+        env = self._make_env(observation_config={"type": "original"})
+
+        obs, _ = env.reset()
+
+        obs_keys = ['ego_idx', 'scans', 'poses_x', 'poses_y', 'poses_theta', 'linear_vels_x',
+                    'linear_vels_y', 'ang_vels_z', 'collisions', 'lap_times', 'lap_counts']
+
+        # check that the observation space has the correct types
+        self.assertTrue(all([isinstance(env.observation_space.spaces[k], Box) for k in obs_keys if k != 'ego_idx']))
+        self.assertTrue(all([env.observation_space.spaces[k].dtype == np.float32 for k in obs_keys if k != 'ego_idx']))
+
+        # check the observation space is a dict
+        self.assertTrue(isinstance(obs, dict))
+
+        # check that the observation has the correct keys
+        self.assertTrue(all([k in obs for k in obs_keys]))
+        self.assertTrue(all([k in obs_keys for k in obs]))
+        self.assertTrue(env.observation_space.contains(obs))
+
+    def test_features_observation(self):
+        """
+        Check the FeatureObservation allows to select an arbitrary subset of features.
+        """
+        features = ["pose_x", "pose_y", "pose_theta"]
+
+        env = self._make_env(observation_config={"type": "features", "features": features})
+
+        # check the observation space is a dict
+        self.assertTrue(isinstance(env.observation_space, gym.spaces.Dict))
+
+        # check that the observation space has the correct keys
+        for agent_id in env.agent_ids:
+            space = env.observation_space.spaces[agent_id].spaces
+            self.assertTrue(all([k in space for k in features]))
+            self.assertTrue(all([k in features for k in space]))
+
+        # check that the observation space has the correct types
+        for agent_id in env.agent_ids:
+            space = env.observation_space.spaces[agent_id].spaces
+            self.assertTrue(all([isinstance(space[k], Box) for k in features]))
+            self.assertTrue(all([space[k].dtype == np.float32 for k in features]))
+
+        # check the actual observation
+        obs, _ = env.reset()
+        obs, _, _, _, _ = env.step(env.action_space.sample())
+
+        for i, agent_id in enumerate(env.agent_ids):
+            pose_x, pose_y, pose_theta = env.sim.agent_poses[i]
+            obs_x, obs_y, obs_theta = obs[agent_id]["pose_x"], obs[agent_id]["pose_y"], obs[agent_id]["pose_theta"]
+
+            for ground_truth, observation in zip([pose_x, pose_y, pose_theta], [obs_x, obs_y, obs_theta]):
+                self.assertAlmostEqual(ground_truth, observation)
+
+    def test_unexisting_obs_space(self):
+        """
+        Check that an error is raised when an unexisting observation type is requested.
+        """
+        env = self._make_env()
+        with self.assertRaises(ValueError):
+            obs = observation_factory(env, vehicle_id=0, type="unexisting_obs_type")
+
+    def test_kinematic_obs_space(self):
+        """
+        Check the kinematic state observation space contains the correct features [x, y, theta, v].
+        """
+        env = self._make_env(observation_config={"type": "kinematic_state"})
+
+        kinematic_features = ["pose_x", "pose_y", "pose_theta", "linear_vel_x", "delta"]
+
+        # check kinematic features are in the observation space
+        for agent_id in env.agent_ids:
+            space = env.observation_space.spaces[agent_id].spaces
+            self.assertTrue(all([k in space for k in kinematic_features]))
+            self.assertTrue(all([k in kinematic_features for k in space]))
+
+        # check the actual observation
+        obs, _ = env.reset()
+        obs, _, _, _, _ = env.step(env.action_space.sample())
+
+        for i, agent_id in enumerate(env.agent_ids):
+            pose_x, pose_y, _, velx, pose_theta, _, _ = env.sim.agents[i].state
+            obs_x, obs_y, obs_theta = obs[agent_id]["pose_x"], obs[agent_id]["pose_y"], obs[agent_id]["pose_theta"]
+            obs_velx = obs[agent_id]["linear_vel_x"]
+
+            for ground_truth, observed in zip([pose_x, pose_y, pose_theta, velx],
+                                              [obs_x, obs_y, obs_theta, obs_velx]):
+                self.assertAlmostEqual(ground_truth, observed)
+
+    def test_dynamic_obs_space(self):
+        """
+        Check the dynamic state observation space contains the correct features.
+        """
+        env = self._make_env(observation_config={"type": "dynamic_state"})
+
+        kinematic_features = ["pose_x", "pose_y", "pose_theta", "linear_vel_x", "ang_vel_z", "delta", "beta"]
+
+        # check kinematic features are in the observation space
+        for agent_id in env.agent_ids:
+            space = env.observation_space.spaces[agent_id].spaces
+            self.assertTrue(all([k in space for k in kinematic_features]))
+            self.assertTrue(all([k in kinematic_features for k in space]))
+
+        # check the actual observation
+        obs, _ = env.reset()
+        obs, _, _, _, _ = env.step(env.action_space.sample())
+
+        for i, agent_id in enumerate(env.agent_ids):
+            pose_x, pose_y, delta, velx, pose_theta, _, beta = env.sim.agents[i].state
+
+            agent_obs = obs[agent_id]
+            obs_x, obs_y, obs_theta = agent_obs["pose_x"], agent_obs["pose_y"], agent_obs["pose_theta"]
+            obs_velx, obs_delta, obs_beta = agent_obs["linear_vel_x"], agent_obs["delta"], agent_obs["beta"]
+
+            for ground_truth, observed in zip([pose_x, pose_y, pose_theta, velx, delta, beta],
+                                              [obs_x, obs_y, obs_theta, obs_velx, obs_delta, obs_beta]):
+                self.assertAlmostEqual(ground_truth, observed)

From 71ceeaefad0672a1632bfed292a5868e175271f1 Mon Sep 17 00:00:00 2001
From: Luigi Berducci <luigi.berd@gmail.com>
Date: Fri, 23 Jun 2023 17:09:22 +0200
Subject: [PATCH 10/42] restructure map directory with subfolders adapt
 map_path in config_example_map.yaml

---
 examples/config_example_map.yaml                    |   2 +-
 gym/f110_gym/envs/laser_models.py                   |   6 +++---
 gym/f110_gym/{envs/maps => maps/Berlin}/berlin.png  | Bin
 gym/f110_gym/{envs/maps => maps/Berlin}/berlin.yaml |   0
 .../f110_gym/maps/Example}/example_map.png          | Bin
 .../f110_gym/maps/Example}/example_map.yaml         |   0
 gym/f110_gym/{envs/maps => maps/Levine}/levine.pgm  |   0
 gym/f110_gym/{envs/maps => maps/Levine}/levine.yaml |   0
 gym/f110_gym/{envs/maps => maps/Skirk}/skirk.png    | Bin
 gym/f110_gym/{envs/maps => maps/Skirk}/skirk.yaml   |   0
 .../maps => maps/StataBasement}/stata_basement.png  | Bin
 .../maps => maps/StataBasement}/stata_basement.yaml |   0
 gym/f110_gym/{envs/maps => maps/Vegas}/vegas.png    | Bin
 gym/f110_gym/{envs/maps => maps/Vegas}/vegas.yaml   |   0
 14 files changed, 4 insertions(+), 4 deletions(-)
 rename gym/f110_gym/{envs/maps => maps/Berlin}/berlin.png (100%)
 rename gym/f110_gym/{envs/maps => maps/Berlin}/berlin.yaml (100%)
 rename {examples => gym/f110_gym/maps/Example}/example_map.png (100%)
 rename {examples => gym/f110_gym/maps/Example}/example_map.yaml (100%)
 rename gym/f110_gym/{envs/maps => maps/Levine}/levine.pgm (100%)
 rename gym/f110_gym/{envs/maps => maps/Levine}/levine.yaml (100%)
 rename gym/f110_gym/{envs/maps => maps/Skirk}/skirk.png (100%)
 rename gym/f110_gym/{envs/maps => maps/Skirk}/skirk.yaml (100%)
 rename gym/f110_gym/{envs/maps => maps/StataBasement}/stata_basement.png (100%)
 rename gym/f110_gym/{envs/maps => maps/StataBasement}/stata_basement.yaml (100%)
 rename gym/f110_gym/{envs/maps => maps/Vegas}/vegas.png (100%)
 rename gym/f110_gym/{envs/maps => maps/Vegas}/vegas.yaml (100%)

diff --git a/examples/config_example_map.yaml b/examples/config_example_map.yaml
index 14b8702c..5228fac5 100644
--- a/examples/config_example_map.yaml
+++ b/examples/config_example_map.yaml
@@ -4,7 +4,7 @@ run_name: 'map_wide'
 perf_num: 6
 
 # map paths
-map_path: './example_map'
+map_path: '../gym/f110_gym/maps/Example/example_map'
 map_ext: '.png'
 
 # starting pose for map
diff --git a/gym/f110_gym/envs/laser_models.py b/gym/f110_gym/envs/laser_models.py
index 158060b1..1cef17f9 100644
--- a/gym/f110_gym/envs/laser_models.py
+++ b/gym/f110_gym/envs/laser_models.py
@@ -536,7 +536,7 @@ def test_fps(self):
 
         scan_rng = np.random.default_rng(seed=12345)
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
-        map_path = '../envs/maps/berlin.yaml'
+        map_path = '../maps/Berlin/berlin.yaml'
         map_ext = '.png'
         scan_sim.set_map(map_path, map_ext)
         
@@ -554,7 +554,7 @@ def test_fps(self):
     def test_rng(self):
         num_beams = 1080
         fov = 4.7
-        map_path = '../envs/maps/berlin.yaml'
+        map_path = '../maps/Berlin/berlin.yaml'
         map_ext = '.png'
         it = 100
 
@@ -584,7 +584,7 @@ def main():
     num_beams = 1080
     fov = 4.7
     # map_path = '../envs/maps/berlin.yaml'
-    map_path = '../../../examples/example_map.yaml'
+    map_path = '../maps/Example/example_map.yaml'
     map_ext = '.png'
     scan_rng = np.random.default_rng(seed=12345)
     scan_sim = ScanSimulator2D(num_beams, fov)
diff --git a/gym/f110_gym/envs/maps/berlin.png b/gym/f110_gym/maps/Berlin/berlin.png
similarity index 100%
rename from gym/f110_gym/envs/maps/berlin.png
rename to gym/f110_gym/maps/Berlin/berlin.png
diff --git a/gym/f110_gym/envs/maps/berlin.yaml b/gym/f110_gym/maps/Berlin/berlin.yaml
similarity index 100%
rename from gym/f110_gym/envs/maps/berlin.yaml
rename to gym/f110_gym/maps/Berlin/berlin.yaml
diff --git a/examples/example_map.png b/gym/f110_gym/maps/Example/example_map.png
similarity index 100%
rename from examples/example_map.png
rename to gym/f110_gym/maps/Example/example_map.png
diff --git a/examples/example_map.yaml b/gym/f110_gym/maps/Example/example_map.yaml
similarity index 100%
rename from examples/example_map.yaml
rename to gym/f110_gym/maps/Example/example_map.yaml
diff --git a/gym/f110_gym/envs/maps/levine.pgm b/gym/f110_gym/maps/Levine/levine.pgm
similarity index 100%
rename from gym/f110_gym/envs/maps/levine.pgm
rename to gym/f110_gym/maps/Levine/levine.pgm
diff --git a/gym/f110_gym/envs/maps/levine.yaml b/gym/f110_gym/maps/Levine/levine.yaml
similarity index 100%
rename from gym/f110_gym/envs/maps/levine.yaml
rename to gym/f110_gym/maps/Levine/levine.yaml
diff --git a/gym/f110_gym/envs/maps/skirk.png b/gym/f110_gym/maps/Skirk/skirk.png
similarity index 100%
rename from gym/f110_gym/envs/maps/skirk.png
rename to gym/f110_gym/maps/Skirk/skirk.png
diff --git a/gym/f110_gym/envs/maps/skirk.yaml b/gym/f110_gym/maps/Skirk/skirk.yaml
similarity index 100%
rename from gym/f110_gym/envs/maps/skirk.yaml
rename to gym/f110_gym/maps/Skirk/skirk.yaml
diff --git a/gym/f110_gym/envs/maps/stata_basement.png b/gym/f110_gym/maps/StataBasement/stata_basement.png
similarity index 100%
rename from gym/f110_gym/envs/maps/stata_basement.png
rename to gym/f110_gym/maps/StataBasement/stata_basement.png
diff --git a/gym/f110_gym/envs/maps/stata_basement.yaml b/gym/f110_gym/maps/StataBasement/stata_basement.yaml
similarity index 100%
rename from gym/f110_gym/envs/maps/stata_basement.yaml
rename to gym/f110_gym/maps/StataBasement/stata_basement.yaml
diff --git a/gym/f110_gym/envs/maps/vegas.png b/gym/f110_gym/maps/Vegas/vegas.png
similarity index 100%
rename from gym/f110_gym/envs/maps/vegas.png
rename to gym/f110_gym/maps/Vegas/vegas.png
diff --git a/gym/f110_gym/envs/maps/vegas.yaml b/gym/f110_gym/maps/Vegas/vegas.yaml
similarity index 100%
rename from gym/f110_gym/envs/maps/vegas.yaml
rename to gym/f110_gym/maps/Vegas/vegas.yaml

From 68dbc6b26e0d33c2ae350c095859f9f2f230d9a2 Mon Sep 17 00:00:00 2001
From: Luigi Berducci <luigi.berd@gmail.com>
Date: Fri, 23 Jun 2023 17:36:55 +0200
Subject: [PATCH 11/42] rename map filenames add track loading and test add
 dependency for yaml dataclass

---
 gym/f110_gym/envs/f110_env.py                 |   8 +-
 gym/f110_gym/envs/f110_env_backup.py          |   2 +-
 gym/f110_gym/envs/laser_models.py             |  12 +--
 gym/f110_gym/envs/track.py                    |  87 ++++++++++++++++++
 .../Berlin/{berlin.png => Berlin_map.png}     | Bin
 .../Berlin/{berlin.yaml => Berlin_map.yaml}   |   2 +-
 .../{example_map.png => Example_map.png}      | Bin
 .../{example_map.yaml => Example_map.yaml}    |   2 +-
 .../Levine/{levine.pgm => Levine_map.pgm}     |   0
 .../Levine/{levine.yaml => Levine_map.yaml}   |   2 +-
 .../maps/Skirk/{skirk.png => Skirk_map.png}   | Bin
 .../maps/Skirk/{skirk.yaml => Skirk_map.yaml} |   2 +-
 ...ata_basement.png => StataBasement_map.png} | Bin
 ...a_basement.yaml => StataBasement_map.yaml} |   2 +-
 .../maps/Vegas/{vegas.png => Vegas_map.png}   | Bin
 .../maps/Vegas/{vegas.yaml => Vegas_map.yaml} |   2 +-
 gym/f110_gym/unittest/legacy_scan_gen.py      |   6 +-
 gym/f110_gym/unittest/scan_sim.py             |   8 +-
 gym/f110_gym/unittest/track_test.py           |  14 +++
 setup.py                                      |   4 +-
 20 files changed, 128 insertions(+), 25 deletions(-)
 create mode 100644 gym/f110_gym/envs/track.py
 rename gym/f110_gym/maps/Berlin/{berlin.png => Berlin_map.png} (100%)
 rename gym/f110_gym/maps/Berlin/{berlin.yaml => Berlin_map.yaml} (84%)
 rename gym/f110_gym/maps/Example/{example_map.png => Example_map.png} (100%)
 rename gym/f110_gym/maps/Example/{example_map.yaml => Example_map.yaml} (84%)
 rename gym/f110_gym/maps/Levine/{levine.pgm => Levine_map.pgm} (100%)
 rename gym/f110_gym/maps/Levine/{levine.yaml => Levine_map.yaml} (84%)
 rename gym/f110_gym/maps/Skirk/{skirk.png => Skirk_map.png} (100%)
 rename gym/f110_gym/maps/Skirk/{skirk.yaml => Skirk_map.yaml} (83%)
 rename gym/f110_gym/maps/StataBasement/{stata_basement.png => StataBasement_map.png} (100%)
 rename gym/f110_gym/maps/StataBasement/{stata_basement.yaml => StataBasement_map.yaml} (78%)
 rename gym/f110_gym/maps/Vegas/{vegas.png => Vegas_map.png} (100%)
 rename gym/f110_gym/maps/Vegas/{vegas.yaml => Vegas_map.yaml} (84%)
 create mode 100644 gym/f110_gym/unittest/track_test.py

diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 66b728c3..09908d2b 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -110,15 +110,15 @@ def __init__(self, render_mode=None, **kwargs):
             self.map_name = kwargs['map']
             # different default maps
             if self.map_name == 'berlin':
-                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/berlin.yaml'
+                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/Berlin_map.yaml'
             elif self.map_name == 'skirk':
-                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/skirk.yaml'
+                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/Skirk_map.yaml'
             elif self.map_name == 'levine':
-                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/levine.yaml'
+                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/Levine_map.yaml'
             else:
                 self.map_path = self.map_name + '.yaml'
         except:
-            self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/vegas.yaml'
+            self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/Vegas_map.yaml'
 
         try:
             self.map_ext = kwargs['map_ext']
diff --git a/gym/f110_gym/envs/f110_env_backup.py b/gym/f110_gym/envs/f110_env_backup.py
index fa818b6f..6744d0a0 100644
--- a/gym/f110_gym/envs/f110_env_backup.py
+++ b/gym/f110_gym/envs/f110_env_backup.py
@@ -79,7 +79,7 @@ def __init__(self):
         self.map_img = None
 
         # current_dir = os.path.dirname(os.path.abspath(__file__))
-        # map_path = current_dir + '/../../../maps/levine.yaml'
+        # map_path = current_dir + '/../../../maps/Levine_map.yaml'
 
         # default
         self.ego_idx = 0
diff --git a/gym/f110_gym/envs/laser_models.py b/gym/f110_gym/envs/laser_models.py
index 1cef17f9..d121f279 100644
--- a/gym/f110_gym/envs/laser_models.py
+++ b/gym/f110_gym/envs/laser_models.py
@@ -486,7 +486,7 @@ def setUp(self):
     #     scan_rng = np.random.default_rng(seed=12345)
     #     scan_sim = ScanSimulator2D(self.num_beams, self.fov)
     #     new_berlin = np.empty((self.num_test, self.num_beams))
-    #     map_path = '../../../maps/berlin.yaml'
+    #     map_path = '../../../maps/Berlin_map.yaml'
     #     map_ext = '.png'
     #     scan_sim.set_map(map_path, map_ext)
     #     # scan gen loop
@@ -510,7 +510,7 @@ def setUp(self):
     #     scan_rng = np.random.default_rng(seed=12345)
     #     scan_sim = ScanSimulator2D(self.num_beams, self.fov)
     #     new_skirk = np.empty((self.num_test, self.num_beams))
-    #     map_path = '../../../maps/skirk.yaml'
+    #     map_path = '../../../maps/Skirk_map.yaml'
     #     map_ext = '.png'
     #     scan_sim.set_map(map_path, map_ext)
     #     print('map set')
@@ -536,7 +536,7 @@ def test_fps(self):
 
         scan_rng = np.random.default_rng(seed=12345)
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
-        map_path = '../maps/Berlin/berlin.yaml'
+        map_path = '../maps/Berlin/Berlin_map.yaml'
         map_ext = '.png'
         scan_sim.set_map(map_path, map_ext)
         
@@ -554,7 +554,7 @@ def test_fps(self):
     def test_rng(self):
         num_beams = 1080
         fov = 4.7
-        map_path = '../maps/Berlin/berlin.yaml'
+        map_path = '../maps/Berlin/Berlin_map.yaml'
         map_ext = '.png'
         it = 100
 
@@ -583,8 +583,8 @@ def test_rng(self):
 def main():
     num_beams = 1080
     fov = 4.7
-    # map_path = '../envs/maps/berlin.yaml'
-    map_path = '../maps/Example/example_map.yaml'
+    # map_path = '../envs/maps/Berlin_map.yaml'
+    map_path = '../maps/Example/Example_map.yaml'
     map_ext = '.png'
     scan_rng = np.random.default_rng(seed=12345)
     scan_sim = ScanSimulator2D(num_beams, fov)
diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
new file mode 100644
index 00000000..96cd1c90
--- /dev/null
+++ b/gym/f110_gym/envs/track.py
@@ -0,0 +1,87 @@
+import pathlib
+from dataclasses import dataclass
+from typing import Tuple
+
+import numpy as np
+import yaml
+from PIL import Image
+from PIL.Image import Transpose
+from yamldataclassconfig.config import YamlDataClassConfig
+
+
+@dataclass
+class TrackSpec(YamlDataClassConfig):
+    name: str
+    image: str
+    resolution: float
+    origin: Tuple[float, float, float]
+    negate: int
+    occupied_thresh: float
+    free_thresh: float
+
+
+def find_track_dir(track_name):
+    # we assume there are no blank space in the track name. however, to take into account eventual blank spaces in
+    # the map dirpath, we loop over all possible maps and check if there is a matching with the current track
+    map_dir = pathlib.Path(__file__).parent.parent / "maps"
+
+    for base_dir in [map_dir]:
+        if not base_dir.exists():
+            continue
+
+        for dir in base_dir.iterdir():
+            if track_name == str(dir.stem).replace(" ", ""):
+                return dir
+
+    raise FileNotFoundError(f'no mapdir matching {track_name} in {[map_dir]}')
+
+
+@dataclass
+class Track:
+    spec: TrackSpec
+    filepath: str
+    ext: str
+    occupancy_map: np.ndarray
+
+    def __init__(
+            self,
+            spec: TrackSpec,
+            filepath: str,
+            ext: str,
+            occupancy_map: np.ndarray,
+    ):
+        self.spec = spec
+        self.filepath = filepath
+        self.ext = ext
+        self.occupancy_map = occupancy_map
+
+    @staticmethod
+    def from_track_name(track: str):
+        try:
+            track_dir = find_track_dir(track)
+            # load track spec
+            with open(track_dir / f"{track}_map.yaml", 'r') as yaml_stream:
+                map_metadata = yaml.safe_load(yaml_stream)
+                track_spec = TrackSpec(name=track, **map_metadata)
+
+            # load occupancy grid
+            map_filename = pathlib.Path(track_spec.image)
+            image = Image.open(track_dir / str(map_filename)).transpose(Transpose.FLIP_TOP_BOTTOM)
+            occupancy_map = np.array(image).astype(np.float32)
+            occupancy_map[occupancy_map <= 128] = 0.0
+            occupancy_map[occupancy_map > 128] = 255.0
+
+            return Track(
+                spec=track_spec,
+                filepath=str((track_dir / map_filename.stem).absolute()),
+                ext=map_filename.suffix,
+                occupancy_map=occupancy_map,
+            )
+
+        except Exception as ex:
+            raise FileNotFoundError(f"could not load track {track}") from ex
+
+
+if __name__ == "__main__":
+    track = Track.from_track_name("Example")
+    print("[Result] map loaded successfully")
diff --git a/gym/f110_gym/maps/Berlin/berlin.png b/gym/f110_gym/maps/Berlin/Berlin_map.png
similarity index 100%
rename from gym/f110_gym/maps/Berlin/berlin.png
rename to gym/f110_gym/maps/Berlin/Berlin_map.png
diff --git a/gym/f110_gym/maps/Berlin/berlin.yaml b/gym/f110_gym/maps/Berlin/Berlin_map.yaml
similarity index 84%
rename from gym/f110_gym/maps/Berlin/berlin.yaml
rename to gym/f110_gym/maps/Berlin/Berlin_map.yaml
index 55dce04d..15640f38 100644
--- a/gym/f110_gym/maps/Berlin/berlin.yaml
+++ b/gym/f110_gym/maps/Berlin/Berlin_map.yaml
@@ -1,4 +1,4 @@
-image: berlin.png
+image: Berlin_map.png
 resolution: 0.050000
 origin: [-11.606540, -26.520793, 0.000000]
 negate: 0
diff --git a/gym/f110_gym/maps/Example/example_map.png b/gym/f110_gym/maps/Example/Example_map.png
similarity index 100%
rename from gym/f110_gym/maps/Example/example_map.png
rename to gym/f110_gym/maps/Example/Example_map.png
diff --git a/gym/f110_gym/maps/Example/example_map.yaml b/gym/f110_gym/maps/Example/Example_map.yaml
similarity index 84%
rename from gym/f110_gym/maps/Example/example_map.yaml
rename to gym/f110_gym/maps/Example/Example_map.yaml
index 9e0d29ac..60983203 100644
--- a/gym/f110_gym/maps/Example/example_map.yaml
+++ b/gym/f110_gym/maps/Example/Example_map.yaml
@@ -1,4 +1,4 @@
-image: example_map.png
+image: Example_map.png
 resolution: 0.062500
 origin: [-78.21853769831466,-44.37590462453829, 0.000000]
 negate: 0
diff --git a/gym/f110_gym/maps/Levine/levine.pgm b/gym/f110_gym/maps/Levine/Levine_map.pgm
similarity index 100%
rename from gym/f110_gym/maps/Levine/levine.pgm
rename to gym/f110_gym/maps/Levine/Levine_map.pgm
diff --git a/gym/f110_gym/maps/Levine/levine.yaml b/gym/f110_gym/maps/Levine/Levine_map.yaml
similarity index 84%
rename from gym/f110_gym/maps/Levine/levine.yaml
rename to gym/f110_gym/maps/Levine/Levine_map.yaml
index b944592f..3646dcd1 100644
--- a/gym/f110_gym/maps/Levine/levine.yaml
+++ b/gym/f110_gym/maps/Levine/Levine_map.yaml
@@ -1,4 +1,4 @@
-image: levine.png
+image: Levine_map.pgm
 resolution: 0.050000
 origin: [-51.224998, -51.224998, 0.000000]
 negate: 0
diff --git a/gym/f110_gym/maps/Skirk/skirk.png b/gym/f110_gym/maps/Skirk/Skirk_map.png
similarity index 100%
rename from gym/f110_gym/maps/Skirk/skirk.png
rename to gym/f110_gym/maps/Skirk/Skirk_map.png
diff --git a/gym/f110_gym/maps/Skirk/skirk.yaml b/gym/f110_gym/maps/Skirk/Skirk_map.yaml
similarity index 83%
rename from gym/f110_gym/maps/Skirk/skirk.yaml
rename to gym/f110_gym/maps/Skirk/Skirk_map.yaml
index c7d284b7..fe794f67 100644
--- a/gym/f110_gym/maps/Skirk/skirk.yaml
+++ b/gym/f110_gym/maps/Skirk/Skirk_map.yaml
@@ -1,4 +1,4 @@
-image: skirk.png
+image: Skirk_map.png
 resolution: 0.050000
 origin: [-7.801, -16.388, 0.000000]
 negate: 0
diff --git a/gym/f110_gym/maps/StataBasement/stata_basement.png b/gym/f110_gym/maps/StataBasement/StataBasement_map.png
similarity index 100%
rename from gym/f110_gym/maps/StataBasement/stata_basement.png
rename to gym/f110_gym/maps/StataBasement/StataBasement_map.png
diff --git a/gym/f110_gym/maps/StataBasement/stata_basement.yaml b/gym/f110_gym/maps/StataBasement/StataBasement_map.yaml
similarity index 78%
rename from gym/f110_gym/maps/StataBasement/stata_basement.yaml
rename to gym/f110_gym/maps/StataBasement/StataBasement_map.yaml
index f6c62fb1..dbf4ab49 100644
--- a/gym/f110_gym/maps/StataBasement/stata_basement.yaml
+++ b/gym/f110_gym/maps/StataBasement/StataBasement_map.yaml
@@ -1,4 +1,4 @@
-image: stata_basement.png
+image: StataBasement_map.png
 resolution: 0.0504
 origin: [-26.900000, -16.50000, 0.0]
 negate: 0
diff --git a/gym/f110_gym/maps/Vegas/vegas.png b/gym/f110_gym/maps/Vegas/Vegas_map.png
similarity index 100%
rename from gym/f110_gym/maps/Vegas/vegas.png
rename to gym/f110_gym/maps/Vegas/Vegas_map.png
diff --git a/gym/f110_gym/maps/Vegas/vegas.yaml b/gym/f110_gym/maps/Vegas/Vegas_map.yaml
similarity index 84%
rename from gym/f110_gym/maps/Vegas/vegas.yaml
rename to gym/f110_gym/maps/Vegas/Vegas_map.yaml
index b0acfb2c..ad45e120 100644
--- a/gym/f110_gym/maps/Vegas/vegas.yaml
+++ b/gym/f110_gym/maps/Vegas/Vegas_map.yaml
@@ -1,4 +1,4 @@
-image: vegas.png
+image: Vegas_map.png
 resolution: 0.050000
 origin: [-11.606540, -27.320793, 0.000000]
 negate: 0
diff --git a/gym/f110_gym/unittest/legacy_scan_gen.py b/gym/f110_gym/unittest/legacy_scan_gen.py
index 045d50d7..20aa69db 100644
--- a/gym/f110_gym/unittest/legacy_scan_gen.py
+++ b/gym/f110_gym/unittest/legacy_scan_gen.py
@@ -57,7 +57,7 @@
 test_poses[:, 2] = np.linspace(-1., 1., num=num_test)
 
 # map 1: vegas
-map_path = '../../../maps/vegas.yaml'
+map_path = '../../../maps/Vegas_map.yaml'
 map_ext = '.png'
 racecar_env = gym.make('f110_gym:f110-v0')
 racecar_env.init_map(map_path, map_ext, False, False)
@@ -71,7 +71,7 @@
     vegas_scan[i,:] = obs['scans'][0]
 
 # map 2: berlin
-map_path = '../../../maps/berlin.yaml'
+map_path = '../../../maps/Berlin_map.yaml'
 map_ext = '.png'
 racecar_env = gym.make('f110_gym:f110-v0')
 racecar_env.init_map(map_path, map_ext, False, False)
@@ -85,7 +85,7 @@
     berlin_scan[i,:] = obs['scans'][0]
 
 # map 3: skirk
-map_path = '../../../maps/skirk.yaml'
+map_path = '../../../maps/Skirk_map.yaml'
 map_ext = '.png'
 racecar_env = gym.make('f110_gym:f110-v0')
 racecar_env.init_map(map_path, map_ext, False, False)
diff --git a/gym/f110_gym/unittest/scan_sim.py b/gym/f110_gym/unittest/scan_sim.py
index 7f203bfe..9cb89aee 100644
--- a/gym/f110_gym/unittest/scan_sim.py
+++ b/gym/f110_gym/unittest/scan_sim.py
@@ -321,7 +321,7 @@ def setUp(self):
     def test_map_berlin(self):
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
         new_berlin = np.empty((self.num_test, self.num_beams))
-        map_path = '../../../maps/berlin.yaml'
+        map_path = '../../../maps/Berlin_map.yaml'
         map_ext = '.png'
         scan_sim.set_map(map_path, map_ext)
         # scan gen loop
@@ -344,7 +344,7 @@ def test_map_berlin(self):
     def test_map_skirk(self):
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
         new_skirk = np.empty((self.num_test, self.num_beams))
-        map_path = '../../../maps/skirk.yaml'
+        map_path = '../../../maps/Skirk_map.yaml'
         map_ext = '.png'
         scan_sim.set_map(map_path, map_ext)
         print('map set')
@@ -368,7 +368,7 @@ def test_map_skirk(self):
     def test_fps(self):
         # scan fps should be greater than 500
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
-        map_path = '../../../maps/skirk.yaml'
+        map_path = '../../../maps/Skirk_map.yaml'
         map_ext = '.png'
         scan_sim.set_map(map_path, map_ext)
         
@@ -387,7 +387,7 @@ def test_fps(self):
 def main():
     num_beams = 1080
     fov = 4.7
-    # map_path = '../envs/maps/berlin.yaml'
+    # map_path = '../envs/maps/Berlin_map.yaml'
     map_path = '/home/f1tenth-eval/tunercar/es/maps/map0.yaml'
     map_ext = '.png'
     scan_sim = ScanSimulator2D(num_beams, fov)
diff --git a/gym/f110_gym/unittest/track_test.py b/gym/f110_gym/unittest/track_test.py
new file mode 100644
index 00000000..410712f1
--- /dev/null
+++ b/gym/f110_gym/unittest/track_test.py
@@ -0,0 +1,14 @@
+import unittest
+from f110_gym.envs.track import Track
+
+
+class TestTrack(unittest.TestCase):
+    def test_loading_default_tracks(self):
+        track_names = ["Berlin", "Example", "Levine", "Skirk", "StataBasement", "Vegas"]
+        for track_name in track_names:
+            track = Track.from_track_name(track_name)
+            self.assertEqual(track.spec.name, track_name)
+
+    def test_error_handling(self):
+        wrong_track_name = "i_dont_exists"
+        self.assertRaises(FileNotFoundError, Track.from_track_name, wrong_track_name)
diff --git a/setup.py b/setup.py
index 054546fe..7dfc0cc3 100644
--- a/setup.py
+++ b/setup.py
@@ -14,5 +14,7 @@
           'numba>=0.55.2',
           'pyyaml>=5.3.1',
           'pyglet<1.5',
-          'pyopengl']
+          'pyopengl',
+          'yamldataclassconfig'
+      ]
       )

From 306c34b0a3381acd22dc73b971155d6d9c550ca0 Mon Sep 17 00:00:00 2001
From: Luigi Berducci <luigi.berd@gmail.com>
Date: Fri, 23 Jun 2023 18:28:42 +0200
Subject: [PATCH 12/42] make use of track in gym env refactor simulator
 accordingly refactoring example, waypoint follower

---
 examples/config_example_map.yaml              |   5 +-
 examples/waypoint_follow.py                   |  21 +-
 gym/f110_gym/envs/base_classes.py             |  14 +-
 gym/f110_gym/envs/cubic_spline.py             | 224 ++++++++++++++++++
 gym/f110_gym/envs/f110_env.py                 |  33 +--
 gym/f110_gym/envs/laser_models.py             |  32 +--
 gym/f110_gym/envs/rendering.py                |  24 +-
 gym/f110_gym/envs/track.py                    | 116 ++++++++-
 .../maps/Example/Example_raceline.csv         |   0
 gym/f110_gym/unittest/gym_api_test.py         |  10 +-
 gym/f110_gym/unittest/track_test.py           |   2 +-
 11 files changed, 390 insertions(+), 91 deletions(-)
 create mode 100644 gym/f110_gym/envs/cubic_spline.py
 rename examples/example_waypoints.csv => gym/f110_gym/maps/Example/Example_raceline.csv (100%)

diff --git a/examples/config_example_map.yaml b/examples/config_example_map.yaml
index 5228fac5..7fcc00aa 100644
--- a/examples/config_example_map.yaml
+++ b/examples/config_example_map.yaml
@@ -4,8 +4,7 @@ run_name: 'map_wide'
 perf_num: 6
 
 # map paths
-map_path: '../gym/f110_gym/maps/Example/example_map'
-map_ext: '.png'
+map_name: 'Example'
 
 # starting pose for map
 sx: 0.7
@@ -13,7 +12,7 @@ sy: 0.0
 stheta: 1.37079632679
 
 # raceline path and indices
-wpt_path: './example_waypoints.csv'
+wpt_path: './Example_raceline.csv'
 wpt_delim: ';'
 wpt_rowskip: 3
 wpt_xind: 1
diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py
index 71a40cbf..ec65a727 100644
--- a/examples/waypoint_follow.py
+++ b/examples/waypoint_follow.py
@@ -154,10 +154,10 @@ class PurePursuitPlanner:
     Example Planner
     """
 
-    def __init__(self, conf, wb):
+    def __init__(self, track, conf, wb):
         self.wheelbase = wb
-        self.conf = conf
-        self.load_waypoints(conf)
+        self.conf = conf    # todo: remove this
+        self.waypoints = np.stack([track.raceline.xs, track.raceline.ys, track.raceline.vxs]).T
         self.max_reacquire = 20.
 
         self.drawn_waypoints = []
@@ -177,7 +177,7 @@ def render_waypoints(self, e):
 
         # points = self.waypoints
 
-        points = np.vstack((self.waypoints[:, self.conf.wpt_xind], self.waypoints[:, self.conf.wpt_yind])).T
+        points = self.waypoints[:, :2]
 
         scaled_points = 50. * points
 
@@ -193,7 +193,7 @@ def _get_current_waypoint(self, waypoints, lookahead_distance, position, theta):
         """
         gets the current waypoint to follow
         """
-        wpts = np.vstack((self.waypoints[:, self.conf.wpt_xind], self.waypoints[:, self.conf.wpt_yind])).T
+        wpts = waypoints[:, :2]
         lookahead_distance = np.float32(lookahead_distance)
         nearest_point, nearest_dist, t, i = nearest_point_on_trajectory(position, wpts)
         if nearest_dist < lookahead_distance:
@@ -206,7 +206,7 @@ def _get_current_waypoint(self, waypoints, lookahead_distance, position, theta):
             # x, y
             current_waypoint[0:2] = wpts[i2, :]
             # speed
-            current_waypoint[2] = waypoints[i, self.conf.wpt_vind]
+            current_waypoint[2] = waypoints[i, -1]
             return current_waypoint
         elif nearest_dist < self.max_reacquire:
             # NOTE: specify type or numba complains
@@ -264,7 +264,10 @@ def main():
         conf_dict = yaml.load(file, Loader=yaml.FullLoader)
     conf = Namespace(**conf_dict)
 
-    planner = PurePursuitPlanner(conf, (0.17145 + 0.15875))  # FlippyPlanner(speed=0.2, flip_every=1, steer=10)
+    env = gym.make('f110_gym:f110-v0', map=conf.map_name, num_agents=1,
+                   timestep=0.01, integrator=Integrator.RK4, render_mode='human')
+
+    planner = PurePursuitPlanner(track=env.track, conf=conf, wb=0.17145 + 0.15875)  # FlippyPlanner(speed=0.2, flip_every=1, steer=10)
 
     def render_callback(env_renderer):
         # custom extra drawing function
@@ -284,9 +287,7 @@ def render_callback(env_renderer):
 
         planner.render_waypoints(env_renderer)
 
-    env = gym.make('f110_gym:f110-v0', map=conf.map_path, map_ext=conf.map_ext,
-                   num_agents=1, timestep=0.01, integrator=Integrator.RK4,
-                   render_mode='human')
+
     env.add_render_callback(render_callback)
 
     poses = np.array([[conf.sx, conf.sy, conf.stheta]])
diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index 192e32eb..9e3ccb9d 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -168,15 +168,14 @@ def update_params(self, params):
         """
         self.params = params
     
-    def set_map(self, map_path, map_ext):
+    def set_map(self, map_name: str):
         """
         Sets the map for scan simulator
         
         Args:
-            map_path (str): absolute path to the map yaml file
-            map_ext (str): extension of the map image file
+            map_name (str): name of the map
         """
-        RaceCar.scan_simulator.set_map(map_path, map_ext)
+        RaceCar.scan_simulator.set_map(map_name)
 
     def reset(self, pose):
         """
@@ -489,19 +488,18 @@ def __init__(self, params, num_agents, seed, time_step=0.01, ego_idx=0, integrat
                 agent = RaceCar(params, self.seed, is_ego=False, time_step=self.time_step, integrator=integrator)
                 self.agents.append(agent)
 
-    def set_map(self, map_path, map_ext):
+    def set_map(self, map_name):
         """
         Sets the map of the environment and sets the map for scan simulator of each agent
 
         Args:
-            map_path (str): path to the map yaml file
-            map_ext (str): extension for the map image file
+            map_name (str): name of the map
 
         Returns:
             None
         """
         for agent in self.agents:
-            agent.set_map(map_path, map_ext)
+            agent.set_map(map_name)
 
 
     def update_params(self, params, agent_idx=-1):
diff --git a/gym/f110_gym/envs/cubic_spline.py b/gym/f110_gym/envs/cubic_spline.py
new file mode 100644
index 00000000..ba75add9
--- /dev/null
+++ b/gym/f110_gym/envs/cubic_spline.py
@@ -0,0 +1,224 @@
+"""
+Code from Cubic spline planner
+Author: Atsushi Sakai(@Atsushi_twi)
+"""
+import math
+import numpy as np
+import bisect
+
+
+class CubicSpline1D:
+    """
+    1D Cubic Spline class
+    Parameters
+    ----------
+    x : list
+        x coordinates for data points. This x coordinates must be
+        sorted in ascending order.
+    y : list
+        y coordinates for data points
+    """
+
+    def __init__(self, x, y):
+
+        h = np.diff(x)
+        if np.any(h < 0):
+            raise ValueError("x coordinates must be sorted in ascending order")
+
+        self.a, self.b, self.c, self.d = [], [], [], []
+        self.x = x
+        self.y = y
+        self.nx = len(x)  # dimension of x
+
+        # calc coefficient a
+        self.a = [iy for iy in y]
+
+        # calc coefficient c
+        A = self.__calc_A(h)
+        B = self.__calc_B(h, self.a)
+        self.c = np.linalg.solve(A, B)
+
+        # calc spline coefficient b and d
+        for i in range(self.nx - 1):
+            d = (self.c[i + 1] - self.c[i]) / (3.0 * h[i])
+            b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) \
+                - h[i] / 3.0 * (2.0 * self.c[i] + self.c[i + 1])
+            self.d.append(d)
+            self.b.append(b)
+
+    def calc_position(self, x):
+        """
+        Calc `y` position for given `x`.
+        if `x` is outside the data point's `x` range, return None.
+        Returns
+        -------
+        y : float
+            y position for given x.
+        """
+        if x < self.x[0]:
+            return None
+        elif x > self.x[-1]:
+            return None
+
+        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
+
+        return position
+
+    def calc_first_derivative(self, x):
+        """
+        Calc first derivative at given x.
+        if x is outside the input x, return None
+        Returns
+        -------
+        dy : float
+            first derivative for given x.
+        """
+
+        if x < self.x[0]:
+            return None
+        elif x > self.x[-1]:
+            return None
+
+        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
+        return dy
+
+    def calc_second_derivative(self, x):
+        """
+        Calc second derivative at given x.
+        if x is outside the input x, return None
+        Returns
+        -------
+        ddy : float
+            second derivative for given x.
+        """
+
+        if x < self.x[0]:
+            return None
+        elif x > self.x[-1]:
+            return None
+
+        i = self.__search_index(x)
+        dx = x - self.x[i]
+        ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
+        return ddy
+
+    def __search_index(self, x):
+        """
+        search data segment index
+        """
+        return bisect.bisect(self.x[:-1], x) - 1
+
+    def __calc_A(self, h):
+        """
+        calc matrix A for spline coefficient c
+        """
+        A = np.zeros((self.nx, self.nx))
+        A[0, 0] = 1.0
+        for i in range(self.nx - 1):
+            if i != (self.nx - 2):
+                A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
+            A[i + 1, i] = h[i]
+            A[i, i + 1] = h[i]
+
+        A[0, 1] = 0.0
+        A[self.nx - 1, self.nx - 2] = 0.0
+        A[self.nx - 1, self.nx - 1] = 1.0
+        return A
+
+    def __calc_B(self, h, a):
+        """
+        calc matrix B for spline coefficient c
+        """
+        B = np.zeros(self.nx)
+        for i in range(self.nx - 2):
+            B[i + 1] = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1] \
+                       - 3.0 * (a[i + 1] - a[i]) / h[i]
+        return B
+
+
+class CubicSpline2D:
+    """
+    Cubic CubicSpline2D class
+    Parameters
+    ----------
+    x : list
+        x coordinates for data points.
+    y : list
+        y coordinates for data points.
+    """
+
+    def __init__(self, x, y):
+        self.s = self.__calc_s(x, y)
+        self.sx = CubicSpline1D(self.s, x)
+        self.sy = CubicSpline1D(self.s, y)
+
+    def __calc_s(self, x, y):
+        dx = np.diff(x)
+        dy = np.diff(y)
+        self.ds = np.hypot(dx, dy)
+        s = [0]
+        s.extend(np.cumsum(self.ds))
+        return s
+
+    def calc_position(self, s):
+        """
+        calc position
+        Parameters
+        ----------
+        s : float
+            distance from the start point. if `s` is outside the data point's
+            range, return None.
+        Returns
+        -------
+        x : float
+            x position for given s.
+        y : float
+            y position for given s.
+        """
+        x = self.sx.calc_position(s)
+        y = self.sy.calc_position(s)
+
+        return x, y
+
+    def calc_curvature(self, s):
+        """
+        calc curvature
+        Parameters
+        ----------
+        s : float
+            distance from the start point. if `s` is outside the data point's
+            range, return None.
+        Returns
+        -------
+        k : float
+            curvature for given s.
+        """
+        dx = self.sx.calc_first_derivative(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))
+        return k
+
+    def calc_yaw(self, s):
+        """
+        calc yaw
+        Parameters
+        ----------
+        s : float
+            distance from the start point. if `s` is outside the data point's
+            range, return None.
+        Returns
+        -------
+        yaw : float
+            yaw angle (tangent vector) for given s.
+        """
+        dx = self.sx.calc_first_derivative(s)
+        dy = self.sy.calc_first_derivative(s)
+        yaw = math.atan2(dy, dx)
+        return yaw
diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 09908d2b..384897a1 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -27,6 +27,7 @@
 # gym imports
 import gymnasium as gym
 
+from f110_gym.envs.track import Track
 # base classes
 from f110_gym.envs.base_classes import Simulator, Integrator
 
@@ -108,22 +109,8 @@ def __init__(self, render_mode=None, **kwargs):
             self.seed = 12345
         try:
             self.map_name = kwargs['map']
-            # different default maps
-            if self.map_name == 'berlin':
-                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/Berlin_map.yaml'
-            elif self.map_name == 'skirk':
-                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/Skirk_map.yaml'
-            elif self.map_name == 'levine':
-                self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/Levine_map.yaml'
-            else:
-                self.map_path = self.map_name + '.yaml'
         except:
-            self.map_path = os.path.dirname(os.path.abspath(__file__)) + '/maps/Vegas_map.yaml'
-
-        try:
-            self.map_ext = kwargs['map_ext']
-        except:
-            self.map_ext = '.png'
+            self.map_name = "Vegas"
 
         try:
             self.params = kwargs['params']
@@ -189,11 +176,13 @@ def __init__(self, render_mode=None, **kwargs):
         # initiate stuff
         self.sim = Simulator(self.params, self.num_agents, self.seed, time_step=self.timestep,
                              integrator=self.integrator)
-        self.sim.set_map(self.map_path, self.map_ext)
+        self.sim.set_map(self.map_name)
+        self.track = Track.from_track_name(self.map_name)  # load track in gym env for convenience
 
         # observation space
         # NOTE: keep original structure of observation space (dict). just define it as a dict space and define bounds
-        scan_size, scan_range = self.sim.agents[0].scan_simulator.num_beams, self.sim.agents[0].scan_simulator.max_range
+        scan_size = self.sim.agents[0].scan_simulator.num_beams
+        scan_range = self.sim.agents[0].scan_simulator.max_range + 0.1  # add 10cm because scan is not capped
         large_num = 1e30  # large number to avoid unbounded obs space (ie., low=-inf or high=inf)
         self.observation_space = gym.spaces.Dict({
             'ego_idx': gym.spaces.Discrete(self.num_agents),
@@ -395,18 +384,18 @@ def reset(self, seed=None, options=None):
 
         return obs, info
 
-    def update_map(self, map_path, map_ext):
+    def update_map(self, map_name: str):
         """
         Updates the map used by simulation
 
         Args:
-            map_path (str): absolute path to the map yaml file
-            map_ext (str): extension of the map image file
+            map_name (str): name of the map
 
         Returns:
             None
         """
-        self.sim.set_map(map_path, map_ext)
+        self.sim.set_map(map_name)
+        self.track = Track.from_track_name(map_name)
 
     def update_params(self, params, index=-1):
         """
@@ -469,7 +458,7 @@ def render_frame(self, mode):
             # first call, initialize everything
             from f110_gym.envs.rendering import EnvRenderer
             F110Env.renderer = EnvRenderer(WINDOW_W, WINDOW_H)
-            F110Env.renderer.update_map(self.map_name, self.map_ext)
+            F110Env.renderer.update_map(track=self.track)
 
         F110Env.renderer.update_obs(self.render_obs)
 
diff --git a/gym/f110_gym/envs/laser_models.py b/gym/f110_gym/envs/laser_models.py
index d121f279..616653c0 100644
--- a/gym/f110_gym/envs/laser_models.py
+++ b/gym/f110_gym/envs/laser_models.py
@@ -37,6 +37,9 @@
 import unittest
 import timeit
 
+from f110_gym.envs.track import Track
+
+
 def get_dt(bitmap, resolution):
     """
     Distance transformation, returns the distance matrix from the input bitmap.
@@ -380,42 +383,27 @@ def __init__(self, num_beams, fov, eps=0.0001, theta_dis=2000, max_range=30.0):
         self.sines = np.sin(theta_arr)
         self.cosines = np.cos(theta_arr)
     
-    def set_map(self, map_path, map_ext):
+    def set_map(self, map_name: str):
         """
         Set the bitmap of the scan simulator by path
 
             Args:
-                map_path (str): path to the map yaml file
-                map_ext (str): extension (image type) of the map image
+                map_name (str): name of the racetrack in the map dir, e.g. "Levine"
 
             Returns:
                 flag (bool): if image reading and loading is successful
         """
-        # TODO: do we open the option to flip the images, and turn rgb into grayscale? or specify the exact requirements in documentation.
-        # TODO: throw error if image specification isn't met
+        self.track = Track.from_track_name(map_name)
 
         # load map image
-        map_img_path = os.path.splitext(map_path)[0] + map_ext
-        self.map_img = np.array(Image.open(map_img_path).transpose(Image.FLIP_TOP_BOTTOM))
-        self.map_img = self.map_img.astype(np.float64)
-
-        # grayscale -> binary
-        self.map_img[self.map_img <= 128.] = 0.
-        self.map_img[self.map_img > 128.] = 255.
-
+        self.map_img = self.track.occupancy_map
         self.map_height = self.map_img.shape[0]
         self.map_width = self.map_img.shape[1]
 
-        # load map yaml
-        with open(map_path, 'r') as yaml_stream:
-            try:
-                map_metadata = yaml.safe_load(yaml_stream)
-                self.map_resolution = map_metadata['resolution']
-                self.origin = map_metadata['origin']
-            except yaml.YAMLError as ex:
-                print(ex)
+        # load map specification
+        self.map_resolution = self.track.spec.resolution
+        self.origin = self.track.spec.origin
 
-        # calculate map parameters
         self.orig_x = self.origin[0]
         self.orig_y = self.origin[1]
         self.orig_s = np.sin(self.origin[2])
diff --git a/gym/f110_gym/envs/rendering.py b/gym/f110_gym/envs/rendering.py
index 8d8ba1ae..374c3a98 100644
--- a/gym/f110_gym/envs/rendering.py
+++ b/gym/f110_gym/envs/rendering.py
@@ -36,6 +36,7 @@
 from PIL import Image
 import yaml
 
+from f110_gym.envs.track import Track
 # helpers
 from f110_gym.envs.collision_models import get_vertices
 
@@ -108,7 +109,7 @@ def __init__(self, width, height, *args, **kwargs):
 
         self.fps_display = pyglet.window.FPSDisplay(self)
 
-    def update_map(self, map_path, map_ext):
+    def update_map(self, track: Track):
         """
         Update the map being drawn by the renderer. Converts image to a list of 3D points representing each obstacle pixel in the map.
 
@@ -119,23 +120,16 @@ def update_map(self, map_path, map_ext):
         Returns:
             None
         """
-
-        # load map metadata
-        with open(map_path + '.yaml', 'r') as yaml_stream:
-            try:
-                map_metadata = yaml.safe_load(yaml_stream)
-                map_resolution = map_metadata['resolution']
-                origin = map_metadata['origin']
-                origin_x = origin[0]
-                origin_y = origin[1]
-            except yaml.YAMLError as ex:
-                print(ex)
-
-        # load map image
-        map_img = np.array(Image.open(map_path + map_ext).transpose(Image.FLIP_TOP_BOTTOM)).astype(np.float64)
+        # occupancy map
+        map_img = track.occupancy_map
         map_height = map_img.shape[0]
         map_width = map_img.shape[1]
 
+        # map spec
+        map_resolution = track.spec.resolution
+        origin = track.spec.origin
+        origin_x, origin_y, _ = origin
+
         # convert map pixels to coordinates
         range_x = np.arange(map_width)
         range_y = np.arange(map_height)
diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
index 96cd1c90..dfb782dd 100644
--- a/gym/f110_gym/envs/track.py
+++ b/gym/f110_gym/envs/track.py
@@ -8,6 +8,101 @@
 from PIL.Image import Transpose
 from yamldataclassconfig.config import YamlDataClassConfig
 
+from f110_gym.envs.cubic_spline import CubicSpline2D
+
+
+class Raceline:
+    n: int
+
+    ss: np.ndarray  # cumulative distance along the raceline
+    xs: np.ndarray  # x-coordinates of the raceline
+    ys: np.ndarray  # y-coordinates of the raceline
+    yaws: np.ndarray  # yaw angle of the raceline
+    ks: np.ndarray  # curvature of the raceline
+    vxs: np.ndarray  # velocity along the raceline
+    axs: np.ndarray  # acceleration along the raceline
+
+    length: float
+
+    def __init__(self,
+                 xs: np.ndarray,
+                 ys: np.ndarray,
+                 velxs: np.ndarray,
+                 ss: np.ndarray = None,
+                 psis: np.ndarray = None,
+                 kappas: np.ndarray = None,
+                 accxs: np.ndarray = None,
+                 spline: CubicSpline2D = None):
+        assert xs.shape == ys.shape == velxs.shape, "inconsistent shapes for x, y, vel"
+
+        self.n = xs.shape[0]
+        self.ss = ss
+        self.xs = xs
+        self.ys = ys
+        self.yaws = psis
+        self.ks = kappas
+        self.vxs = velxs
+        self.axs = accxs
+
+        # approximate track length by linear-interpolation of x,y waypoints
+        # note: we could use 'ss' but sometimes it is normalized to [0,1], so we recompute it here
+        self.length = np.sum(np.sqrt(np.diff(xs) ** 2 + np.diff(ys) ** 2))
+
+        # compute spline
+        self.spline = spline if spline is not None else CubicSpline2D(xs, ys)
+
+    @staticmethod
+    def from_centerline_file(filepath: pathlib.Path, delimiter: str = ',', fixed_speed: float = 1.0):
+        assert filepath.exists(), f"input filepath does not exist ({filepath})"
+        waypoints = np.loadtxt(filepath, delimiter=delimiter)
+        assert waypoints.shape[1] == 4, "expected waypoints as [x, y, w_left, w_right]"
+
+        # fit cubic spline to waypoints
+        xx, yy = waypoints[:, 0], waypoints[:, 1]
+        # close loop
+        xx = np.append(xx, xx[0])
+        yy = np.append(yy, yy[0])
+        spline = CubicSpline2D(xx, yy)
+        ds = 0.1
+
+        ss, xs, ys, yaws, ks = [], [], [], [], []
+
+        for i_s in np.arange(0, spline.s[-1], ds):
+            x, y = spline.calc_position(i_s)
+            yaw = spline.calc_yaw(i_s)
+            k = spline.calc_curvature(i_s)
+
+            xs.append(x)
+            ys.append(y)
+            yaws.append(yaw)
+            ks.append(k)
+            ss.append(i_s)
+
+        return Raceline(
+            ss=np.array(ss).astype(np.float32),
+            xs=np.array(xs).astype(np.float32),
+            ys=np.array(ys).astype(np.float32),
+            psis=np.array(yaws).astype(np.float32),
+            kappas=np.array(ks).astype(np.float32),
+            velxs=np.ones_like(ss).astype(np.float32),  # centerline does not have a speed profile, keep it constant at 1.0 m/s
+            accxs=np.zeros_like(ss).astype(np.float32),  # constant acceleration
+        )
+
+    @staticmethod
+    def from_raceline_file(filepath: pathlib.Path, delimiter: str = ';'):
+        assert filepath.exists(), f"input filepath does not exist ({filepath})"
+        waypoints = np.loadtxt(filepath, delimiter=delimiter).astype(np.float32)
+        assert waypoints.shape[1] == 7, "expected waypoints as [s, x, y, psi, k, vx, ax]"
+        return Raceline(
+            ss=waypoints[:, 0],
+            xs=waypoints[:, 1],
+            ys=waypoints[:, 2],
+            psis=waypoints[:, 3],
+            kappas=waypoints[:, 4],
+            velxs=waypoints[:, 5],
+            accxs=waypoints[:, 6],
+        )
+
 
 @dataclass
 class TrackSpec(YamlDataClassConfig):
@@ -42,6 +137,8 @@ class Track:
     filepath: str
     ext: str
     occupancy_map: np.ndarray
+    centerline: Raceline
+    raceline: Raceline
 
     def __init__(
             self,
@@ -49,11 +146,15 @@ def __init__(
             filepath: str,
             ext: str,
             occupancy_map: np.ndarray,
+            centerline: Raceline = None,
+            raceline: Raceline = None,
     ):
         self.spec = spec
         self.filepath = filepath
         self.ext = ext
         self.occupancy_map = occupancy_map
+        self.centerline = centerline
+        self.raceline = raceline
 
     @staticmethod
     def from_track_name(track: str):
@@ -71,13 +172,26 @@ def from_track_name(track: str):
             occupancy_map[occupancy_map <= 128] = 0.0
             occupancy_map[occupancy_map > 128] = 255.0
 
+            # if exists, load centerline
+            if (track_dir / f"{track}_centerline.csv").exists():
+                centerline = Raceline.from_centerline_file(track_dir / f"{track}_centerline.csv")
+            else:
+                centerline = None
+
+            # if exists, load raceline
+            if (track_dir / f"{track}_raceline.csv").exists():
+                raceline = Raceline.from_raceline_file(track_dir / f"{track}_raceline.csv")
+            else:
+                raceline = centerline
+
             return Track(
                 spec=track_spec,
                 filepath=str((track_dir / map_filename.stem).absolute()),
                 ext=map_filename.suffix,
                 occupancy_map=occupancy_map,
+                centerline=centerline,
+                raceline=raceline,
             )
-
         except Exception as ex:
             raise FileNotFoundError(f"could not load track {track}") from ex
 
diff --git a/examples/example_waypoints.csv b/gym/f110_gym/maps/Example/Example_raceline.csv
similarity index 100%
rename from examples/example_waypoints.csv
rename to gym/f110_gym/maps/Example/Example_raceline.csv
diff --git a/gym/f110_gym/unittest/gym_api_test.py b/gym/f110_gym/unittest/gym_api_test.py
index 6d3eedc8..d703001d 100644
--- a/gym/f110_gym/unittest/gym_api_test.py
+++ b/gym/f110_gym/unittest/gym_api_test.py
@@ -13,14 +13,6 @@ def test_gymnasium_api(self):
         from gymnasium.utils.env_checker import check_env
         import gymnasium as gym
 
-        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / 'examples'
-
-        with open(example_dir / 'config_example_map.yaml') as file:
-            conf_dict = yaml.load(file, Loader=yaml.FullLoader)
-        conf = Namespace(**conf_dict)
-        conf.map_path = str(example_dir / conf.map_path)
-
-        env = gym.make('f110_gym:f110-v0', map=conf.map_path, map_ext=conf.map_ext, num_agents=1, timestep=0.01,
-                       integrator=Integrator.Euler)
+        env = gym.make('f110_gym:f110-v0', num_agents=1)
 
         check_env(env.unwrapped)
\ No newline at end of file
diff --git a/gym/f110_gym/unittest/track_test.py b/gym/f110_gym/unittest/track_test.py
index 410712f1..76381547 100644
--- a/gym/f110_gym/unittest/track_test.py
+++ b/gym/f110_gym/unittest/track_test.py
@@ -11,4 +11,4 @@ def test_loading_default_tracks(self):
 
     def test_error_handling(self):
         wrong_track_name = "i_dont_exists"
-        self.assertRaises(FileNotFoundError, Track.from_track_name, wrong_track_name)
+        self.assertRaises(FileNotFoundError, Track.from_track_name, wrong_track_name)
\ No newline at end of file

From 0164d499f367b919f7dda0c77b53b5802efc1fc0 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Sat, 24 Jun 2023 12:01:27 +0200
Subject: [PATCH 13/42] fix shape/type observation to match space, add test to
 check consistency observation and space, add test to check observation_types
 don't break gym-API.

---
 gym/f110_gym/envs/observation.py          | 29 ++++++++++++++---------
 gym/f110_gym/unittest/observation_test.py | 24 +++++++++++++++++++
 2 files changed, 42 insertions(+), 11 deletions(-)

diff --git a/gym/f110_gym/envs/observation.py b/gym/f110_gym/envs/observation.py
index 09b49d0b..1883c6b6 100644
--- a/gym/f110_gym/envs/observation.py
+++ b/gym/f110_gym/envs/observation.py
@@ -114,17 +114,17 @@ def space(self):
         for agent_id in self.env.agent_ids:
             agent_dict = {
                 "scan": gym.spaces.Box(low=0.0, high=scan_range, shape=(scan_size,), dtype=np.float32),
-                "pose_x": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
-                "pose_y": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
-                "pose_theta": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
-                "linear_vel_x": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
-                "linear_vel_y": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
-                "ang_vel_z": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
-                "delta": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
-                "beta": gym.spaces.Box(low=-large_num, high=large_num, shape=(1,), dtype=np.float32),
-                "collision": gym.spaces.Box(low=0.0, high=1.0, shape=(1,), dtype=np.float32),
-                "lap_time": gym.spaces.Box(low=0.0, high=large_num, shape=(1,), dtype=np.float32),
-                "lap_count": gym.spaces.Box(low=0.0, high=large_num, shape=(1,), dtype=np.float32),
+                "pose_x": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
+                "pose_y": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
+                "pose_theta": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
+                "linear_vel_x": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
+                "linear_vel_y": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
+                "ang_vel_z": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
+                "delta": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
+                "beta": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
+                "collision": gym.spaces.Box(low=0.0, high=1.0, shape=(), dtype=np.float32),
+                "lap_time": gym.spaces.Box(low=0.0, high=large_num, shape=(), dtype=np.float32),
+                "lap_count": gym.spaces.Box(low=0.0, high=large_num, shape=(), dtype=np.float32),
             }
             complete_space[agent_id] = gym.spaces.Dict({k: agent_dict[k] for k in self.features})
 
@@ -168,6 +168,13 @@ def observe(self):
             # add agent's observation to multi-agent observation
             obs[agent_id] = {k: agent_obs[k] for k in self.features}
 
+            # cast to match observation space
+            for key in obs[agent_id].keys():
+                if isinstance(obs[agent_id][key], np.ndarray) or isinstance(obs[agent_id][key], list):
+                    obs[agent_id][key] = np.array(obs[agent_id][key], dtype=np.float32)
+                if isinstance(obs[agent_id][key], float):
+                    obs[agent_id][key] = np.float32(obs[agent_id][key])
+
         return obs
 
 
diff --git a/gym/f110_gym/unittest/observation_test.py b/gym/f110_gym/unittest/observation_test.py
index 4a27d29b..13d8c72e 100644
--- a/gym/f110_gym/unittest/observation_test.py
+++ b/gym/f110_gym/unittest/observation_test.py
@@ -148,3 +148,27 @@ def test_dynamic_obs_space(self):
             for ground_truth, observed in zip([pose_x, pose_y, pose_theta, velx, delta, beta],
                                               [obs_x, obs_y, obs_theta, obs_velx, obs_delta, obs_beta]):
                 self.assertAlmostEqual(ground_truth, observed)
+
+    def test_consistency_observe_space(self):
+        obs_type_ids = ["kinematic_state", "dynamic_state", "original"]
+
+        env = self._make_env()
+        env.reset()
+
+        for obs_type_id in obs_type_ids:
+            obs_type = observation_factory(env, type=obs_type_id)
+            space = obs_type.space()
+            observation = obs_type.observe()
+
+            self.assertTrue(space.contains(observation), f"Observation {obs_type_id} is not contained in its space")
+
+    def test_gymnasium_api(self):
+        import f110_gym
+        from gymnasium.utils.env_checker import check_env
+        import gymnasium as gym
+
+        obs_type_ids = ["kinematic_state", "dynamic_state", "original"]
+
+        for obs_type_id in obs_type_ids:
+            env = self._make_env(observation_config={"type": obs_type_id})
+            check_env(env.unwrapped, f"Observation {obs_type_id} breaks the gymnasium API")
\ No newline at end of file

From 5770801425f194bc6c91c203567da0a6e0b0a62f Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Sat, 24 Jun 2023 12:35:09 +0200
Subject: [PATCH 14/42] refactor example without loading conf file, delete
 unused config file, adapt gym_api_test.py to use map name, add test to check
 consistency on the loaded raceline, add test to check return None if missing
 raceline

---
 examples/config_example_map.yaml      | 43 ----------------------
 examples/waypoint_follow.py           | 52 ++++++---------------------
 gym/f110_gym/unittest/gym_api_test.py | 10 ++----
 gym/f110_gym/unittest/track_test.py   | 34 ++++++++++++++++--
 4 files changed, 44 insertions(+), 95 deletions(-)
 delete mode 100644 examples/config_example_map.yaml

diff --git a/examples/config_example_map.yaml b/examples/config_example_map.yaml
deleted file mode 100644
index 7fcc00aa..00000000
--- a/examples/config_example_map.yaml
+++ /dev/null
@@ -1,43 +0,0 @@
-# metadata
-run_name: 'map_wide'
-# characteristic number for map
-perf_num: 6
-
-# map paths
-map_name: 'Example'
-
-# starting pose for map
-sx: 0.7
-sy: 0.0
-stheta: 1.37079632679
-
-# raceline path and indices
-wpt_path: './Example_raceline.csv'
-wpt_delim: ';'
-wpt_rowskip: 3
-wpt_xind: 1
-wpt_yind: 2
-wpt_thind: 3
-wpt_vind: 5
-
-# varied params bound
-# physical params
-mass_min: 3.0
-mass_max: 4.0
-lf_min: 0.147
-lf_max: 0.170
-# controller params
-tlad_min: 0.2
-tlad_max: 5.
-vgain_min: 0.5
-vgain_max: 1.5
-
-# computation budget (can think of it as gen_num times pop_size)
-popsize: 100
-budget: 1000
-
-# optimization method
-optim_method: 'CMA'
-
-# seed
-seed: 12345
diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py
index ec65a727..e4a38a39 100644
--- a/examples/waypoint_follow.py
+++ b/examples/waypoint_follow.py
@@ -154,9 +154,8 @@ class PurePursuitPlanner:
     Example Planner
     """
 
-    def __init__(self, track, conf, wb):
+    def __init__(self, track, wb):
         self.wheelbase = wb
-        self.conf = conf    # todo: remove this
         self.waypoints = np.stack([track.raceline.xs, track.raceline.ys, track.raceline.vxs]).T
         self.max_reacquire = 20.
 
@@ -210,7 +209,7 @@ def _get_current_waypoint(self, waypoints, lookahead_distance, position, theta):
             return current_waypoint
         elif nearest_dist < self.max_reacquire:
             # NOTE: specify type or numba complains
-            return np.append(wpts[i, :], waypoints[i, self.conf.wpt_vind]).astype(np.float32)
+            return np.append(wpts[i, :], waypoints[i, -1]).astype(np.float32)
         else:
             return None
 
@@ -230,44 +229,14 @@ def plan(self, pose_x, pose_y, pose_theta, lookahead_distance, vgain):
         return speed, steering_angle
 
 
-class FlippyPlanner:
-    """
-    Planner designed to exploit integration methods and dynamics.
-    For testing only. To observe this error, use single track dynamics for all velocities >0.1
-    """
-
-    def __init__(self, speed=1, flip_every=1, steer=2):
-        self.speed = speed
-        self.flip_every = flip_every
-        self.counter = 0
-        self.steer = steer
-
-    def render_waypoints(self, *args, **kwargs):
-        pass
-
-    def plan(self, *args, **kwargs):
-        if self.counter % self.flip_every == 0:
-            self.counter = 0
-            self.steer *= -1
-        return self.speed, self.steer
-
-
 def main():
-    """
-    main entry point
-    """
+    work = {'mass': 3.463388126201571, 'lf': 0.15597534362552312,
+            'tlad': 0.82461887897713965, 'vgain': 1.375}
 
-    work = {'mass': 3.463388126201571, 'lf': 0.15597534362552312, 'tlad': 0.82461887897713965,
-            'vgain': 1.375}  # 0.90338203837889}
+    env = gym.make('f110_gym:f110-v0', map="Example", num_agents=1,
+                   timestep=0.01, integrator=Integrator.Euler, render_mode='human')
 
-    with open('config_example_map.yaml') as file:
-        conf_dict = yaml.load(file, Loader=yaml.FullLoader)
-    conf = Namespace(**conf_dict)
-
-    env = gym.make('f110_gym:f110-v0', map=conf.map_name, num_agents=1,
-                   timestep=0.01, integrator=Integrator.RK4, render_mode='human')
-
-    planner = PurePursuitPlanner(track=env.track, conf=conf, wb=0.17145 + 0.15875)  # FlippyPlanner(speed=0.2, flip_every=1, steer=10)
+    planner = PurePursuitPlanner(track=env.track, wb=0.17145 + 0.15875)  # FlippyPlanner(speed=0.2, flip_every=1, steer=10)
 
     def render_callback(env_renderer):
         # custom extra drawing function
@@ -287,10 +256,9 @@ def render_callback(env_renderer):
 
         planner.render_waypoints(env_renderer)
 
-
     env.add_render_callback(render_callback)
 
-    poses = np.array([[conf.sx, conf.sy, conf.stheta]])
+    poses = np.array([[0.7, 0.0, 1.37]])
     obs, info = env.reset(options={"poses": poses})
     done = False
     env.render()
@@ -299,8 +267,8 @@ def render_callback(env_renderer):
     start = time.time()
 
     while not done:
-        speed, steer = planner.plan(obs['poses_x'][0], obs['poses_y'][0], obs['poses_theta'][0], work['tlad'],
-                                    work['vgain'])
+        speed, steer = planner.plan(obs['poses_x'][0], obs['poses_y'][0], obs['poses_theta'][0],
+                                    work['tlad'], work['vgain'])
         obs, step_reward, done, truncated, info = env.step(np.array([[steer, speed]]))
         laptime += step_reward
         env.render()
diff --git a/gym/f110_gym/unittest/gym_api_test.py b/gym/f110_gym/unittest/gym_api_test.py
index d703001d..b365cafd 100644
--- a/gym/f110_gym/unittest/gym_api_test.py
+++ b/gym/f110_gym/unittest/gym_api_test.py
@@ -1,18 +1,12 @@
-import pathlib
 import unittest
-from argparse import Namespace
-
-import yaml
-
-from f110_gym.envs import Integrator
 
 
 class TestEnvInterface(unittest.TestCase):
     def test_gymnasium_api(self):
+        import gymnasium as gymnasium
         import f110_gym
         from gymnasium.utils.env_checker import check_env
-        import gymnasium as gym
 
-        env = gym.make('f110_gym:f110-v0', num_agents=1)
+        env = gymnasium.make('f110_gym:f110-v0', num_agents=1)
 
         check_env(env.unwrapped)
\ No newline at end of file
diff --git a/gym/f110_gym/unittest/track_test.py b/gym/f110_gym/unittest/track_test.py
index 76381547..5f395fb0 100644
--- a/gym/f110_gym/unittest/track_test.py
+++ b/gym/f110_gym/unittest/track_test.py
@@ -1,5 +1,8 @@
 import unittest
-from f110_gym.envs.track import Track
+
+import numpy as np
+
+from f110_gym.envs.track import Track, find_track_dir
 
 
 class TestTrack(unittest.TestCase):
@@ -11,4 +14,31 @@ def test_loading_default_tracks(self):
 
     def test_error_handling(self):
         wrong_track_name = "i_dont_exists"
-        self.assertRaises(FileNotFoundError, Track.from_track_name, wrong_track_name)
\ No newline at end of file
+        self.assertRaises(FileNotFoundError, Track.from_track_name, wrong_track_name)
+
+    def test_raceline(self):
+        track_name = "Example"  # Example is the only track with a raceline for now
+        track = Track.from_track_name(track_name)
+
+        # check raceline is not None
+        self.assertNotEqual(track.raceline, None)
+
+        # check loaded raceline match the one in the csv file
+        track_dir = find_track_dir(track_name)
+        assert track_dir is not None and track_dir.exists(), "track_dir does not exist"
+
+        raceline = np.loadtxt(track_dir / f"{track_name}_raceline.csv", delimiter=";")
+        s_idx, x_idx, y_idx, psi_idx, kappa_idx, vx_idx, ax_idx = range(7)
+
+        self.assertTrue(np.isclose(track.raceline.ss, raceline[:, s_idx]).all())
+        self.assertTrue(np.isclose(track.raceline.xs, raceline[:, x_idx]).all())
+        self.assertTrue(np.isclose(track.raceline.ys, raceline[:, y_idx]).all())
+        self.assertTrue(np.isclose(track.raceline.yaws, raceline[:, psi_idx]).all())
+        self.assertTrue(np.isclose(track.raceline.ks, raceline[:, kappa_idx]).all())
+        self.assertTrue(np.isclose(track.raceline.vxs, raceline[:, vx_idx]).all())
+        self.assertTrue(np.isclose(track.raceline.axs, raceline[:, ax_idx]).all())
+
+    def test_missing_raceline(self):
+        track = Track.from_track_name("Vegas")
+        self.assertEqual(track.raceline, None)
+        self.assertEqual(track.centerline, None)

From a0a7fefb1a2660a71a9d34f8346c247bd7cfe1d6 Mon Sep 17 00:00:00 2001
From: "Hongrui (Billy) Zheng" <hzheng40@users.noreply.github.com>
Date: Wed, 5 Jul 2023 11:25:34 -0400
Subject: [PATCH 15/42] Update gym/f110_gym/envs/base_classes.py

update string mapping method

Co-authored-by: Luigi Berducci <luigi.berd@gmail.com>
---
 gym/f110_gym/envs/base_classes.py | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index 6284a314..1fe91a24 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -41,6 +41,15 @@ class Integrator(Enum):
     RK4 = 1
     Euler = 2
 
+    @staticmethod
+    def from_string(integrator: str):
+        if integrator == "rk4":
+            return Integrator.RK4
+        elif integrator == "euler":
+            return Integrator.Euler
+        else:
+            raise ValueError(f"Unknown integrator type {integrator}")
+
 
 class RaceCar(object):
     """

From 5a4b2b1182ea7e79388294e7cf03e701beff5684 Mon Sep 17 00:00:00 2001
From: "Hongrui (Billy) Zheng" <hzheng40@users.noreply.github.com>
Date: Wed, 5 Jul 2023 11:25:49 -0400
Subject: [PATCH 16/42] Update gym/f110_gym/envs/f110_env.py

update string mapping method

Co-authored-by: Luigi Berducci <luigi.berd@gmail.com>
---
 gym/f110_gym/envs/f110_env.py | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index a0ff01ec..c2a1a725 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -47,12 +47,6 @@
 WINDOW_W = 1000
 WINDOW_H = 800
 
-# config map
-CONFIG_MAP = {
-    "rk4": Integrator.RK4,
-    "euler": Integrator.Euler,
-}
-
 
 class F110Env(gym.Env):
     """

From 9345034bf880af340c5f7cd5b7bec72e44f0753a Mon Sep 17 00:00:00 2001
From: "Hongrui (Billy) Zheng" <hzheng40@users.noreply.github.com>
Date: Wed, 5 Jul 2023 11:26:15 -0400
Subject: [PATCH 17/42] Update gym/f110_gym/envs/f110_env.py

update string mapping method

Co-authored-by: Luigi Berducci <luigi.berd@gmail.com>
---
 gym/f110_gym/envs/f110_env.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index c2a1a725..623f81a7 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -112,7 +112,7 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
         self.num_agents = self.config["num_agents"]
         self.timestep = self.config["timestep"]
         self.ego_idx = self.config["ego_idx"]
-        self.integrator = CONFIG_MAP[self.config["integrator"]]
+        self.integrator = Integrator.from_string(self.config["integrator"])
         self.control_input = self.config["control_input"]
 
         # radius to consider done

From b0b3216041d6f408df4eebe4954a02f7aea82d75 Mon Sep 17 00:00:00 2001
From: Hongrui Zheng <billyzheng.bz@gmail.com>
Date: Wed, 5 Jul 2023 13:56:55 -0400
Subject: [PATCH 18/42] Abstracted action types, fixed config dict update

---
 examples/waypoint_follow.py       |  2 +
 gym/f110_gym/envs/base_classes.py | 90 +++++++++++++++++++++++--------
 gym/f110_gym/envs/f110_env.py     | 34 ++++++++++--
 3 files changed, 99 insertions(+), 27 deletions(-)

diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py
index 9a70c998..dc814e2d 100644
--- a/examples/waypoint_follow.py
+++ b/examples/waypoint_follow.py
@@ -356,6 +356,8 @@ def render_callback(env_renderer):
             "num_agents": 1,
             "timestep": 0.01,
             "integrator": "rk4",
+            "control_input": "speed",
+            "params": {"mu": 1.0},
         },
         render_mode="human",
     )
diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index 1fe91a24..db5371c7 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -51,6 +51,64 @@ def from_string(integrator: str):
             raise ValueError(f"Unknown integrator type {integrator}")
 
 
+class CarActionEnum(Enum):
+    Accl = 1
+    Speed = 2
+
+    @staticmethod
+    def from_string(action: str):
+        if action == "accl":
+            return AcclAction()
+        elif action == "speed":
+            return SpeedAction()
+        else:
+            raise ValueError(f"Unknown action type {action}")
+
+
+class CarAction(object):
+    def __init__(self) -> None:
+        self._action_type = None
+
+    def act(self, action: tuple[float, float]) -> tuple[float, float]:
+        return 0.0, 0.0
+
+    @property
+    def type(self) -> str:
+        return self._action_type
+
+
+class AcclAction(CarAction):
+    def __init__(self) -> None:
+        super().__init__()
+        self._action_type = "accl"
+
+    def act(
+        self, action: tuple[float, float], state=None, params=None
+    ) -> tuple[float, float]:
+        return action
+
+
+class SpeedAction(CarAction):
+    def __init__(self) -> None:
+        super().__init__()
+        self._action_type = "speed"
+
+    def act(
+        self, action: tuple[float, float], state: np.ndarray, params: dict
+    ) -> tuple[float, float]:
+        accl, sv = pid(
+            action[0],
+            action[1],
+            state[3],
+            state[2],
+            params["sv_max"],
+            params["a_max"],
+            params["v_max"],
+            params["v_min"],
+        )
+        return accl, sv
+
+
 class RaceCar(object):
     """
     Base level race car class, handles the physics and laser scan of a single vehicle
@@ -84,7 +142,7 @@ def __init__(
         num_beams=1080,
         fov=4.7,
         integrator=Integrator.Euler,
-        control_input="speed",
+        action_type=SpeedAction(),
     ):
         """
         Init function
@@ -112,7 +170,7 @@ def __init__(
             warnings.warn(
                 f"Chosen integrator is RK4. This is different from previous versions of the gym."
             )
-        self.control_input = control_input
+        self.action_type = action_type
 
         # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
         self.state = np.zeros((7,))
@@ -311,24 +369,10 @@ def update_pose(self, raw_steer, vel):
             self.steer_buffer = self.steer_buffer[:-1]
             self.steer_buffer = np.append(raw_steer, self.steer_buffer)
 
-        if self.control_input == "speed":
-            # steering angle velocity input to steering velocity acceleration input
-            accl, sv = pid(
-                vel,
-                steer,
-                self.state[3],
-                self.state[2],
-                self.params["sv_max"],
-                self.params["a_max"],
-                self.params["v_max"],
-                self.params["v_min"],
-            )
-        elif self.control_input == "acceleration":
-            accl, sv = vel, steer
-        else:
-            raise SyntaxError(
-                f"Invalid Control Input Type Specified. Provided {self.control_input}. Please choose speed or acceleration"
-            )
+        if self.action_type.type is None:
+            raise ValueError("No Control Action Type Specified.")
+
+        accl, sv = self.action_type.act((vel, steer), self.state, self.params)
 
         if self.integrator is Integrator.RK4:
             # RK4 integration
@@ -526,7 +570,7 @@ def __init__(
         time_step=0.01,
         ego_idx=0,
         integrator=Integrator.RK4,
-        control_input="speed",
+        action_type=SpeedAction(),
     ):
         """
         Init function
@@ -560,7 +604,7 @@ def __init__(
                     is_ego=True,
                     time_step=self.time_step,
                     integrator=integrator,
-                    control_input=control_input,
+                    action_type=action_type,
                 )
                 self.agents.append(ego_car)
             else:
@@ -570,7 +614,7 @@ def __init__(
                     is_ego=False,
                     time_step=self.time_step,
                     integrator=integrator,
-                    control_input=control_input,
+                    action_type=action_type,
                 )
                 self.agents.append(agent)
 
diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 623f81a7..6b576c0e 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -28,7 +28,12 @@
 import gymnasium as gym
 
 # base classes
-from f110_gym.envs.base_classes import Simulator, Integrator
+from f110_gym.envs.base_classes import Simulator, Integrator, CarActionEnum
+
+# types
+from typing import Dict, Any, TypeVar
+
+KeyType = TypeVar("KeyType")
 
 # others
 import numpy as np
@@ -113,7 +118,7 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
         self.timestep = self.config["timestep"]
         self.ego_idx = self.config["ego_idx"]
         self.integrator = Integrator.from_string(self.config["integrator"])
-        self.control_input = self.config["control_input"]
+        self.action_type = CarActionEnum.from_string(self.config["control_input"])
 
         # radius to consider done
         self.start_thresh = 0.5  # 10cm
@@ -152,7 +157,7 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
             self.seed,
             time_step=self.timestep,
             integrator=self.integrator,
-            control_input=self.control_input,
+            action_type=self.action_type,
         )
         self.sim.set_map(self.map_path, self.map_ext)
 
@@ -248,6 +253,27 @@ def __del__(self):
         """
         pass
 
+    @classmethod
+    def deep_update(
+        cls, mapping: Dict[KeyType, Any], *updating_mappings: Dict[KeyType, Any]
+    ) -> Dict[KeyType, Any]:
+        """
+        Dictionary deep update for nested dictionaries from pydantic:
+        https://github.com/pydantic/pydantic/blob/fd2991fe6a73819b48c906e3c3274e8e47d0f761/pydantic/utils.py#L200
+        """
+        updated_mapping = mapping.copy()
+        for updating_mapping in updating_mappings:
+            for k, v in updating_mapping.items():
+                if (
+                    k in updated_mapping
+                    and isinstance(updated_mapping[k], dict)
+                    and isinstance(v, dict)
+                ):
+                    updated_mapping[k] = F110Env.deep_update(updated_mapping[k], v)
+                else:
+                    updated_mapping[k] = v
+        return updated_mapping
+
     @classmethod
     def default_config(cls) -> dict:
         """
@@ -294,7 +320,7 @@ def default_config(cls) -> dict:
 
     def configure(self, config: dict) -> None:
         if config:
-            self.config.update(config)
+            self.config = F110Env.deep_update(self.config, config)
 
     def _check_done(self):
         """

From ac5ac5886d1be7cafce381e29b4152a71ba041b6 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Thu, 6 Jul 2023 11:42:33 +0200
Subject: [PATCH 19/42] correct import scan simulator update gitignore with zip
 files

---
 .gitignore                        | 3 ++-
 gym/f110_gym/unittest/__init__.py | 2 +-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/.gitignore b/.gitignore
index 3641cd67..2d07a8ea 100644
--- a/.gitignore
+++ b/.gitignore
@@ -169,4 +169,5 @@ cython_debug/
 #  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
 #  and can be added to the global gitignore or merged into this file.  For a more nuclear
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
-#.idea/
\ No newline at end of file
+#.idea/
+*.zip
diff --git a/gym/f110_gym/unittest/__init__.py b/gym/f110_gym/unittest/__init__.py
index 53d19b7c..5be97ba3 100644
--- a/gym/f110_gym/unittest/__init__.py
+++ b/gym/f110_gym/unittest/__init__.py
@@ -1 +1 @@
-from gym.envs.unittest.scan_sim import *
\ No newline at end of file
+from .scan_sim import *
\ No newline at end of file

From 6fe92b86b06c90bd0a27ac9d9f4718a291bc1a8d Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Fri, 7 Jul 2023 12:10:50 +0200
Subject: [PATCH 20/42] update test to match new env signature

---
 gym/f110_gym/unittest/gym_api_test.py | 22 ++++++++++++++++------
 1 file changed, 16 insertions(+), 6 deletions(-)

diff --git a/gym/f110_gym/unittest/gym_api_test.py b/gym/f110_gym/unittest/gym_api_test.py
index 6d3eedc8..60cb045c 100644
--- a/gym/f110_gym/unittest/gym_api_test.py
+++ b/gym/f110_gym/unittest/gym_api_test.py
@@ -13,14 +13,24 @@ def test_gymnasium_api(self):
         from gymnasium.utils.env_checker import check_env
         import gymnasium as gym
 
-        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / 'examples'
+        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / "examples"
 
-        with open(example_dir / 'config_example_map.yaml') as file:
+        with open(example_dir / "config_example_map.yaml") as file:
             conf_dict = yaml.load(file, Loader=yaml.FullLoader)
         conf = Namespace(**conf_dict)
         conf.map_path = str(example_dir / conf.map_path)
 
-        env = gym.make('f110_gym:f110-v0', map=conf.map_path, map_ext=conf.map_ext, num_agents=1, timestep=0.01,
-                       integrator=Integrator.Euler)
-
-        check_env(env.unwrapped)
\ No newline at end of file
+        env = gym.make(
+            "f110_gym:f110-v0",
+            config={
+                "map": conf.map_path,
+                "map_ext": conf.map_ext,
+                "num_agents": 1,
+                "timestep": 0.01,
+                "integrator": "rk4",
+                "control_input": "speed",
+                "params": {"mu": 1.0},
+            },
+        )
+
+        check_env(env.unwrapped)

From 62b59694f68e63c4dec725a4f1175cae4a16ec15 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Fri, 7 Jul 2023 12:12:51 +0200
Subject: [PATCH 21/42] create action.py to separate action implementation from
 base_classes.py, move deep_update in utils.py, update f110_env.py accordingly

---
 gym/f110_gym/envs/action.py       | 62 ++++++++++++++++++++++++++++
 gym/f110_gym/envs/base_classes.py | 67 +++----------------------------
 gym/f110_gym/envs/f110_env.py     | 32 +++------------
 gym/f110_gym/envs/utils.py        | 27 +++++++++++++
 4 files changed, 100 insertions(+), 88 deletions(-)
 create mode 100644 gym/f110_gym/envs/action.py
 create mode 100644 gym/f110_gym/envs/utils.py

diff --git a/gym/f110_gym/envs/action.py b/gym/f110_gym/envs/action.py
new file mode 100644
index 00000000..f300b6ca
--- /dev/null
+++ b/gym/f110_gym/envs/action.py
@@ -0,0 +1,62 @@
+from abc import abstractmethod
+from enum import Enum
+from typing import Any, Tuple, Dict
+
+import numpy as np
+from f110_gym.envs.dynamic_models import pid
+
+
+class CarActionEnum(Enum):
+    Accl = 1
+    Speed = 2
+
+    @staticmethod
+    def from_string(action: str):
+        if action == "accl":
+            return AcclAction()
+        elif action == "speed":
+            return SpeedAction()
+        else:
+            raise ValueError(f"Unknown action type {action}")
+
+class CarAction:
+    def __init__(self) -> None:
+        self._action_type = None
+
+    @abstractmethod
+    def act(self, action: Any, **kwargs) -> Tuple[float, float]:
+        raise NotImplementedError("act method not implemented")
+
+    @property
+    def type(self) -> str:
+        return self._action_type
+
+
+class AcclAction(CarAction):
+    def __init__(self) -> None:
+        super().__init__()
+        self._action_type = "accl"
+
+    def act(
+        self, action: Tuple[float, float], state, params
+    ) -> Tuple[float, float]:
+        return action
+
+
+class SpeedAction(CarAction):
+    def __init__(self) -> None:
+        super().__init__()
+        self._action_type = "speed"
+
+    def act(self, action: Tuple[float, float], state: np.ndarray, params: Dict) -> Tuple[float, float]:
+        accl, sv = pid(
+            action[0],
+            action[1],
+            state[3],
+            state[2],
+            params["sv_max"],
+            params["a_max"],
+            params["v_max"],
+            params["v_min"],
+        )
+        return accl, sv
diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index db5371c7..885fd9ec 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -30,9 +30,9 @@
 import warnings
 
 import numpy as np
-from numba import njit
 
-from f110_gym.envs.dynamic_models import vehicle_dynamics_st, pid
+from f110_gym.envs.action import SpeedAction
+from f110_gym.envs.dynamic_models import vehicle_dynamics_st
 from f110_gym.envs.laser_models import ScanSimulator2D, check_ttc_jit, ray_cast
 from f110_gym.envs.collision_models import get_vertices, collision_multiple
 
@@ -51,64 +51,6 @@ def from_string(integrator: str):
             raise ValueError(f"Unknown integrator type {integrator}")
 
 
-class CarActionEnum(Enum):
-    Accl = 1
-    Speed = 2
-
-    @staticmethod
-    def from_string(action: str):
-        if action == "accl":
-            return AcclAction()
-        elif action == "speed":
-            return SpeedAction()
-        else:
-            raise ValueError(f"Unknown action type {action}")
-
-
-class CarAction(object):
-    def __init__(self) -> None:
-        self._action_type = None
-
-    def act(self, action: tuple[float, float]) -> tuple[float, float]:
-        return 0.0, 0.0
-
-    @property
-    def type(self) -> str:
-        return self._action_type
-
-
-class AcclAction(CarAction):
-    def __init__(self) -> None:
-        super().__init__()
-        self._action_type = "accl"
-
-    def act(
-        self, action: tuple[float, float], state=None, params=None
-    ) -> tuple[float, float]:
-        return action
-
-
-class SpeedAction(CarAction):
-    def __init__(self) -> None:
-        super().__init__()
-        self._action_type = "speed"
-
-    def act(
-        self, action: tuple[float, float], state: np.ndarray, params: dict
-    ) -> tuple[float, float]:
-        accl, sv = pid(
-            action[0],
-            action[1],
-            state[3],
-            state[2],
-            params["sv_max"],
-            params["a_max"],
-            params["v_max"],
-            params["v_min"],
-        )
-        return accl, sv
-
-
 class RaceCar(object):
     """
     Base level race car class, handles the physics and laser scan of a single vehicle
@@ -372,7 +314,7 @@ def update_pose(self, raw_steer, vel):
         if self.action_type.type is None:
             raise ValueError("No Control Action Type Specified.")
 
-        accl, sv = self.action_type.act((vel, steer), self.state, self.params)
+        accl, sv = self.action_type.act(action=(vel, steer), state=self.state, params=self.params)
 
         if self.integrator is Integrator.RK4:
             # RK4 integration
@@ -550,7 +492,7 @@ def update_scan(self, agent_scans, agent_index):
 
 class Simulator(object):
     """
-    Simulator class, handles the interaction and update of all vehicles in the environment
+    Simulator class, handles the interaction and update o0000000f all vehicles in the environment
 
     Data Members:
         num_agents (int): number of agents in the environment
@@ -643,6 +585,7 @@ def update_params(self, params, agent_idx=-1):
         Returns:
             None
         """
+        self.params = params
         if agent_idx < 0:
             # update params for all
             for agent in self.agents:
diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 6b576c0e..5a7c0f49 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -27,13 +27,14 @@
 # gym imports
 import gymnasium as gym
 
+from f110_gym.envs.action import CarActionEnum
+
 # base classes
-from f110_gym.envs.base_classes import Simulator, Integrator, CarActionEnum
+from f110_gym.envs.base_classes import Simulator, Integrator
+
 
-# types
-from typing import Dict, Any, TypeVar
+from f110_gym.envs.utils import deep_update
 
-KeyType = TypeVar("KeyType")
 
 # others
 import numpy as np
@@ -253,27 +254,6 @@ def __del__(self):
         """
         pass
 
-    @classmethod
-    def deep_update(
-        cls, mapping: Dict[KeyType, Any], *updating_mappings: Dict[KeyType, Any]
-    ) -> Dict[KeyType, Any]:
-        """
-        Dictionary deep update for nested dictionaries from pydantic:
-        https://github.com/pydantic/pydantic/blob/fd2991fe6a73819b48c906e3c3274e8e47d0f761/pydantic/utils.py#L200
-        """
-        updated_mapping = mapping.copy()
-        for updating_mapping in updating_mappings:
-            for k, v in updating_mapping.items():
-                if (
-                    k in updated_mapping
-                    and isinstance(updated_mapping[k], dict)
-                    and isinstance(v, dict)
-                ):
-                    updated_mapping[k] = F110Env.deep_update(updated_mapping[k], v)
-                else:
-                    updated_mapping[k] = v
-        return updated_mapping
-
     @classmethod
     def default_config(cls) -> dict:
         """
@@ -320,7 +300,7 @@ def default_config(cls) -> dict:
 
     def configure(self, config: dict) -> None:
         if config:
-            self.config = F110Env.deep_update(self.config, config)
+            self.config = deep_update(self.config, config)
 
     def _check_done(self):
         """
diff --git a/gym/f110_gym/envs/utils.py b/gym/f110_gym/envs/utils.py
new file mode 100644
index 00000000..5b84e30d
--- /dev/null
+++ b/gym/f110_gym/envs/utils.py
@@ -0,0 +1,27 @@
+from typing import Dict, Any
+
+# types
+from typing import TypeVar
+
+KeyType = TypeVar("KeyType")
+
+
+def deep_update(
+    mapping: Dict[KeyType, Any], *updating_mappings: Dict[KeyType, Any]
+) -> Dict[KeyType, Any]:
+    """
+    Dictionary deep update for nested dictionaries from pydantic:
+    https://github.com/pydantic/pydantic/blob/fd2991fe6a73819b48c906e3c3274e8e47d0f761/pydantic/utils.py#L200
+    """
+    updated_mapping = mapping.copy()
+    for updating_mapping in updating_mappings:
+        for k, v in updating_mapping.items():
+            if (
+                k in updated_mapping
+                and isinstance(updated_mapping[k], dict)
+                and isinstance(v, dict)
+            ):
+                updated_mapping[k] = deep_update(updated_mapping[k], v)
+            else:
+                updated_mapping[k] = v
+    return updated_mapping

From 18afffe4bb7ed13d204a0a8e9550c4469a120556 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Fri, 7 Jul 2023 12:30:28 +0200
Subject: [PATCH 22/42] update parameters in simulator, add test to check
 params update is propagated to simulator and racecars

---
 gym/f110_gym/envs/f110_env.py       |   2 +
 gym/f110_gym/unittest/utils_test.py | 107 ++++++++++++++++++++++++++++
 2 files changed, 109 insertions(+)
 create mode 100644 gym/f110_gym/unittest/utils_test.py

diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 5a7c0f49..1cf378e1 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -301,6 +301,8 @@ def default_config(cls) -> dict:
     def configure(self, config: dict) -> None:
         if config:
             self.config = deep_update(self.config, config)
+            if hasattr(self, "sim"):
+                self.sim.update_params(self.config["params"])
 
     def _check_done(self):
         """
diff --git a/gym/f110_gym/unittest/utils_test.py b/gym/f110_gym/unittest/utils_test.py
new file mode 100644
index 00000000..ed04d520
--- /dev/null
+++ b/gym/f110_gym/unittest/utils_test.py
@@ -0,0 +1,107 @@
+import pathlib
+import unittest
+from argparse import Namespace
+
+import numpy as np
+import yaml
+
+
+class TestUtilities(unittest.TestCase):
+    def test_deep_update(self):
+        import f110_gym
+        import gymnasium as gym
+
+        default_env = gym.make("f110_gym:f110-v0")
+        custom_env = gym.make("f110_gym:f110-v0", config={"params": {"mu": 1.0}})
+
+        # check all parameters are the same except for mu
+        for par in default_env.sim.params:
+            default_val = default_env.sim.params[par]
+            custom_val = custom_env.sim.params[par]
+
+            if par == "mu":
+                self.assertNotEqual(default_val, custom_val, "mu should be different")
+            else:
+                self.assertEqual(default_val, custom_val, f"{par} should be the same")
+
+        default_env.close()
+        custom_env.close()
+
+    def test_configure_method(self):
+        import f110_gym
+        import gymnasium as gym
+
+        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / "examples"
+
+        with open(example_dir / "config_example_map.yaml") as file:
+            conf_dict = yaml.load(file, Loader=yaml.FullLoader)
+        conf = Namespace(**conf_dict)
+        conf.map_path = str(example_dir / conf.map_path)
+
+        config = {
+            "map": conf.map_path,
+            "map_ext": conf.map_ext,
+            "num_agents": 1,
+            "timestep": 0.01,
+            "integrator": "rk4",
+            "control_input": "speed",
+        }
+
+        extended_config = config.copy()
+        extended_config["params"] = {"width": 15.0}
+
+        base_env = gym.make(
+            "f110_gym:f110-v0",
+            config=config,
+            render_mode="human",
+        )
+        base_env.configure(config={"params": {"width": 15.0}})
+
+        extended_env = gym.make(
+            "f110_gym:f110-v0",
+            config=extended_config,
+            render_mode="human",
+        )
+
+        # check all parameters in config dictionary are the same
+        for par in base_env.sim.params:
+            base_val = base_env.config["params"][par]
+            extended_val = extended_env.config["params"][par]
+
+            self.assertEqual(base_val, extended_val, f"{par} should be the same")
+
+        # check all params in simulator are the same
+        for par in base_env.sim.params:
+            base_val = base_env.sim.params[par]
+            extended_val = extended_env.sim.params[par]
+
+            self.assertEqual(base_val, extended_val, f"{par} should be the same")
+
+        # check all params in agents are the same
+        for agent, ext_agent in zip(base_env.sim.agents, extended_env.sim.agents):
+            for par in agent.params:
+                base_val = agent.params[par]
+                extended_val = ext_agent.params[par]
+
+                self.assertEqual(base_val, extended_val, f"{par} should be the same")
+
+        # run a simulation and check that the observations are the same
+        obs0, _ = base_env.reset()
+        obs1, _ = extended_env.reset()
+        done0 = done1 = False
+        t = 0
+
+        while not done0 and not done1:
+            action = base_env.action_space.sample()
+            obs0, _, done0, _, _ = base_env.step(action)
+            obs1, _, done1, _, _ = extended_env.step(action)
+            base_env.render()
+            for k in obs0:
+                self.assertTrue(np.allclose(obs0[k], obs1[k]), f"Observations {k} should be the same")
+            self.assertTrue(done0 == done1, "Done should be the same")
+            t += 1
+
+        print(f"Done after {t} steps")
+
+        base_env.close()
+        extended_env.close()

From 5590485e53f1d9d7ceb63f960595523760d9a68e Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Sat, 8 Jul 2023 22:24:08 +0200
Subject: [PATCH 23/42] refactoring test: rename gym_api_test.py to
 f110_env_test.py, move test configure() in f110_env_test.py, refactoring
 utility test

---
 gym/f110_gym/unittest/f110_env_test.py | 105 +++++++++++++++++++++++++
 gym/f110_gym/unittest/gym_api_test.py  |  36 ---------
 gym/f110_gym/unittest/utils_test.py    |  88 +--------------------
 3 files changed, 109 insertions(+), 120 deletions(-)
 create mode 100644 gym/f110_gym/unittest/f110_env_test.py
 delete mode 100644 gym/f110_gym/unittest/gym_api_test.py

diff --git a/gym/f110_gym/unittest/f110_env_test.py b/gym/f110_gym/unittest/f110_env_test.py
new file mode 100644
index 00000000..940a7ab8
--- /dev/null
+++ b/gym/f110_gym/unittest/f110_env_test.py
@@ -0,0 +1,105 @@
+import pathlib
+import unittest
+from argparse import Namespace
+
+import numpy as np
+import yaml
+import gymnasium as gym
+
+from f110_gym.envs.utils import deep_update
+
+
+class TestEnvInterface(unittest.TestCase):
+    def _make_env(self, config={}):
+        import f110_gym
+
+        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / "examples"
+
+        with open(example_dir / "config_example_map.yaml") as file:
+            conf_dict = yaml.load(file, Loader=yaml.FullLoader)
+        conf = Namespace(**conf_dict)
+        conf.map_path = str(example_dir / conf.map_path)
+
+        conf = {
+            "map": conf.map_path,
+            "map_ext": conf.map_ext,
+            "num_agents": 1,
+            "timestep": 0.01,
+            "integrator": "rk4",
+            "control_input": "speed",
+            "params": {"mu": 1.0},
+        }
+        conf = deep_update(conf, config)
+
+        env = gym.make(
+            "f110_gym:f110-v0",
+            config=conf,
+        )
+        return env
+
+    def test_gymnasium_api(self):
+        from gymnasium.utils.env_checker import check_env
+
+        env = self._make_env()
+        check_env(env.unwrapped)
+
+    def test_configure_method(self):
+        """
+        Test that the configure method works as expected, and that the parameters are
+        correctly updated in the simulator and agents.
+        """
+        import f110_gym
+
+        # create a base environment and use configure() to change the width
+        config_ext = {"params": {"width": 15.0}}
+        base_env = self._make_env()
+        base_env.configure(config=config_ext)
+
+        # create an extended environment, with the width set on initialization
+        extended_env = self._make_env(config=config_ext)
+
+        # check consistency parameters in config
+        for par in base_env.config["params"]:
+            base_val = base_env.config["params"][par]
+            extended_val = extended_env.config["params"][par]
+
+            self.assertEqual(base_val, extended_val, f"{par} should be the same")
+
+        # check consistency in simulator parameters
+        for par in base_env.sim.params:
+            base_val = base_env.sim.params[par]
+            extended_val = extended_env.sim.params[par]
+
+            self.assertEqual(base_val, extended_val, f"{par} should be the same")
+
+        # check consistency in agent parameters
+        for agent, ext_agent in zip(base_env.sim.agents, extended_env.sim.agents):
+            for par in agent.params:
+                base_val = agent.params[par]
+                extended_val = ext_agent.params[par]
+
+                self.assertEqual(base_val, extended_val, f"{par} should be the same")
+
+        # finally, run a simulation and check that the results are the same
+        obs0, _ = base_env.reset(options={"poses": np.array([[0.0, 0.0, np.pi/2]])})
+        obs1, _ = extended_env.reset(options={"poses": np.array([[0.0, 0.0, np.pi/2]])})
+        done0 = done1 = False
+        t = 0
+
+        while not done0 and not done1:
+            action = base_env.action_space.sample()
+            obs0, _, done0, _, _ = base_env.step(action)
+            obs1, _, done1, _, _ = extended_env.step(action)
+            base_env.render()
+            for k in obs0:
+                self.assertTrue(
+                    np.allclose(obs0[k], obs1[k]),
+                    f"Observations {k} should be the same",
+                )
+            self.assertTrue(done0 == done1, "Done should be the same")
+            t += 1
+
+        print(f"Done after {t} steps")
+
+        base_env.close()
+        extended_env.close()
diff --git a/gym/f110_gym/unittest/gym_api_test.py b/gym/f110_gym/unittest/gym_api_test.py
deleted file mode 100644
index 60cb045c..00000000
--- a/gym/f110_gym/unittest/gym_api_test.py
+++ /dev/null
@@ -1,36 +0,0 @@
-import pathlib
-import unittest
-from argparse import Namespace
-
-import yaml
-
-from f110_gym.envs import Integrator
-
-
-class TestEnvInterface(unittest.TestCase):
-    def test_gymnasium_api(self):
-        import f110_gym
-        from gymnasium.utils.env_checker import check_env
-        import gymnasium as gym
-
-        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / "examples"
-
-        with open(example_dir / "config_example_map.yaml") as file:
-            conf_dict = yaml.load(file, Loader=yaml.FullLoader)
-        conf = Namespace(**conf_dict)
-        conf.map_path = str(example_dir / conf.map_path)
-
-        env = gym.make(
-            "f110_gym:f110-v0",
-            config={
-                "map": conf.map_path,
-                "map_ext": conf.map_ext,
-                "num_agents": 1,
-                "timestep": 0.01,
-                "integrator": "rk4",
-                "control_input": "speed",
-                "params": {"mu": 1.0},
-            },
-        )
-
-        check_env(env.unwrapped)
diff --git a/gym/f110_gym/unittest/utils_test.py b/gym/f110_gym/unittest/utils_test.py
index ed04d520..2ed2f586 100644
--- a/gym/f110_gym/unittest/utils_test.py
+++ b/gym/f110_gym/unittest/utils_test.py
@@ -1,13 +1,12 @@
-import pathlib
 import unittest
-from argparse import Namespace
-
-import numpy as np
-import yaml
 
 
 class TestUtilities(unittest.TestCase):
     def test_deep_update(self):
+        """
+        Test that the deep_update function works as expected with nested dictionaries,
+        by comparing two environments with different mu values.
+        """
         import f110_gym
         import gymnasium as gym
 
@@ -26,82 +25,3 @@ def test_deep_update(self):
 
         default_env.close()
         custom_env.close()
-
-    def test_configure_method(self):
-        import f110_gym
-        import gymnasium as gym
-
-        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / "examples"
-
-        with open(example_dir / "config_example_map.yaml") as file:
-            conf_dict = yaml.load(file, Loader=yaml.FullLoader)
-        conf = Namespace(**conf_dict)
-        conf.map_path = str(example_dir / conf.map_path)
-
-        config = {
-            "map": conf.map_path,
-            "map_ext": conf.map_ext,
-            "num_agents": 1,
-            "timestep": 0.01,
-            "integrator": "rk4",
-            "control_input": "speed",
-        }
-
-        extended_config = config.copy()
-        extended_config["params"] = {"width": 15.0}
-
-        base_env = gym.make(
-            "f110_gym:f110-v0",
-            config=config,
-            render_mode="human",
-        )
-        base_env.configure(config={"params": {"width": 15.0}})
-
-        extended_env = gym.make(
-            "f110_gym:f110-v0",
-            config=extended_config,
-            render_mode="human",
-        )
-
-        # check all parameters in config dictionary are the same
-        for par in base_env.sim.params:
-            base_val = base_env.config["params"][par]
-            extended_val = extended_env.config["params"][par]
-
-            self.assertEqual(base_val, extended_val, f"{par} should be the same")
-
-        # check all params in simulator are the same
-        for par in base_env.sim.params:
-            base_val = base_env.sim.params[par]
-            extended_val = extended_env.sim.params[par]
-
-            self.assertEqual(base_val, extended_val, f"{par} should be the same")
-
-        # check all params in agents are the same
-        for agent, ext_agent in zip(base_env.sim.agents, extended_env.sim.agents):
-            for par in agent.params:
-                base_val = agent.params[par]
-                extended_val = ext_agent.params[par]
-
-                self.assertEqual(base_val, extended_val, f"{par} should be the same")
-
-        # run a simulation and check that the observations are the same
-        obs0, _ = base_env.reset()
-        obs1, _ = extended_env.reset()
-        done0 = done1 = False
-        t = 0
-
-        while not done0 and not done1:
-            action = base_env.action_space.sample()
-            obs0, _, done0, _, _ = base_env.step(action)
-            obs1, _, done1, _, _ = extended_env.step(action)
-            base_env.render()
-            for k in obs0:
-                self.assertTrue(np.allclose(obs0[k], obs1[k]), f"Observations {k} should be the same")
-            self.assertTrue(done0 == done1, "Done should be the same")
-            t += 1
-
-        print(f"Done after {t} steps")
-
-        base_env.close()
-        extended_env.close()

From aa8ea19dde6a6567b3208ced310e5aef3543cfba Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Sat, 8 Jul 2023 22:53:52 +0200
Subject: [PATCH 24/42] extend config dictionary with observation_config,
 update test accordingly

---
 gym/f110_gym/envs/f110_env.py             | 11 +++------
 gym/f110_gym/unittest/observation_test.py | 28 ++++++++++++++++-------
 2 files changed, 23 insertions(+), 16 deletions(-)

diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index bb22bf0d..b70bf3d5 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -31,7 +31,7 @@
 
 # base classes
 from f110_gym.envs.base_classes import Simulator, Integrator
-
+from f110_gym.envs.observation import observation_factory
 
 from f110_gym.envs.utils import deep_update
 
@@ -120,13 +120,7 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
         self.ego_idx = self.config["ego_idx"]
         self.integrator = Integrator.from_string(self.config["integrator"])
         self.action_type = CarActionEnum.from_string(self.config["control_input"])
-
-        # observation
-        try:
-            assert isinstance(kwargs['observation_config'], dict), "expected 'observation_config' to be a dictionary"
-            self.observation_config = kwargs['observation_config']
-        except:
-            self.observation_config = {"type": "original"}
+        self.observation_config = self.config["observation_config"]
 
         # radius to consider done
         self.start_thresh = 0.5  # 10cm
@@ -246,6 +240,7 @@ def default_config(cls) -> dict:
             "ego_idx": 0,
             "integrator": "rk4",
             "control_input": "speed",
+            "observation_config": {"type": "original"},
         }
 
     def configure(self, config: dict) -> None:
diff --git a/gym/f110_gym/unittest/observation_test.py b/gym/f110_gym/unittest/observation_test.py
index 13d8c72e..b3545f4f 100644
--- a/gym/f110_gym/unittest/observation_test.py
+++ b/gym/f110_gym/unittest/observation_test.py
@@ -10,11 +10,13 @@
 from f110_gym.envs.observation import FeaturesObservation, observation_factory
 import gymnasium as gym
 
+from f110_gym.envs.utils import deep_update
+
 
 class TestObservationInterface(unittest.TestCase):
 
     @staticmethod
-    def _make_env(**kwargs) -> F110Env:
+    def _make_env(config={}) -> F110Env:
         import f110_gym
 
         example_dir = pathlib.Path(__file__).parent.parent.parent.parent / 'examples'
@@ -24,15 +26,25 @@ def _make_env(**kwargs) -> F110Env:
         conf = Namespace(**conf_dict)
         conf.map_path = str(example_dir / conf.map_path)
 
-        env = gym.make('f110_gym:f110-v0', map=conf.map_path, map_ext=conf.map_ext,
-                       num_agents=3, **kwargs)
+        conf = {
+            "map": conf.map_path,
+            "map_ext": conf.map_ext,
+            "num_agents": 1,
+            "timestep": 0.01,
+            "integrator": "rk4",
+            "control_input": "speed",
+            "params": {"mu": 1.0},
+        }
+        conf = deep_update(conf, config)
+
+        env = gym.make('f110_gym:f110-v0', config=conf)
         return env
 
     def test_original_obs_space(self):
         """
         Check backward compatibility with the original observation space.
         """
-        env = self._make_env(observation_config={"type": "original"})
+        env = self._make_env(config={"observation_config": {"type": "original"}})
 
         obs, _ = env.reset()
 
@@ -57,7 +69,7 @@ def test_features_observation(self):
         """
         features = ["pose_x", "pose_y", "pose_theta"]
 
-        env = self._make_env(observation_config={"type": "features", "features": features})
+        env = self._make_env(config={"observation_config": {"type": "features", "features": features}})
 
         # check the observation space is a dict
         self.assertTrue(isinstance(env.observation_space, gym.spaces.Dict))
@@ -97,7 +109,7 @@ def test_kinematic_obs_space(self):
         """
         Check the kinematic state observation space contains the correct features [x, y, theta, v].
         """
-        env = self._make_env(observation_config={"type": "kinematic_state"})
+        env = self._make_env(config={"observation_config": {"type": "kinematic_state"}})
 
         kinematic_features = ["pose_x", "pose_y", "pose_theta", "linear_vel_x", "delta"]
 
@@ -124,7 +136,7 @@ def test_dynamic_obs_space(self):
         """
         Check the dynamic state observation space contains the correct features.
         """
-        env = self._make_env(observation_config={"type": "dynamic_state"})
+        env = self._make_env(config={"observation_config": {"type": "dynamic_state"}})
 
         kinematic_features = ["pose_x", "pose_y", "pose_theta", "linear_vel_x", "ang_vel_z", "delta", "beta"]
 
@@ -170,5 +182,5 @@ def test_gymnasium_api(self):
         obs_type_ids = ["kinematic_state", "dynamic_state", "original"]
 
         for obs_type_id in obs_type_ids:
-            env = self._make_env(observation_config={"type": obs_type_id})
+            env = self._make_env(config={"observation_config": {"type": obs_type_id}})
             check_env(env.unwrapped, f"Observation {obs_type_id} breaks the gymnasium API")
\ No newline at end of file

From 9a3a02299308de07de0aef06d1b7219a0f9fa209 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Sat, 8 Jul 2023 22:55:08 +0200
Subject: [PATCH 25/42] linting

---
 gym/f110_gym/envs/f110_env.py             |  16 +-
 gym/f110_gym/envs/observation.py          | 215 ++++++++++++++++------
 gym/f110_gym/unittest/f110_env_test.py    |   6 +-
 gym/f110_gym/unittest/observation_test.py | 108 ++++++++---
 4 files changed, 252 insertions(+), 93 deletions(-)

diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index b70bf3d5..bf61439d 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -166,7 +166,9 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
         # observations
         self.agent_ids = [f"agent_{i}" for i in range(self.num_agents)]
 
-        assert "type" in self.observation_config, "observation_config must contain 'type' key"
+        assert (
+            "type" in self.observation_config
+        ), "observation_config must contain 'type' key"
         self.observation_type = observation_factory(env=self, **self.observation_config)
         self.observation_space = self.observation_type.space()
 
@@ -325,12 +327,12 @@ def step(self, action):
         # rendering observation
         F110Env.current_obs = obs
         self.render_obs = {
-            'ego_idx': self.sim.ego_idx,
-            'poses_x': self.sim.agent_poses[:, 0],
-            'poses_y': self.sim.agent_poses[:, 1],
-            'poses_theta': self.sim.agent_poses[:, 2],
-            'lap_times': self.lap_times,
-            'lap_counts': self.lap_counts,
+            "ego_idx": self.sim.ego_idx,
+            "poses_x": self.sim.agent_poses[:, 0],
+            "poses_y": self.sim.agent_poses[:, 1],
+            "poses_theta": self.sim.agent_poses[:, 2],
+            "lap_times": self.lap_times,
+            "lap_counts": self.lap_counts,
         }
 
         # times
diff --git a/gym/f110_gym/envs/observation.py b/gym/f110_gym/envs/observation.py
index 1883c6b6..7993b751 100644
--- a/gym/f110_gym/envs/observation.py
+++ b/gym/f110_gym/envs/observation.py
@@ -17,7 +17,7 @@ class Observation:
     :param kwargs: Additional arguments.
     """
 
-    def __init__(self, env: 'F110Env'):
+    def __init__(self, env: "F110Env"):
         self.env = env
 
     @abstractmethod
@@ -30,7 +30,7 @@ def observe(self):
 
 
 class OriginalObservation(Observation):
-    def __init__(self, env: 'F110Env'):
+    def __init__(self, env: "F110Env"):
         super().__init__(env)
 
     def space(self):
@@ -39,37 +39,84 @@ def space(self):
         scan_range = self.env.sim.agents[0].scan_simulator.max_range
         large_num = 1e30  # large number to avoid unbounded obs space (ie., low=-inf or high=inf)
 
-        obs_space = gym.spaces.Dict({
-            'ego_idx': gym.spaces.Discrete(num_agents),
-            'scans': gym.spaces.Box(low=0.0, high=scan_range, shape=(num_agents, scan_size), dtype=np.float32),
-            'poses_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
-            'poses_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
-            'poses_theta': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
-            'linear_vels_x': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
-            'linear_vels_y': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
-            'ang_vels_z': gym.spaces.Box(low=-large_num, high=large_num, shape=(num_agents,), dtype=np.float32),
-            'collisions': gym.spaces.Box(low=0.0, high=1.0, shape=(num_agents,), dtype=np.float32),
-            'lap_times': gym.spaces.Box(low=0.0, high=large_num, shape=(num_agents,), dtype=np.float32),
-            'lap_counts': gym.spaces.Box(low=0.0, high=large_num, shape=(num_agents,), dtype=np.float32),
-        })
+        obs_space = gym.spaces.Dict(
+            {
+                "ego_idx": gym.spaces.Discrete(num_agents),
+                "scans": gym.spaces.Box(
+                    low=0.0,
+                    high=scan_range,
+                    shape=(num_agents, scan_size),
+                    dtype=np.float32,
+                ),
+                "poses_x": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(num_agents,),
+                    dtype=np.float32,
+                ),
+                "poses_y": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(num_agents,),
+                    dtype=np.float32,
+                ),
+                "poses_theta": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(num_agents,),
+                    dtype=np.float32,
+                ),
+                "linear_vels_x": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(num_agents,),
+                    dtype=np.float32,
+                ),
+                "linear_vels_y": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(num_agents,),
+                    dtype=np.float32,
+                ),
+                "ang_vels_z": gym.spaces.Box(
+                    low=-large_num,
+                    high=large_num,
+                    shape=(num_agents,),
+                    dtype=np.float32,
+                ),
+                "collisions": gym.spaces.Box(
+                    low=0.0, high=1.0, shape=(num_agents,), dtype=np.float32
+                ),
+                "lap_times": gym.spaces.Box(
+                    low=0.0, high=large_num, shape=(num_agents,), dtype=np.float32
+                ),
+                "lap_counts": gym.spaces.Box(
+                    low=0.0, high=large_num, shape=(num_agents,), dtype=np.float32
+                ),
+            }
+        )
 
         return obs_space
 
     def observe(self):
         # state indices
-        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(7)  # 7 largest state size (ST Model)
-
-        observations = {'ego_idx': self.env.sim.ego_idx,
-                        'scans': [],
-                        'poses_x': [],
-                        'poses_y': [],
-                        'poses_theta': [],
-                        'linear_vels_x': [],
-                        'linear_vels_y': [],
-                        'ang_vels_z': [],
-                        'collisions': [],
-                        'lap_times': [],
-                        'lap_counts': []}
+        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(
+            7
+        )  # 7 largest state size (ST Model)
+
+        observations = {
+            "ego_idx": self.env.sim.ego_idx,
+            "scans": [],
+            "poses_x": [],
+            "poses_y": [],
+            "poses_theta": [],
+            "linear_vels_x": [],
+            "linear_vels_y": [],
+            "ang_vels_z": [],
+            "collisions": [],
+            "lap_times": [],
+            "lap_counts": [],
+        }
 
         for i, agent in enumerate(self.env.sim.agents):
             agent_scan = self.env.sim.agent_scans[i]
@@ -79,29 +126,33 @@ def observe(self):
 
             x, y, theta = agent.state[xi], agent.state[yi], agent.state[yawi]
             vx, vy = agent.state[vxi], 0.0
-            angvel = 0.0 if len(agent.state) < 7 else agent.state[yaw_ratei]  # set 0.0 when KST Model
-
-            observations['scans'].append(agent_scan)
-            observations['poses_x'].append(x)
-            observations['poses_y'].append(y)
-            observations['poses_theta'].append(theta)
-            observations['linear_vels_x'].append(vx)
-            observations['linear_vels_y'].append(vy)
-            observations['ang_vels_z'].append(angvel)
-            observations['collisions'].append(collision)
-            observations['lap_times'].append(lap_time)
-            observations['lap_counts'].append(lap_count)
+            angvel = (
+                0.0 if len(agent.state) < 7 else agent.state[yaw_ratei]
+            )  # set 0.0 when KST Model
+
+            observations["scans"].append(agent_scan)
+            observations["poses_x"].append(x)
+            observations["poses_y"].append(y)
+            observations["poses_theta"].append(theta)
+            observations["linear_vels_x"].append(vx)
+            observations["linear_vels_y"].append(vy)
+            observations["ang_vels_z"].append(angvel)
+            observations["collisions"].append(collision)
+            observations["lap_times"].append(lap_time)
+            observations["lap_counts"].append(lap_count)
 
         # cast to match observation space
         for key in observations.keys():
-            if isinstance(observations[key], np.ndarray) or isinstance(observations[key], list):
+            if isinstance(observations[key], np.ndarray) or isinstance(
+                observations[key], list
+            ):
                 observations[key] = np.array(observations[key], dtype=np.float32)
 
         return observations
 
 
 class FeaturesObservation(Observation):
-    def __init__(self, env: 'F110Env', features: List[str]):
+    def __init__(self, env: "F110Env", features: List[str]):
         super().__init__(env)
         self.features = features
 
@@ -113,27 +164,55 @@ def space(self):
         complete_space = {}
         for agent_id in self.env.agent_ids:
             agent_dict = {
-                "scan": gym.spaces.Box(low=0.0, high=scan_range, shape=(scan_size,), dtype=np.float32),
-                "pose_x": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
-                "pose_y": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
-                "pose_theta": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
-                "linear_vel_x": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
-                "linear_vel_y": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
-                "ang_vel_z": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
-                "delta": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
-                "beta": gym.spaces.Box(low=-large_num, high=large_num, shape=(), dtype=np.float32),
-                "collision": gym.spaces.Box(low=0.0, high=1.0, shape=(), dtype=np.float32),
-                "lap_time": gym.spaces.Box(low=0.0, high=large_num, shape=(), dtype=np.float32),
-                "lap_count": gym.spaces.Box(low=0.0, high=large_num, shape=(), dtype=np.float32),
+                "scan": gym.spaces.Box(
+                    low=0.0, high=scan_range, shape=(scan_size,), dtype=np.float32
+                ),
+                "pose_x": gym.spaces.Box(
+                    low=-large_num, high=large_num, shape=(), dtype=np.float32
+                ),
+                "pose_y": gym.spaces.Box(
+                    low=-large_num, high=large_num, shape=(), dtype=np.float32
+                ),
+                "pose_theta": gym.spaces.Box(
+                    low=-large_num, high=large_num, shape=(), dtype=np.float32
+                ),
+                "linear_vel_x": gym.spaces.Box(
+                    low=-large_num, high=large_num, shape=(), dtype=np.float32
+                ),
+                "linear_vel_y": gym.spaces.Box(
+                    low=-large_num, high=large_num, shape=(), dtype=np.float32
+                ),
+                "ang_vel_z": gym.spaces.Box(
+                    low=-large_num, high=large_num, shape=(), dtype=np.float32
+                ),
+                "delta": gym.spaces.Box(
+                    low=-large_num, high=large_num, shape=(), dtype=np.float32
+                ),
+                "beta": gym.spaces.Box(
+                    low=-large_num, high=large_num, shape=(), dtype=np.float32
+                ),
+                "collision": gym.spaces.Box(
+                    low=0.0, high=1.0, shape=(), dtype=np.float32
+                ),
+                "lap_time": gym.spaces.Box(
+                    low=0.0, high=large_num, shape=(), dtype=np.float32
+                ),
+                "lap_count": gym.spaces.Box(
+                    low=0.0, high=large_num, shape=(), dtype=np.float32
+                ),
             }
-            complete_space[agent_id] = gym.spaces.Dict({k: agent_dict[k] for k in self.features})
+            complete_space[agent_id] = gym.spaces.Dict(
+                {k: agent_dict[k] for k in self.features}
+            )
 
         obs_space = gym.spaces.Dict(complete_space)
         return obs_space
 
     def observe(self):
         # state indices
-        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(7)  # 7 largest state size (ST Model)
+        xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range(
+            7
+        )  # 7 largest state size (ST Model)
 
         obs = {}  # dictionary agent_id -> observation dict
 
@@ -146,8 +225,12 @@ def observe(self):
             x, y, theta = agent.state[xi], agent.state[yi], agent.state[yawi]
             vx, vy = agent.state[vxi], 0.0
             delta = agent.state[deltai]
-            beta = 0.0 if len(agent.state) < 7 else agent.state[slipi]  # set 0.0 when KST Model
-            angvel = 0.0 if len(agent.state) < 7 else agent.state[yaw_ratei]  # set 0.0 when KST Model
+            beta = (
+                0.0 if len(agent.state) < 7 else agent.state[slipi]
+            )  # set 0.0 when KST Model
+            angvel = (
+                0.0 if len(agent.state) < 7 else agent.state[yaw_ratei]
+            )  # set 0.0 when KST Model
 
             # create agent's observation dict
             agent_obs = {
@@ -170,7 +253,9 @@ def observe(self):
 
             # cast to match observation space
             for key in obs[agent_id].keys():
-                if isinstance(obs[agent_id][key], np.ndarray) or isinstance(obs[agent_id][key], list):
+                if isinstance(obs[agent_id][key], np.ndarray) or isinstance(
+                    obs[agent_id][key], list
+                ):
                     obs[agent_id][key] = np.array(obs[agent_id][key], dtype=np.float32)
                 if isinstance(obs[agent_id][key], float):
                     obs[agent_id][key] = np.float32(obs[agent_id][key])
@@ -178,7 +263,7 @@ def observe(self):
         return obs
 
 
-def observation_factory(env: 'F110Env', type: str, **kwargs) -> Observation:
+def observation_factory(env: "F110Env", type: str, **kwargs) -> Observation:
     if type == "original":
         return OriginalObservation(env)
     elif type == "features":
@@ -187,7 +272,15 @@ def observation_factory(env: 'F110Env', type: str, **kwargs) -> Observation:
         features = ["pose_x", "pose_y", "delta", "linear_vel_x", "pose_theta"]
         return FeaturesObservation(env, features=features)
     elif type == "dynamic_state":
-        features = ["pose_x", "pose_y", "delta", "linear_vel_x", "pose_theta", "ang_vel_z", "beta"]
+        features = [
+            "pose_x",
+            "pose_y",
+            "delta",
+            "linear_vel_x",
+            "pose_theta",
+            "ang_vel_z",
+            "beta",
+        ]
         return FeaturesObservation(env, features=features)
     else:
         raise ValueError(f"Invalid observation type {type}.")
diff --git a/gym/f110_gym/unittest/f110_env_test.py b/gym/f110_gym/unittest/f110_env_test.py
index 940a7ab8..3f04eb11 100644
--- a/gym/f110_gym/unittest/f110_env_test.py
+++ b/gym/f110_gym/unittest/f110_env_test.py
@@ -81,8 +81,10 @@ def test_configure_method(self):
                 self.assertEqual(base_val, extended_val, f"{par} should be the same")
 
         # finally, run a simulation and check that the results are the same
-        obs0, _ = base_env.reset(options={"poses": np.array([[0.0, 0.0, np.pi/2]])})
-        obs1, _ = extended_env.reset(options={"poses": np.array([[0.0, 0.0, np.pi/2]])})
+        obs0, _ = base_env.reset(options={"poses": np.array([[0.0, 0.0, np.pi / 2]])})
+        obs1, _ = extended_env.reset(
+            options={"poses": np.array([[0.0, 0.0, np.pi / 2]])}
+        )
         done0 = done1 = False
         t = 0
 
diff --git a/gym/f110_gym/unittest/observation_test.py b/gym/f110_gym/unittest/observation_test.py
index b3545f4f..07a1aa84 100644
--- a/gym/f110_gym/unittest/observation_test.py
+++ b/gym/f110_gym/unittest/observation_test.py
@@ -14,14 +14,13 @@
 
 
 class TestObservationInterface(unittest.TestCase):
-
     @staticmethod
     def _make_env(config={}) -> F110Env:
         import f110_gym
 
-        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / 'examples'
+        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / "examples"
 
-        with open(example_dir / 'config_example_map.yaml') as file:
+        with open(example_dir / "config_example_map.yaml") as file:
             conf_dict = yaml.load(file, Loader=yaml.FullLoader)
         conf = Namespace(**conf_dict)
         conf.map_path = str(example_dir / conf.map_path)
@@ -37,7 +36,7 @@ def _make_env(config={}) -> F110Env:
         }
         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_original_obs_space(self):
@@ -48,12 +47,39 @@ def test_original_obs_space(self):
 
         obs, _ = env.reset()
 
-        obs_keys = ['ego_idx', 'scans', 'poses_x', 'poses_y', 'poses_theta', 'linear_vels_x',
-                    'linear_vels_y', 'ang_vels_z', 'collisions', 'lap_times', 'lap_counts']
+        obs_keys = [
+            "ego_idx",
+            "scans",
+            "poses_x",
+            "poses_y",
+            "poses_theta",
+            "linear_vels_x",
+            "linear_vels_y",
+            "ang_vels_z",
+            "collisions",
+            "lap_times",
+            "lap_counts",
+        ]
 
         # check that the observation space has the correct types
-        self.assertTrue(all([isinstance(env.observation_space.spaces[k], Box) for k in obs_keys if k != 'ego_idx']))
-        self.assertTrue(all([env.observation_space.spaces[k].dtype == np.float32 for k in obs_keys if k != 'ego_idx']))
+        self.assertTrue(
+            all(
+                [
+                    isinstance(env.observation_space.spaces[k], Box)
+                    for k in obs_keys
+                    if k != "ego_idx"
+                ]
+            )
+        )
+        self.assertTrue(
+            all(
+                [
+                    env.observation_space.spaces[k].dtype == np.float32
+                    for k in obs_keys
+                    if k != "ego_idx"
+                ]
+            )
+        )
 
         # check the observation space is a dict
         self.assertTrue(isinstance(obs, dict))
@@ -69,7 +95,9 @@ def test_features_observation(self):
         """
         features = ["pose_x", "pose_y", "pose_theta"]
 
-        env = self._make_env(config={"observation_config": {"type": "features", "features": features}})
+        env = self._make_env(
+            config={"observation_config": {"type": "features", "features": features}}
+        )
 
         # check the observation space is a dict
         self.assertTrue(isinstance(env.observation_space, gym.spaces.Dict))
@@ -92,9 +120,15 @@ def test_features_observation(self):
 
         for i, agent_id in enumerate(env.agent_ids):
             pose_x, pose_y, pose_theta = env.sim.agent_poses[i]
-            obs_x, obs_y, obs_theta = obs[agent_id]["pose_x"], obs[agent_id]["pose_y"], obs[agent_id]["pose_theta"]
-
-            for ground_truth, observation in zip([pose_x, pose_y, pose_theta], [obs_x, obs_y, obs_theta]):
+            obs_x, obs_y, obs_theta = (
+                obs[agent_id]["pose_x"],
+                obs[agent_id]["pose_y"],
+                obs[agent_id]["pose_theta"],
+            )
+
+            for ground_truth, observation in zip(
+                [pose_x, pose_y, pose_theta], [obs_x, obs_y, obs_theta]
+            ):
                 self.assertAlmostEqual(ground_truth, observation)
 
     def test_unexisting_obs_space(self):
@@ -125,11 +159,16 @@ def test_kinematic_obs_space(self):
 
         for i, agent_id in enumerate(env.agent_ids):
             pose_x, pose_y, _, velx, pose_theta, _, _ = env.sim.agents[i].state
-            obs_x, obs_y, obs_theta = obs[agent_id]["pose_x"], obs[agent_id]["pose_y"], obs[agent_id]["pose_theta"]
+            obs_x, obs_y, obs_theta = (
+                obs[agent_id]["pose_x"],
+                obs[agent_id]["pose_y"],
+                obs[agent_id]["pose_theta"],
+            )
             obs_velx = obs[agent_id]["linear_vel_x"]
 
-            for ground_truth, observed in zip([pose_x, pose_y, pose_theta, velx],
-                                              [obs_x, obs_y, obs_theta, obs_velx]):
+            for ground_truth, observed in zip(
+                [pose_x, pose_y, pose_theta, velx], [obs_x, obs_y, obs_theta, obs_velx]
+            ):
                 self.assertAlmostEqual(ground_truth, observed)
 
     def test_dynamic_obs_space(self):
@@ -138,7 +177,15 @@ def test_dynamic_obs_space(self):
         """
         env = self._make_env(config={"observation_config": {"type": "dynamic_state"}})
 
-        kinematic_features = ["pose_x", "pose_y", "pose_theta", "linear_vel_x", "ang_vel_z", "delta", "beta"]
+        kinematic_features = [
+            "pose_x",
+            "pose_y",
+            "pose_theta",
+            "linear_vel_x",
+            "ang_vel_z",
+            "delta",
+            "beta",
+        ]
 
         # check kinematic features are in the observation space
         for agent_id in env.agent_ids:
@@ -154,11 +201,21 @@ def test_dynamic_obs_space(self):
             pose_x, pose_y, delta, velx, pose_theta, _, beta = env.sim.agents[i].state
 
             agent_obs = obs[agent_id]
-            obs_x, obs_y, obs_theta = agent_obs["pose_x"], agent_obs["pose_y"], agent_obs["pose_theta"]
-            obs_velx, obs_delta, obs_beta = agent_obs["linear_vel_x"], agent_obs["delta"], agent_obs["beta"]
-
-            for ground_truth, observed in zip([pose_x, pose_y, pose_theta, velx, delta, beta],
-                                              [obs_x, obs_y, obs_theta, obs_velx, obs_delta, obs_beta]):
+            obs_x, obs_y, obs_theta = (
+                agent_obs["pose_x"],
+                agent_obs["pose_y"],
+                agent_obs["pose_theta"],
+            )
+            obs_velx, obs_delta, obs_beta = (
+                agent_obs["linear_vel_x"],
+                agent_obs["delta"],
+                agent_obs["beta"],
+            )
+
+            for ground_truth, observed in zip(
+                [pose_x, pose_y, pose_theta, velx, delta, beta],
+                [obs_x, obs_y, obs_theta, obs_velx, obs_delta, obs_beta],
+            ):
                 self.assertAlmostEqual(ground_truth, observed)
 
     def test_consistency_observe_space(self):
@@ -172,7 +229,10 @@ def test_consistency_observe_space(self):
             space = obs_type.space()
             observation = obs_type.observe()
 
-            self.assertTrue(space.contains(observation), f"Observation {obs_type_id} is not contained in its space")
+            self.assertTrue(
+                space.contains(observation),
+                f"Observation {obs_type_id} is not contained in its space",
+            )
 
     def test_gymnasium_api(self):
         import f110_gym
@@ -183,4 +243,6 @@ 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")
\ No newline at end of file
+            check_env(
+                env.unwrapped, f"Observation {obs_type_id} breaks the gymnasium API"
+            )

From 6124da2ccd8a45732efff06500ff06996f3ca508 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Sat, 8 Jul 2023 23:00:17 +0200
Subject: [PATCH 26/42] update example to use `kinematic_state` as observation

---
 examples/waypoint_follow.py | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py
index dc814e2d..f7d2b296 100644
--- a/examples/waypoint_follow.py
+++ b/examples/waypoint_follow.py
@@ -357,6 +357,7 @@ def render_callback(env_renderer):
             "timestep": 0.01,
             "integrator": "rk4",
             "control_input": "speed",
+            "observation_config": {"type": "kinematic_state"},
             "params": {"mu": 1.0},
         },
         render_mode="human",
@@ -372,10 +373,11 @@ def render_callback(env_renderer):
     start = time.time()
 
     while not done:
+        agent_id = env.agent_ids[0]
         speed, steer = planner.plan(
-            obs["poses_x"][0],
-            obs["poses_y"][0],
-            obs["poses_theta"][0],
+            obs[agent_id]["pose_x"],
+            obs[agent_id]["pose_y"],
+            obs[agent_id]["pose_theta"],
             work["tlad"],
             work["vgain"],
         )

From c088f03970b83feba740fc691e8ead3d7175ed6c Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Sat, 8 Jul 2023 23:27:28 +0200
Subject: [PATCH 27/42] use new config dictionary, refactor old test to
 simplified track ingestion, linting

---
 examples/waypoint_follow.py            | 12 +----
 gym/f110_gym/envs/action.py            |  9 ++--
 gym/f110_gym/envs/base_classes.py      |  4 +-
 gym/f110_gym/envs/cubic_spline.py      | 20 ++++----
 gym/f110_gym/envs/f110_env.py          | 24 ++++++----
 gym/f110_gym/envs/track.py             | 64 ++++++++++++++++----------
 gym/f110_gym/unittest/f110_env_test.py | 10 +---
 gym/f110_gym/unittest/gym_api_test.py  |  0
 8 files changed, 75 insertions(+), 68 deletions(-)
 delete mode 100644 gym/f110_gym/unittest/gym_api_test.py

diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py
index f04758e9..c7af248f 100644
--- a/examples/waypoint_follow.py
+++ b/examples/waypoint_follow.py
@@ -199,15 +199,6 @@ def render_waypoints(self, e):
         """
         update waypoints being drawn by EnvRenderer
         """
-
-        # points = self.waypoints
-
-        points = np.vstack(
-            (
-                self.waypoints[:, self.conf.wpt_xind],
-                self.waypoints[:, self.conf.wpt_yind],
-            )
-        ).T
         points = self.waypoints[:, :2]
 
         scaled_points = 50.0 * points
@@ -310,8 +301,7 @@ def main():
         "tlad": 0.82461887897713965,
         "vgain": 1.375,
     }  # 0.90338203837889}
-    env = gym.make('f110_gym:f110-v0', map="Example", num_agents=1,
-                   timestep=0.01, integrator=Integrator.Euler, render_mode='human')
+    env = gym.make('f110_gym:f110-v0', config={"map": "Example", "num_agents": 1}, render_mode='human')
 
     planner = PurePursuitPlanner(track=env.track, wb=0.17145 + 0.15875)  # FlippyPlanner(speed=0.2, flip_every=1, steer=10)
 
diff --git a/gym/f110_gym/envs/action.py b/gym/f110_gym/envs/action.py
index f300b6ca..9100b2d4 100644
--- a/gym/f110_gym/envs/action.py
+++ b/gym/f110_gym/envs/action.py
@@ -19,6 +19,7 @@ def from_string(action: str):
         else:
             raise ValueError(f"Unknown action type {action}")
 
+
 class CarAction:
     def __init__(self) -> None:
         self._action_type = None
@@ -37,9 +38,7 @@ def __init__(self) -> None:
         super().__init__()
         self._action_type = "accl"
 
-    def act(
-        self, action: Tuple[float, float], state, params
-    ) -> Tuple[float, float]:
+    def act(self, action: Tuple[float, float], state, params) -> Tuple[float, float]:
         return action
 
 
@@ -48,7 +47,9 @@ def __init__(self) -> None:
         super().__init__()
         self._action_type = "speed"
 
-    def act(self, action: Tuple[float, float], state: np.ndarray, params: Dict) -> Tuple[float, float]:
+    def act(
+        self, action: Tuple[float, float], state: np.ndarray, params: Dict
+    ) -> Tuple[float, float]:
         accl, sv = pid(
             action[0],
             action[1],
diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index 98bde179..86090b69 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -313,7 +313,9 @@ def update_pose(self, raw_steer, vel):
         if self.action_type.type is None:
             raise ValueError("No Control Action Type Specified.")
 
-        accl, sv = self.action_type.act(action=(vel, steer), state=self.state, params=self.params)
+        accl, sv = self.action_type.act(
+            action=(vel, steer), state=self.state, params=self.params
+        )
 
         if self.integrator is Integrator.RK4:
             # RK4 integration
diff --git a/gym/f110_gym/envs/cubic_spline.py b/gym/f110_gym/envs/cubic_spline.py
index ba75add9..925b4d4d 100644
--- a/gym/f110_gym/envs/cubic_spline.py
+++ b/gym/f110_gym/envs/cubic_spline.py
@@ -20,7 +20,6 @@ class CubicSpline1D:
     """
 
     def __init__(self, x, y):
-
         h = np.diff(x)
         if np.any(h < 0):
             raise ValueError("x coordinates must be sorted in ascending order")
@@ -41,8 +40,9 @@ def __init__(self, x, y):
         # calc spline coefficient b and d
         for i in range(self.nx - 1):
             d = (self.c[i + 1] - self.c[i]) / (3.0 * h[i])
-            b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) \
-                - h[i] / 3.0 * (2.0 * self.c[i] + self.c[i + 1])
+            b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) - h[i] / 3.0 * (
+                2.0 * self.c[i] + self.c[i + 1]
+            )
             self.d.append(d)
             self.b.append(b)
 
@@ -62,8 +62,9 @@ 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
+        position = (
+            self.a[i] + self.b[i] * dx + self.c[i] * dx**2.0 + self.d[i] * dx**3.0
+        )
 
         return position
 
@@ -84,7 +85,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):
@@ -136,8 +137,9 @@ def __calc_B(self, h, a):
         """
         B = np.zeros(self.nx)
         for i in range(self.nx - 2):
-            B[i + 1] = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1] \
-                       - 3.0 * (a[i + 1] - a[i]) / h[i]
+            B[i + 1] = (
+                3.0 * (a[i + 2] - a[i + 1]) / h[i + 1] - 3.0 * (a[i + 1] - a[i]) / h[i]
+            )
         return B
 
 
@@ -202,7 +204,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/f110_env.py b/gym/f110_gym/envs/f110_env.py
index cbb2f3f8..6cc0f8c9 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -30,6 +30,7 @@
 from f110_gym.envs.action import CarActionEnum
 
 from f110_gym.envs.track import Track
+
 # base classes
 from f110_gym.envs.base_classes import Simulator, Integrator
 
@@ -64,10 +65,7 @@ class F110Env(gym.Env):
     Args:
         kwargs:
             seed (int, default=12345): seed for random state and reproducibility
-
-            map (str, default='vegas'): name of the map used for the environment. Currently, available environments include: 'berlin', 'vegas', 'skirk'. You could use a string of the absolute path to the yaml file of your custom map.
-
-            map_ext (str, default='png'): image extension of the map image file. For example 'png', 'pgm'
+            map (str, default='vegas'): name of the map used for the environment.
 
             params (dict, default={'mu': 1.0489, 'C_Sf':, 'C_Sr':, 'lf': 0.15875, 'lr': 0.17145, 'h': 0.074, 'm': 3.74, 'I': 0.04712, 's_min': -0.4189, 's_max': 0.4189, 'sv_min': -3.2, 'sv_max': 3.2, 'v_switch':7.319, 'a_max': 9.51, 'v_min':-5.0, 'v_max': 20.0, 'width': 0.31, 'length': 0.58}): dictionary of vehicle parameters.
             mu: surface friction coefficient
@@ -151,16 +149,24 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
         self.start_rot = np.eye(2)
 
         # initiate stuff
-        self.sim = Simulator(self.params, self.num_agents, self.seed, time_step=self.timestep,
-                             integrator=self.integrator)
+        self.sim = Simulator(
+            self.params,
+            self.num_agents,
+            self.seed,
+            time_step=self.timestep,
+            integrator=self.integrator,
+        )
         self.sim.set_map(self.map_name)
-        self.track = Track.from_track_name(self.map_name)  # load track in gym env for convenience
+        self.track = Track.from_track_name(
+            self.map_name
+        )  # load track in gym env for convenience
 
         # observation space
         # NOTE: keep original structure of observation space (dict). just define it as a dict space and define bounds
         scan_size, scan_range = (
             self.sim.agents[0].scan_simulator.num_beams,
-            self.sim.agents[0].scan_simulator.max_range + 0.1,  # add 10cm because scan is not capped,
+            self.sim.agents[0].scan_simulator.max_range
+            + 0.1,  # add 10cm because scan is not capped,
         )
         large_num = 1e30  # large number to avoid unbounded obs space (ie., low=-inf or high=inf)
         self.observation_space = gym.spaces.Dict(
@@ -263,7 +269,7 @@ def default_config(cls) -> dict:
         """
         return {
             "seed": 12345,
-            #"map": os.path.dirname(os.path.abspath(__file__)) + "/maps/skirk",
+            "map": "Example",
             "params": {
                 "mu": 1.0489,
                 "C_Sf": 4.718,
diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
index dfb782dd..a8c9bf67 100644
--- a/gym/f110_gym/envs/track.py
+++ b/gym/f110_gym/envs/track.py
@@ -24,15 +24,17 @@ class Raceline:
 
     length: float
 
-    def __init__(self,
-                 xs: np.ndarray,
-                 ys: np.ndarray,
-                 velxs: np.ndarray,
-                 ss: np.ndarray = None,
-                 psis: np.ndarray = None,
-                 kappas: np.ndarray = None,
-                 accxs: np.ndarray = None,
-                 spline: CubicSpline2D = None):
+    def __init__(
+        self,
+        xs: np.ndarray,
+        ys: np.ndarray,
+        velxs: np.ndarray,
+        ss: np.ndarray = None,
+        psis: np.ndarray = None,
+        kappas: np.ndarray = None,
+        accxs: np.ndarray = None,
+        spline: CubicSpline2D = None,
+    ):
         assert xs.shape == ys.shape == velxs.shape, "inconsistent shapes for x, y, vel"
 
         self.n = xs.shape[0]
@@ -52,7 +54,9 @@ def __init__(self,
         self.spline = spline if spline is not None else CubicSpline2D(xs, ys)
 
     @staticmethod
-    def from_centerline_file(filepath: pathlib.Path, delimiter: str = ',', fixed_speed: float = 1.0):
+    def from_centerline_file(
+        filepath: pathlib.Path, delimiter: str = ",", fixed_speed: float = 1.0
+    ):
         assert filepath.exists(), f"input filepath does not exist ({filepath})"
         waypoints = np.loadtxt(filepath, delimiter=delimiter)
         assert waypoints.shape[1] == 4, "expected waypoints as [x, y, w_left, w_right]"
@@ -84,15 +88,19 @@ def from_centerline_file(filepath: pathlib.Path, delimiter: str = ',', fixed_spe
             ys=np.array(ys).astype(np.float32),
             psis=np.array(yaws).astype(np.float32),
             kappas=np.array(ks).astype(np.float32),
-            velxs=np.ones_like(ss).astype(np.float32),  # centerline does not have a speed profile, keep it constant at 1.0 m/s
+            velxs=np.ones_like(ss).astype(
+                np.float32
+            ),  # centerline does not have a speed profile, keep it constant at 1.0 m/s
             accxs=np.zeros_like(ss).astype(np.float32),  # constant acceleration
         )
 
     @staticmethod
-    def from_raceline_file(filepath: pathlib.Path, delimiter: str = ';'):
+    def from_raceline_file(filepath: pathlib.Path, delimiter: str = ";"):
         assert filepath.exists(), f"input filepath does not exist ({filepath})"
         waypoints = np.loadtxt(filepath, delimiter=delimiter).astype(np.float32)
-        assert waypoints.shape[1] == 7, "expected waypoints as [s, x, y, psi, k, vx, ax]"
+        assert (
+            waypoints.shape[1] == 7
+        ), "expected waypoints as [s, x, y, psi, k, vx, ax]"
         return Raceline(
             ss=waypoints[:, 0],
             xs=waypoints[:, 1],
@@ -128,7 +136,7 @@ def find_track_dir(track_name):
             if track_name == str(dir.stem).replace(" ", ""):
                 return dir
 
-    raise FileNotFoundError(f'no mapdir matching {track_name} in {[map_dir]}')
+    raise FileNotFoundError(f"no mapdir matching {track_name} in {[map_dir]}")
 
 
 @dataclass
@@ -141,13 +149,13 @@ class Track:
     raceline: Raceline
 
     def __init__(
-            self,
-            spec: TrackSpec,
-            filepath: str,
-            ext: str,
-            occupancy_map: np.ndarray,
-            centerline: Raceline = None,
-            raceline: Raceline = None,
+        self,
+        spec: TrackSpec,
+        filepath: str,
+        ext: str,
+        occupancy_map: np.ndarray,
+        centerline: Raceline = None,
+        raceline: Raceline = None,
     ):
         self.spec = spec
         self.filepath = filepath
@@ -161,26 +169,32 @@ def from_track_name(track: str):
         try:
             track_dir = find_track_dir(track)
             # load track spec
-            with open(track_dir / f"{track}_map.yaml", 'r') as yaml_stream:
+            with open(track_dir / f"{track}_map.yaml", "r") as yaml_stream:
                 map_metadata = yaml.safe_load(yaml_stream)
                 track_spec = TrackSpec(name=track, **map_metadata)
 
             # load occupancy grid
             map_filename = pathlib.Path(track_spec.image)
-            image = Image.open(track_dir / str(map_filename)).transpose(Transpose.FLIP_TOP_BOTTOM)
+            image = Image.open(track_dir / str(map_filename)).transpose(
+                Transpose.FLIP_TOP_BOTTOM
+            )
             occupancy_map = np.array(image).astype(np.float32)
             occupancy_map[occupancy_map <= 128] = 0.0
             occupancy_map[occupancy_map > 128] = 255.0
 
             # if exists, load centerline
             if (track_dir / f"{track}_centerline.csv").exists():
-                centerline = Raceline.from_centerline_file(track_dir / f"{track}_centerline.csv")
+                centerline = Raceline.from_centerline_file(
+                    track_dir / f"{track}_centerline.csv"
+                )
             else:
                 centerline = None
 
             # if exists, load raceline
             if (track_dir / f"{track}_raceline.csv").exists():
-                raceline = Raceline.from_raceline_file(track_dir / f"{track}_raceline.csv")
+                raceline = Raceline.from_raceline_file(
+                    track_dir / f"{track}_raceline.csv"
+                )
             else:
                 raceline = centerline
 
diff --git a/gym/f110_gym/unittest/f110_env_test.py b/gym/f110_gym/unittest/f110_env_test.py
index 940a7ab8..16df8859 100644
--- a/gym/f110_gym/unittest/f110_env_test.py
+++ b/gym/f110_gym/unittest/f110_env_test.py
@@ -13,16 +13,8 @@ class TestEnvInterface(unittest.TestCase):
     def _make_env(self, config={}):
         import f110_gym
 
-        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / "examples"
-
-        with open(example_dir / "config_example_map.yaml") as file:
-            conf_dict = yaml.load(file, Loader=yaml.FullLoader)
-        conf = Namespace(**conf_dict)
-        conf.map_path = str(example_dir / conf.map_path)
-
         conf = {
-            "map": conf.map_path,
-            "map_ext": conf.map_ext,
+            "map": "Example",
             "num_agents": 1,
             "timestep": 0.01,
             "integrator": "rk4",
diff --git a/gym/f110_gym/unittest/gym_api_test.py b/gym/f110_gym/unittest/gym_api_test.py
deleted file mode 100644
index e69de29b..00000000

From 6e159a3b391683229af3cc149ad40fdb08e671de Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Sat, 8 Jul 2023 23:28:53 +0200
Subject: [PATCH 28/42] Set default map vegas, as originally done

---
 gym/f110_gym/envs/f110_env.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 6cc0f8c9..00466a08 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -269,7 +269,7 @@ def default_config(cls) -> dict:
         """
         return {
             "seed": 12345,
-            "map": "Example",
+            "map": "Vegas",
             "params": {
                 "mu": 1.0489,
                 "C_Sf": 4.718,

From 0ca3a1c3646cc10fdb16be7ffd9360d38c85476c Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Sun, 9 Jul 2023 14:08:57 +0200
Subject: [PATCH 29/42] merge v.1.0.0 and adapt test with simplified track
 ingestion, add load_spec method for reuse in test, add test to check map dir
 structure, update waypoint_follow example, linting

---
 examples/waypoint_follow.py               |  41 ++--
 gym/f110_gym/envs/track.py                |  20 +-
 gym/f110_gym/unittest/__init__.py         |   1 -
 gym/f110_gym/unittest/observation_test.py |  10 +-
 gym/f110_gym/unittest/scan_sim.py         | 259 +++++++++++++++-------
 gym/f110_gym/unittest/track_test.py       |  55 ++++-
 6 files changed, 273 insertions(+), 113 deletions(-)

diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py
index 1767f741..080003bf 100644
--- a/examples/waypoint_follow.py
+++ b/examples/waypoint_follow.py
@@ -181,8 +181,10 @@ class PurePursuitPlanner:
 
     def __init__(self, track, wb):
         self.wheelbase = wb
-        self.waypoints = np.stack([track.raceline.xs, track.raceline.ys, track.raceline.vxs]).T
-        self.max_reacquire = 20.
+        self.waypoints = np.stack(
+            [track.raceline.xs, track.raceline.ys, track.raceline.vxs]
+        ).T
+        self.max_reacquire = 20.0
 
         self.drawn_waypoints = []
 
@@ -301,9 +303,20 @@ def main():
         "tlad": 0.82461887897713965,
         "vgain": 1.375,
     }  # 0.90338203837889}
-    env = gym.make('f110_gym:f110-v0', config={"map": "Example", "num_agents": 1}, render_mode='human')
 
-    planner = PurePursuitPlanner(track=env.track, wb=0.17145 + 0.15875)  # FlippyPlanner(speed=0.2, flip_every=1, steer=10)
+    env = gym.make(
+        "f110_gym:f110-v0",
+        config={
+            "map": "Example",
+            "num_agents": 1,
+            "observation_config": {"type": "kinematic_state"},
+        },
+        render_mode="human",
+    )
+
+    planner = PurePursuitPlanner(
+        track=env.track, wb=0.17145 + 0.15875
+    )  # FlippyPlanner(speed=0.2, flip_every=1, steer=10)
 
     def render_callback(env_renderer):
         # custom extra drawing function
@@ -323,26 +336,6 @@ def render_callback(env_renderer):
 
         planner.render_waypoints(env_renderer)
 
-    # old API
-    # env = gym.make('f110_gym:f110-v0', map=conf.map_path, map_ext=conf.map_ext,
-    #                num_agents=1, timestep=0.01, integrator=Integrator.RK4,
-    #                render_mode='human')
-
-    # new API
-    env = gym.make(
-        "f110_gym:f110-v0",
-        config={
-            "map": conf.map_path,
-            "map_ext": conf.map_ext,
-            "num_agents": 1,
-            "timestep": 0.01,
-            "integrator": "rk4",
-            "control_input": "speed",
-            "observation_config": {"type": "kinematic_state"},
-            "params": {"mu": 1.0},
-        },
-        render_mode="human",
-    )
     env.add_render_callback(render_callback)
 
     poses = np.array([[0.7, 0.0, 1.37]])
diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
index a8c9bf67..f90377a1 100644
--- a/gym/f110_gym/envs/track.py
+++ b/gym/f110_gym/envs/track.py
@@ -164,14 +164,26 @@ def __init__(
         self.centerline = centerline
         self.raceline = raceline
 
+    @staticmethod
+    def load_spec(track: str, filespec: str):
+        """
+        Load track specification from yaml file.
+
+        Args:
+
+        """
+        with open(filespec, "r") as yaml_stream:
+            map_metadata = yaml.safe_load(yaml_stream)
+            track_spec = TrackSpec(name=track, **map_metadata)
+        return track_spec
+
     @staticmethod
     def from_track_name(track: str):
         try:
             track_dir = find_track_dir(track)
-            # load track spec
-            with open(track_dir / f"{track}_map.yaml", "r") as yaml_stream:
-                map_metadata = yaml.safe_load(yaml_stream)
-                track_spec = TrackSpec(name=track, **map_metadata)
+            track_spec = Track.load_spec(
+                track=track, filespec=str(track_dir / f"{track_dir.stem}_map.yaml")
+            )
 
             # load occupancy grid
             map_filename = pathlib.Path(track_spec.image)
diff --git a/gym/f110_gym/unittest/__init__.py b/gym/f110_gym/unittest/__init__.py
index 5be97ba3..e69de29b 100644
--- a/gym/f110_gym/unittest/__init__.py
+++ b/gym/f110_gym/unittest/__init__.py
@@ -1 +0,0 @@
-from .scan_sim import *
\ No newline at end of file
diff --git a/gym/f110_gym/unittest/observation_test.py b/gym/f110_gym/unittest/observation_test.py
index 07a1aa84..9a7c02cf 100644
--- a/gym/f110_gym/unittest/observation_test.py
+++ b/gym/f110_gym/unittest/observation_test.py
@@ -18,16 +18,8 @@ class TestObservationInterface(unittest.TestCase):
     def _make_env(config={}) -> F110Env:
         import f110_gym
 
-        example_dir = pathlib.Path(__file__).parent.parent.parent.parent / "examples"
-
-        with open(example_dir / "config_example_map.yaml") as file:
-            conf_dict = yaml.load(file, Loader=yaml.FullLoader)
-        conf = Namespace(**conf_dict)
-        conf.map_path = str(example_dir / conf.map_path)
-
         conf = {
-            "map": conf.map_path,
-            "map_ext": conf.map_ext,
+            "map": "Example",
             "num_agents": 1,
             "timestep": 0.01,
             "integrator": "rk4",
diff --git a/gym/f110_gym/unittest/scan_sim.py b/gym/f110_gym/unittest/scan_sim.py
index 9cb89aee..bb1ca344 100644
--- a/gym/f110_gym/unittest/scan_sim.py
+++ b/gym/f110_gym/unittest/scan_sim.py
@@ -21,7 +21,6 @@
 # SOFTWARE.
 
 
-
 """
 Prototype of Utility functions and classes for simulating 2D LIDAR scans
 Author: Hongrui Zheng
@@ -37,6 +36,7 @@
 import unittest
 import timeit
 
+
 def get_dt(bitmap, resolution):
     """
     Distance transformation, returns the distance matrix from the input bitmap.
@@ -52,6 +52,7 @@ def get_dt(bitmap, resolution):
     dt = resolution * edt(bitmap)
     return dt
 
+
 @njit(cache=True)
 def xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution):
     """
@@ -62,7 +63,7 @@ def xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution):
             y (float): coordinate in y (m)
             orig_x (float): x coordinate of the map origin (m)
             orig_y (float): y coordinate of the map origin (m)
-        
+
         Returns:
             r (int): row number in the transform matrix of the given point
             c (int): column number in the transform matrix of the given point
@@ -76,17 +77,25 @@ def xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution):
     y_rot = -x_trans * orig_s + y_trans * orig_c
 
     # clip the state to be a cell
-    if x_rot < 0 or x_rot >= width * resolution or y_rot < 0 or y_rot >= height * resolution:
+    if (
+        x_rot < 0
+        or x_rot >= width * resolution
+        or y_rot < 0
+        or y_rot >= height * resolution
+    ):
         c = -1
         r = -1
     else:
-        c = int(x_rot/resolution)
-        r = int(y_rot/resolution)
+        c = int(x_rot / resolution)
+        r = int(y_rot / resolution)
 
     return r, c
 
+
 @njit(cache=True)
-def distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt):
+def distance_transform(
+    x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt
+):
     """
     Look up corresponding distance in the distance matrix
 
@@ -103,8 +112,25 @@ def distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, reso
     distance = dt[r, c]
     return distance
 
+
 @njit(cache=True)
-def trace_ray(x, y, theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt, max_range):
+def trace_ray(
+    x,
+    y,
+    theta_index,
+    sines,
+    cosines,
+    eps,
+    orig_x,
+    orig_y,
+    orig_c,
+    orig_s,
+    height,
+    width,
+    resolution,
+    dt,
+    max_range,
+):
     """
     Find the length of a specific ray at a specific scan angle theta
     Purely math calculation and loops, should be JITted.
@@ -119,14 +145,16 @@ def trace_ray(x, y, theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, or
         Returns:
             total_distance (float): the distance to first obstacle on the current scan beam
     """
-    
+
     # int casting, and index precal trigs
     theta_index_ = int(theta_index)
     s = sines[theta_index_]
     c = cosines[theta_index_]
 
     # distance to nearest initialization
-    dist_to_nearest = distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt)
+    dist_to_nearest = distance_transform(
+        x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt
+    )
     total_dist = dist_to_nearest
 
     # ray tracing iterations
@@ -137,13 +165,34 @@ def trace_ray(x, y, theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, or
 
         # update dist_to_nearest for current point on ray
         # also keeps track of total ray length
-        dist_to_nearest = distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt)
+        dist_to_nearest = distance_transform(
+            x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt
+        )
         total_dist += dist_to_nearest
-    
+
     return total_dist
 
+
 @njit(cache=True)
-def get_scan(pose, theta_dis, fov, num_beams, theta_index_increment, sines, cosines, eps, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt, max_range):
+def get_scan(
+    pose,
+    theta_dis,
+    fov,
+    num_beams,
+    theta_index_increment,
+    sines,
+    cosines,
+    eps,
+    orig_x,
+    orig_y,
+    orig_c,
+    orig_s,
+    height,
+    width,
+    resolution,
+    dt,
+    max_range,
+):
     """
     Perform the scan for each discretized angle of each beam of the laser, loop heavy, should be JITted
 
@@ -161,17 +210,33 @@ def get_scan(pose, theta_dis, fov, num_beams, theta_index_increment, sines, cosi
     scan = np.empty((num_beams,))
 
     # make theta discrete by mapping the range [-pi, pi] onto [0, theta_dis]
-    theta_index = theta_dis * (pose[2] - fov/2.)/(2. * np.pi)
+    theta_index = theta_dis * (pose[2] - fov / 2.0) / (2.0 * np.pi)
 
     # make sure it's wrapped properly
     theta_index = np.fmod(theta_index, theta_dis)
-    while (theta_index < 0):
+    while theta_index < 0:
         theta_index += theta_dis
 
     # sweep through each beam
     for i in range(0, num_beams):
         # trace the current beam
-        scan[i] = trace_ray(pose[0], pose[1], theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt, max_range)
+        scan[i] = trace_ray(
+            pose[0],
+            pose[1],
+            theta_index,
+            sines,
+            cosines,
+            eps,
+            orig_x,
+            orig_y,
+            orig_c,
+            orig_s,
+            height,
+            width,
+            resolution,
+            dt,
+            max_range,
+        )
 
         # increment the beam index
         theta_index += theta_index_increment
@@ -182,6 +247,7 @@ def get_scan(pose, theta_dis, fov, num_beams, theta_index_increment, sines, cosi
 
     return scan
 
+
 class ScanSimulator2D(object):
     """
     2D LIDAR scan simulator class
@@ -196,8 +262,17 @@ class ScanSimulator2D(object):
         seed (int, default=123): seed for random number generator for the whitenoise in scan
     """
 
-    def __init__(self, num_beams, fov, std_dev=0.01, eps=0.0001, theta_dis=2000, max_range=30.0, seed=123):
-        # initialization 
+    def __init__(
+        self,
+        num_beams,
+        fov,
+        std_dev=0.01,
+        eps=0.0001,
+        theta_dis=2000,
+        max_range=30.0,
+        seed=123,
+    ):
+        # initialization
         self.num_beams = num_beams
         self.fov = fov
         self.std_dev = std_dev
@@ -205,7 +280,7 @@ def __init__(self, num_beams, fov, std_dev=0.01, eps=0.0001, theta_dis=2000, max
         self.theta_dis = theta_dis
         self.max_range = max_range
         self.angle_increment = self.fov / (self.num_beams - 1)
-        self.theta_index_increment = theta_dis * self.angle_increment / (2. * np.pi)
+        self.theta_index_increment = theta_dis * self.angle_increment / (2.0 * np.pi)
         self.orig_c = None
         self.orig_s = None
         self.orig_x = None
@@ -214,15 +289,15 @@ def __init__(self, num_beams, fov, std_dev=0.01, eps=0.0001, theta_dis=2000, max
         self.map_width = None
         self.map_resolution = None
         self.dt = None
-        
+
         # white noise generator
         self.rng = np.random.default_rng(seed=seed)
 
         # precomputing corresponding cosines and sines of the angle array
-        theta_arr = np.linspace(0.0, 2*np.pi, num=theta_dis)
+        theta_arr = np.linspace(0.0, 2 * np.pi, num=theta_dis)
         self.sines = np.sin(theta_arr)
         self.cosines = np.cos(theta_arr)
-    
+
     def set_map(self, map_path, map_ext):
         """
         Set the bitmap of the scan simulator by path
@@ -239,22 +314,24 @@ def set_map(self, map_path, map_ext):
 
         # load map image
         map_img_path = os.path.splitext(map_path)[0] + map_ext
-        self.map_img = np.array(Image.open(map_img_path).transpose(Image.FLIP_TOP_BOTTOM))
+        self.map_img = np.array(
+            Image.open(map_img_path).transpose(Image.FLIP_TOP_BOTTOM)
+        )
         self.map_img = self.map_img.astype(np.float64)
 
         # grayscale -> binary
-        self.map_img[self.map_img <= 128.] = 0.
-        self.map_img[self.map_img > 128.] = 255.
+        self.map_img[self.map_img <= 128.0] = 0.0
+        self.map_img[self.map_img > 128.0] = 255.0
 
         self.map_height = self.map_img.shape[0]
         self.map_width = self.map_img.shape[1]
 
         # load map yaml
-        with open(map_path, 'r') as yaml_stream:
+        with open(map_path, "r") as yaml_stream:
             try:
                 map_metadata = yaml.safe_load(yaml_stream)
-                self.map_resolution = map_metadata['resolution']
-                self.origin = map_metadata['origin']
+                self.map_resolution = map_metadata["resolution"]
+                self.origin = map_metadata["origin"]
             except yaml.YAMLError as ex:
                 print(ex)
 
@@ -283,9 +360,27 @@ def scan(self, pose):
                 ValueError: when scan is called before a map is set
         """
         if self.map_height is None:
-            raise ValueError('Map is not set for scan simulator.')
-        scan = get_scan(pose, self.theta_dis, self.fov, self.num_beams, self.theta_index_increment, self.sines, self.cosines, self.eps, self.orig_x, self.orig_y, self.orig_c, self.orig_s, self.map_height, self.map_width, self.map_resolution, self.dt, self.max_range)
-        noise = self.rng.normal(0., self.std_dev, size=self.num_beams)
+            raise ValueError("Map is not set for scan simulator.")
+        scan = get_scan(
+            pose,
+            self.theta_dis,
+            self.fov,
+            self.num_beams,
+            self.theta_index_increment,
+            self.sines,
+            self.cosines,
+            self.eps,
+            self.orig_x,
+            self.orig_y,
+            self.orig_c,
+            self.orig_s,
+            self.map_height,
+            self.map_width,
+            self.map_resolution,
+            self.dt,
+            self.max_range,
+        )
+        noise = self.rng.normal(0.0, self.std_dev, size=self.num_beams)
         final_scan = scan + noise
         return final_scan
 
@@ -308,123 +403,139 @@ def setUp(self):
         # test params
         self.num_beams = 1080
         self.fov = 4.7
-        
+
         self.num_test = 10
         self.test_poses = np.zeros((self.num_test, 3))
-        self.test_poses[:, 2] = np.linspace(-1., 1., num=self.num_test)
+        self.test_poses[:, 2] = np.linspace(-1.0, 1.0, num=self.num_test)
 
         # legacy gym data
-        sample_scan = np.load('legacy_scan.npz')
-        self.berlin_scan = sample_scan['berlin']
-        self.skirk_scan = sample_scan['skirk']
+        wdir = os.path.dirname(os.path.abspath(__file__))
+        sample_scan = np.load(f"{wdir}/legacy_scan.npz")
+        self.berlin_scan = sample_scan["berlin"]
+        self.skirk_scan = sample_scan["skirk"]
 
     def test_map_berlin(self):
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
         new_berlin = np.empty((self.num_test, self.num_beams))
-        map_path = '../../../maps/Berlin_map.yaml'
-        map_ext = '.png'
+        map_path = (
+            os.path.dirname(os.path.abspath(__file__))
+            + "/../maps/Berlin/Berlin_map.yaml"
+        )
+        map_ext = ".png"
         scan_sim.set_map(map_path, map_ext)
         # scan gen loop
         for i in range(self.num_test):
             test_pose = self.test_poses[i]
-            new_berlin[i,:] = scan_sim.scan(test_pose)
+            new_berlin[i, :] = scan_sim.scan(test_pose)
         diff = self.berlin_scan - new_berlin
         mse = np.mean(diff**2)
         # print('Levine distance test, norm: ' + str(norm))
 
         # plotting
         import matplotlib.pyplot as plt
-        theta = np.linspace(-self.fov/2., self.fov/2., num=self.num_beams)
-        plt.polar(theta, new_berlin[1,:], '.', lw=0)
-        plt.polar(theta, self.berlin_scan[1,:], '.', lw=0)
+
+        theta = np.linspace(-self.fov / 2.0, self.fov / 2.0, num=self.num_beams)
+        plt.polar(theta, new_berlin[1, :], ".", lw=0)
+        plt.polar(theta, self.berlin_scan[1, :], ".", lw=0)
         plt.show()
 
-        self.assertLess(mse, 2.)
+        self.assertLess(mse, 2.0)
 
     def test_map_skirk(self):
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
         new_skirk = np.empty((self.num_test, self.num_beams))
-        map_path = '../../../maps/Skirk_map.yaml'
-        map_ext = '.png'
+        map_path = (
+            os.path.dirname(os.path.abspath(__file__)) + "/../maps/Skirk/Skirk_map.yaml"
+        )
+        map_ext = ".png"
         scan_sim.set_map(map_path, map_ext)
-        print('map set')
+        print("map set")
         # scan gen loop
         for i in range(self.num_test):
             test_pose = self.test_poses[i]
-            new_skirk[i,:] = scan_sim.scan(test_pose)
+            new_skirk[i, :] = scan_sim.scan(test_pose)
         diff = self.skirk_scan - new_skirk
         mse = np.mean(diff**2)
-        print('skirk distance test, mse: ' + str(mse))
+        print("skirk distance test, mse: " + str(mse))
 
         # plotting
         import matplotlib.pyplot as plt
-        theta = np.linspace(-self.fov/2., self.fov/2., num=self.num_beams)
-        plt.polar(theta, new_skirk[1,:], '.', lw=0)
-        plt.polar(theta, self.skirk_scan[1,:], '.', lw=0)
+
+        theta = np.linspace(-self.fov / 2.0, self.fov / 2.0, num=self.num_beams)
+        plt.polar(theta, new_skirk[1, :], ".", lw=0)
+        plt.polar(theta, self.skirk_scan[1, :], ".", lw=0)
         plt.show()
 
-        self.assertLess(mse, 2.)
+        self.assertLess(mse, 2.0)
 
     def test_fps(self):
         # scan fps should be greater than 500
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
-        map_path = '../../../maps/Skirk_map.yaml'
-        map_ext = '.png'
+        map_path = (
+            os.path.dirname(os.path.abspath(__file__)) + "/../maps/Skirk/Skirk_map.yaml"
+        )
+        map_ext = ".png"
         scan_sim.set_map(map_path, map_ext)
-        
+
         import time
+
         start = time.time()
         for i in range(10000):
-            x_test = i/10000
-            scan = scan_sim.scan(np.array([x_test, 0., 0.]))
+            x_test = i / 10000
+            scan = scan_sim.scan(np.array([x_test, 0.0, 0.0]))
         end = time.time()
-        fps = 10000/(end-start)
+        fps = 10000 / (end - start)
         # print('FPS test')
         # print('Elapsed time: ' + str(end-start) + ' , FPS: ' + str(1/fps))
-        self.assertGreater(fps, 500.)
+        self.assertGreater(fps, 500.0)
 
 
 def main():
     num_beams = 1080
     fov = 4.7
     # map_path = '../envs/maps/Berlin_map.yaml'
-    map_path = '/home/f1tenth-eval/tunercar/es/maps/map0.yaml'
-    map_ext = '.png'
+    map_path = "/home/f1tenth-eval/tunercar/es/maps/map0.yaml"
+    map_ext = ".png"
     scan_sim = ScanSimulator2D(num_beams, fov)
     scan_sim.set_map(map_path, map_ext)
-    scan = scan_sim.scan(np.array([0., 0., 0.]))
+    scan = scan_sim.scan(np.array([0.0, 0.0, 0.0]))
 
     # fps test
     import time
+
     start = time.time()
     for i in range(10000):
-        x_test = i/10000
-        scan = scan_sim.scan(np.array([x_test, 0., 0.]))
+        x_test = i / 10000
+        scan = scan_sim.scan(np.array([x_test, 0.0, 0.0]))
     end = time.time()
-    fps = (end-start)/10000
-    print('FPS test')
-    print('Elapsed time: ' + str(end-start) + ' , FPS: ' + str(1/fps))
+    fps = (end - start) / 10000
+    print("FPS test")
+    print("Elapsed time: " + str(end - start) + " , FPS: " + str(1 / fps))
 
     # visualization
     import matplotlib.pyplot as plt
     from matplotlib.animation import FuncAnimation
+
     num_iter = 100
-    theta = np.linspace(-fov/2., fov/2., num=num_beams)
+    theta = np.linspace(-fov / 2.0, fov / 2.0, num=num_beams)
     fig = plt.figure()
-    ax = fig.add_subplot(111, projection='polar')
+    ax = fig.add_subplot(111, projection="polar")
     ax.set_ylim(0, 70)
-    line, = ax.plot([], [], '.', lw=0)
+    (line,) = ax.plot([], [], ".", lw=0)
+
     def update(i):
         # x_ani = i * 3. / num_iter
         theta_ani = -i * 2 * np.pi / num_iter
-        x_ani = 0.
-        current_scan = scan_sim.scan(np.array([x_ani, 0., theta_ani]))
+        x_ani = 0.0
+        current_scan = scan_sim.scan(np.array([x_ani, 0.0, theta_ani]))
         print(np.max(current_scan))
         line.set_data(theta, current_scan)
-        return line, 
+        return (line,)
+
     ani = FuncAnimation(fig, update, frames=num_iter, blit=True)
     plt.show()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     # unittest.main()
-    main()
\ No newline at end of file
+    main()
diff --git a/gym/f110_gym/unittest/track_test.py b/gym/f110_gym/unittest/track_test.py
index 5f395fb0..7feb86c3 100644
--- a/gym/f110_gym/unittest/track_test.py
+++ b/gym/f110_gym/unittest/track_test.py
@@ -1,8 +1,9 @@
+import pathlib
 import unittest
 
 import numpy as np
 
-from f110_gym.envs.track import Track, find_track_dir
+from f110_gym.envs.track import Track, find_track_dir, Raceline
 
 
 class TestTrack(unittest.TestCase):
@@ -42,3 +43,55 @@ def test_missing_raceline(self):
         track = Track.from_track_name("Vegas")
         self.assertEqual(track.raceline, None)
         self.assertEqual(track.centerline, None)
+
+    def test_map_dir_structure(self):
+        """
+        Check that the map dir structure is correct:
+        - maps/
+            - Trackname/
+                - Trackname_map.*               # map image
+                - Trackname_map.yaml            # map specification
+                - [Trackname_raceline.csv]      # raceline (optional)
+                - [Trackname_centerline.csv]    # centerline (optional)
+        """
+        mapdir = pathlib.Path(__file__).parent.parent / "maps"
+        for trackdir in mapdir.iterdir():
+            if trackdir.is_file():
+                continue
+
+            # check subdir is capitalized (at least first letter is capitalized)
+            trackdirname = trackdir.stem
+            self.assertTrue(
+                trackdirname[0].isupper(), f"trackdir {trackdirname} is not capitalized"
+            )
+
+            # check map spec file exists
+            file_spec = trackdir / f"{trackdirname}_map.yaml"
+            self.assertTrue(
+                file_spec.exists(),
+                f"map spec file {file_spec} does not exist in {trackdir}",
+            )
+
+            # read map image file from spec
+            map_spec = Track.load_spec(track=str(trackdir), filespec=str(file_spec))
+            file_image = trackdir / map_spec.image
+
+            # check map image file exists
+            self.assertTrue(
+                file_image.exists(),
+                f"map image file {file_image} does not exist in {trackdir}",
+            )
+
+            # check raceline and centerline files
+            file_raceline = trackdir / f"{trackdir.stem}_raceline.csv"
+            file_centerline = trackdir / f"{trackdir.stem}_centerline.csv"
+
+            if file_raceline.exists():
+                # try to load raceline files
+                # it will raise an assertion error if the file format are not valid
+                raceline = Raceline.from_raceline_file(file_raceline)
+
+            if file_centerline.exists():
+                # try to load raceline files
+                # it will raise an assertion error if the file format are not valid
+                centerline = Raceline.from_raceline_file(file_centerline)

From b03ec4aef1d9c81b5c8fdc8171bda18b857c96f6 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Sun, 9 Jul 2023 17:38:21 +0200
Subject: [PATCH 30/42] merge changes from v.1.0.0, move integrator and dynamic
 model out from base classes, clean dynamic test, adapt f110-env and
 base-classes accordingly, clean imports waypoint_follow.py

---
 examples/waypoint_follow.py            |    1 -
 gym/f110_gym/envs/__init__.py          |    5 +-
 gym/f110_gym/envs/base_classes.py      |  227 +----
 gym/f110_gym/envs/dynamic_models.py    |  431 ++++-----
 gym/f110_gym/envs/f110_env.py          |    8 +-
 gym/f110_gym/envs/integrator.py        |  164 ++++
 gym/f110_gym/unittest/dynamics_test.py | 1175 +++++++++++++++++++-----
 7 files changed, 1314 insertions(+), 697 deletions(-)
 create mode 100644 gym/f110_gym/envs/integrator.py

diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py
index f7d2b296..a857fbae 100644
--- a/examples/waypoint_follow.py
+++ b/examples/waypoint_follow.py
@@ -1,5 +1,4 @@
 import time
-from f110_gym.envs.base_classes import Integrator
 import yaml
 import gymnasium as gym
 import numpy as np
diff --git a/gym/f110_gym/envs/__init__.py b/gym/f110_gym/envs/__init__.py
index 7aa297ae..1cf17c53 100644
--- a/gym/f110_gym/envs/__init__.py
+++ b/gym/f110_gym/envs/__init__.py
@@ -1,5 +1,6 @@
-from f110_gym.envs.f110_env import F110Env
+from f110_gym.envs.integrator import *
 from f110_gym.envs.dynamic_models import *
+from f110_gym.envs.f110_env import F110Env
 from f110_gym.envs.laser_models import *
 from f110_gym.envs.base_classes import *
-from f110_gym.envs.collision_models import *
\ No newline at end of file
+from f110_gym.envs.collision_models import *
diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index f00c6e23..ed0b230f 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -26,34 +26,15 @@
 Replacement of the old RaceCar, Simulator classes in C++
 Author: Hongrui Zheng
 """
-from enum import Enum
-import warnings
-
 import numpy as np
 
+from f110_gym.envs import DynamicModel
 from f110_gym.envs.action import SpeedAction
-from f110_gym.envs.dynamic_models import vehicle_dynamics_st, vehicle_dynamics_ks, pid
+from f110_gym.envs.integrator import EulerIntegrator, IntegratorType
 from f110_gym.envs.laser_models import ScanSimulator2D, check_ttc_jit, ray_cast
 from f110_gym.envs.collision_models import get_vertices, collision_multiple
 
 
-class Integrator(Enum):
-    RK4 = 1
-    Euler = 2
-
-    @staticmethod
-    def from_string(integrator: str):
-        if integrator == "rk4":
-            return Integrator.RK4
-        elif integrator == "euler":
-            return Integrator.Euler
-        else:
-            raise ValueError(f"Unknown integrator type {integrator}")
-
-class Model(Enum):
-    KS = 1      # Kinematic Single Track
-    ST = 2      # Single Track
-
 class RaceCar(object):
     """
     Base level race car class, handles the physics and laser scan of a single vehicle
@@ -69,7 +50,6 @@ class RaceCar(object):
         accel (float): current acceleration input
         steer_angle_vel (float): current steering velocity input
         in_collision (bool): collision indicator
-
     """
 
     # static objects that don't need to be stored in class instances
@@ -86,8 +66,8 @@ def __init__(
         time_step=0.01,
         num_beams=1080,
         fov=4.7,
-        integrator=Integrator.Euler,
-        model=Model.ST,
+        integrator=EulerIntegrator(),
+        model=DynamicModel.ST,
         action_type=SpeedAction(),
     ):
         """
@@ -96,11 +76,15 @@ def __init__(
         Init function
 
         Args:
-            params (dict): vehicle parameter dictionary, includes {'mu', 'C_Sf', 'C_Sr', 'lf', 'lr', 'h', 'm', 'I', 's_min', 's_max', 'sv_min', 'sv_max', 'v_switch', 'a_max': 9.51, 'v_min', 'v_max', 'length', 'width'}
+            params (dict): vehicle parameters dictionary
+            seed (int): random seed
             is_ego (bool, default=False): ego identifier
             time_step (float, default=0.01): physics sim time step
             num_beams (int, default=1080): number of beams in the laser scan
             fov (float, default=4.7): field of view of the laser
+            integrator (Integrator, default=EulerIntegrator()): integrator type
+            model (Model, default=Model.ST): vehicle model type
+            action_type (Action, default=SpeedAction()): action type
 
         Returns:
             None
@@ -114,24 +98,11 @@ def __init__(
         self.num_beams = num_beams
         self.fov = fov
         self.integrator = integrator
-        if self.integrator is Integrator.RK4:
-            warnings.warn(
-                f"Chosen integrator is RK4. This is different from previous versions of the gym."
-            )
         self.action_type = action_type
-
         self.model = model
-        if self.model is not Model.ST:
-            warnings.warn(f"Chosen model is not ST. This is different from previous versions of the gym.")
-
-        if self.model is Model.ST:
-            # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
-            self.state = np.zeros((7, ))
-        elif self.model is Model.KS:
-            # state is [x, y, steer_angle, vel, yaw_angle]
-            self.state = np.zeros((5, ))
-        else:
-            raise NotImplementedError(f"Model {self.model} is not implemented.")
+
+        # state of the vehicle
+        self.state = self.model.get_initial_state()
 
         # pose of opponents in the world
         self.opp_poses = None
@@ -231,16 +202,9 @@ def reset(self, pose):
         self.steer_angle_vel = 0.0
         # clear collision indicator
         self.in_collision = False
-        # clear state
-        if self.model is Model.ST:
-            self.state = np.zeros((7, ))
-        elif self.model is Model.KS:
-            self.state = np.zeros((5, ))
-        else:
-            raise NotImplementedError(f"Model {self.model} is not implemented.")
+        # init state from pose
+        self.state = self.model.get_initial_state(pose=pose)
 
-        self.state[0:2] = pose[0:2]
-        self.state[4] = pose[2]
         self.steer_buffer = np.empty((0,))
         # reset scan random generator
         self.scan_rng = np.random.default_rng(seed=self.seed)
@@ -321,8 +285,6 @@ def update_pose(self, raw_steer, vel):
             current_scan
         """
 
-        # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
-
         # steering delay
         steer = 0.0
         if self.steer_buffer.shape[0] < self.steer_buffer_size:
@@ -336,139 +298,15 @@ def update_pose(self, raw_steer, vel):
         if self.action_type.type is None:
             raise ValueError("No Control Action Type Specified.")
 
-        accl, sv = self.action_type.act(action=(vel, steer), state=self.state, params=self.params)
-
-        if self.model is Model.KS:
-            f_dynamics = vehicle_dynamics_ks
-        elif self.model is Model.ST:
-            f_dynamics = vehicle_dynamics_st
-        else:
-            raise ValueError('Invalid vehicle model')
-
-        if self.integrator is Integrator.RK4:
-            # RK4 integration
-            k1 = f_dynamics(
-                self.state,
-                np.array([sv, accl]),
-                self.params["mu"],
-                self.params["C_Sf"],
-                self.params["C_Sr"],
-                self.params["lf"],
-                self.params["lr"],
-                self.params["h"],
-                self.params["m"],
-                self.params["I"],
-                self.params["s_min"],
-                self.params["s_max"],
-                self.params["sv_min"],
-                self.params["sv_max"],
-                self.params["v_switch"],
-                self.params["a_max"],
-                self.params["v_min"],
-                self.params["v_max"],
-            )
-
-            k2_state = self.state + self.time_step * (k1 / 2)
-
-            k2 = f_dynamics(
-                k2_state,
-                np.array([sv, accl]),
-                self.params["mu"],
-                self.params["C_Sf"],
-                self.params["C_Sr"],
-                self.params["lf"],
-                self.params["lr"],
-                self.params["h"],
-                self.params["m"],
-                self.params["I"],
-                self.params["s_min"],
-                self.params["s_max"],
-                self.params["sv_min"],
-                self.params["sv_max"],
-                self.params["v_switch"],
-                self.params["a_max"],
-                self.params["v_min"],
-                self.params["v_max"],
-            )
-
-            k3_state = self.state + self.time_step * (k2 / 2)
-
-            k3 = f_dynamics(
-                k3_state,
-                np.array([sv, accl]),
-                self.params["mu"],
-                self.params["C_Sf"],
-                self.params["C_Sr"],
-                self.params["lf"],
-                self.params["lr"],
-                self.params["h"],
-                self.params["m"],
-                self.params["I"],
-                self.params["s_min"],
-                self.params["s_max"],
-                self.params["sv_min"],
-                self.params["sv_max"],
-                self.params["v_switch"],
-                self.params["a_max"],
-                self.params["v_min"],
-                self.params["v_max"],
-            )
-
-            k4_state = self.state + self.time_step * k3
-
-            k4 = f_dynamics(
-                k4_state,
-                np.array([sv, accl]),
-                self.params["mu"],
-                self.params["C_Sf"],
-                self.params["C_Sr"],
-                self.params["lf"],
-                self.params["lr"],
-                self.params["h"],
-                self.params["m"],
-                self.params["I"],
-                self.params["s_min"],
-                self.params["s_max"],
-                self.params["sv_min"],
-                self.params["sv_max"],
-                self.params["v_switch"],
-                self.params["a_max"],
-                self.params["v_min"],
-                self.params["v_max"],
-            )
-
-            # dynamics integration
-            self.state = self.state + self.time_step * (1 / 6) * (
-                k1 + 2 * k2 + 2 * k3 + k4
-            )
-
-        elif self.integrator is Integrator.Euler:
-            f = f_dynamics(
-                self.state,
-                np.array([sv, accl]),
-                self.params["mu"],
-                self.params["C_Sf"],
-                self.params["C_Sr"],
-                self.params["lf"],
-                self.params["lr"],
-                self.params["h"],
-                self.params["m"],
-                self.params["I"],
-                self.params["s_min"],
-                self.params["s_max"],
-                self.params["sv_min"],
-                self.params["sv_max"],
-                self.params["v_switch"],
-                self.params["a_max"],
-                self.params["v_min"],
-                self.params["v_max"],
-            )
-            self.state = self.state + self.time_step * f
+        accl, sv = self.action_type.act(
+            action=(vel, steer), state=self.state, params=self.params
+        )
+        u_np = np.array([sv, accl])
 
-        else:
-            raise SyntaxError(
-                f"Invalid Integrator Specified. Provided {self.integrator.name}. Please choose RK4 or Euler"
-            )
+        f_dynamics = self.model.f_dynamics
+        self.state = self.integrator.integrate(
+            f=f_dynamics, x=self.state, u=u_np, dt=self.time_step, params=self.params
+        )
 
         # bound yaw angle
         if self.state[4] > 2 * np.pi:
@@ -521,7 +359,7 @@ def update_scan(self, agent_scans, agent_index):
 
 class Simulator(object):
     """
-    Simulator class, handles the interaction and update o0000000f all vehicles in the environment
+    Simulator class, handles the interaction and update of all vehicles in the environment
 
     TODO check description
 
@@ -534,6 +372,7 @@ class Simulator(object):
         collision_idx (np.ndarray(num_agents, )): which agent is each agent in collision with
         integrator (Integrator): integrator to use for vehicle dynamics
         model (Model): model to use for vehicle dynamics
+        action_type (Action): action type to use for vehicle dynamics
     """
 
     def __init__(
@@ -543,8 +382,8 @@ def __init__(
         seed,
         time_step=0.01,
         ego_idx=0,
-        integrator=Integrator.RK4,
-        model=Model.ST,
+        integrator=IntegratorType.RK4,
+        model=DynamicModel.ST,
         action_type=SpeedAction(),
     ):
         """
@@ -557,8 +396,8 @@ def __init__(
             time_step (float, default=0.01): physics time step
             ego_idx (int, default=0): ego vehicle's index in list of agents
             integrator (Integrator, default=Integrator.RK4): integrator to use for vehicle dynamics
-            model TODO
             model (Model, default=Model.ST): vehicle dynamics model to use
+            action_type (Action, default=SpeedAction()): action type to use for controlling the vehicle
         Returns:
             None
         """
@@ -578,7 +417,7 @@ def __init__(
             car = RaceCar(
                 params,
                 self.seed,
-                is_ego=bool(i==ego_idx),
+                is_ego=bool(i == ego_idx),
                 time_step=self.time_step,
                 integrator=integrator,
                 model=model,
@@ -686,16 +525,6 @@ def step(self, control_inputs):
             if agent.in_collision:
                 self.collisions[i] = 1.0
 
-            # todo: check observation manages ang_vel_z
-            """
-            if self.model == Model.ST:
-                observations['ang_vels_z'].append(agent.state[5])
-            elif self.model == Model.KS:
-                observations['ang_vels_z'].append(0.)
-            else:
-                raise NotImplementedError(f"Model {self.model} not implemented")
-            """
-
     def reset(self, poses):
         """
         Resets the simulation environment by given poses
diff --git a/gym/f110_gym/envs/dynamic_models.py b/gym/f110_gym/envs/dynamic_models.py
index ec18e909..f2d244cf 100644
--- a/gym/f110_gym/envs/dynamic_models.py
+++ b/gym/f110_gym/envs/dynamic_models.py
@@ -11,7 +11,6 @@
 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
-
 """
 Prototype of vehicle dynamics functions and classes for simulating 2D Single
 Track dynamic model
@@ -19,12 +18,56 @@
 Original implementation: https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/
 Author: Hongrui Zheng
 """
+import warnings
+from enum import Enum
 
 import numpy as np
 from numba import njit
 
-import unittest
-import time
+
+class DynamicModel(Enum):
+    KS = 1  # Kinematic Single Track
+    ST = 2  # Single Track
+
+    @staticmethod
+    def from_string(model: str):
+        if model == "ks":
+            warnings.warn(
+                f"Chosen model is KS. This is different from previous versions of the gym."
+            )
+            return DynamicModel.KS
+        elif model == "st":
+            return DynamicModel.ST
+        else:
+            raise ValueError(f"Unknown model type {model}")
+
+    def get_initial_state(self, pose=None):
+        # initialize zero state
+        if self == DynamicModel.KS:
+            # state is [x, y, steer_angle, vel, yaw_angle]
+            state = np.zeros(5)
+        elif self == DynamicModel.ST:
+            # state is [x, y, steer_angle, vel, yaw_angle, yaw_rate, slip_angle]
+            state = np.zeros(7)
+        else:
+            raise ValueError(f"Unknown model type {self}")
+
+        # set initial pose if provided
+        if pose is not None:
+            state[0:2] = pose[0:2]
+            state[4] = pose[2]
+
+        return state
+
+    @property
+    def f_dynamics(self):
+        if self == DynamicModel.KS:
+            return vehicle_dynamics_ks
+        elif self == DynamicModel.ST:
+            return vehicle_dynamics_st
+        else:
+            raise ValueError(f"Unknown model type {self}")
+
 
 @njit(cache=True)
 def accl_constraints(vel, accl, v_switch, a_max, v_min, v_max):
@@ -45,13 +88,13 @@ def accl_constraints(vel, accl, v_switch, a_max, v_min, v_max):
 
     # positive accl limit
     if vel > v_switch:
-        pos_limit = a_max*v_switch/vel
+        pos_limit = a_max * v_switch / vel
     else:
         pos_limit = a_max
 
     # accl limit reached?
     if (vel <= v_min and accl <= 0) or (vel >= v_max and accl >= 0):
-        accl = 0.
+        accl = 0.0
     elif accl <= -a_max:
         accl = -a_max
     elif accl >= pos_limit:
@@ -59,8 +102,11 @@ def accl_constraints(vel, accl, v_switch, a_max, v_min, v_max):
 
     return accl
 
+
 @njit(cache=True)
-def steering_constraint(steering_angle, steering_velocity, s_min, s_max, sv_min, sv_max):
+def steering_constraint(
+    steering_angle, steering_velocity, s_min, s_max, sv_min, sv_max
+):
     """
     Steering constraints, adjusts the steering velocity based on constraints
 
@@ -77,8 +123,10 @@ def steering_constraint(steering_angle, steering_velocity, s_min, s_max, sv_min,
     """
 
     # constraint steering velocity
-    if (steering_angle <= s_min and steering_velocity <= 0) or (steering_angle >= s_max and steering_velocity >= 0):
-        steering_velocity = 0.
+    if (steering_angle <= s_min and steering_velocity <= 0) or (
+        steering_angle >= s_max and steering_velocity >= 0
+    ):
+        steering_velocity = 0.0
     elif steering_velocity <= sv_min:
         steering_velocity = sv_min
     elif steering_velocity >= sv_max:
@@ -88,7 +136,26 @@ def steering_constraint(steering_angle, steering_velocity, s_min, s_max, sv_min,
 
 
 @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,
+    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.
 
@@ -110,18 +177,47 @@ def vehicle_dynamics_ks(x, u_init, mu, C_Sf, C_Sr, lf, lr, h, m, I, s_min, s_max
     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)])
+    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])])
+    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_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,
+    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 Dynamic Vehicle Dynamics.
 
@@ -146,7 +242,12 @@ def vehicle_dynamics_st(x, u_init, mu, C_Sf, C_Sr, lf, lr, h, m, I, s_min, s_max
     g = 9.81
 
     # 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)])
+    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),
+        ]
+    )
 
     # switch to kinematic model for small velocities
     if abs(x[3]) < 0.5:
@@ -155,26 +256,83 @@ def vehicle_dynamics_st(x, u_init, mu, C_Sf, C_Sr, lf, lr, h, m, I, s_min, s_max
 
         # system dynamics
         x_ks = x[0:5]
-        f_ks = vehicle_dynamics_ks(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)
-        f = np.hstack((f_ks, np.array([u[1]/lwb*np.tan(x[2])+x[3]/(lwb*np.cos(x[2])**2)*u[0],
-        0])))
+        f_ks = vehicle_dynamics_ks(
+            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,
+        )
+        f = np.hstack(
+            (
+                f_ks,
+                np.array(
+                    [
+                        u[1] / lwb * np.tan(x[2])
+                        + x[3] / (lwb * np.cos(x[2]) ** 2) * u[0],
+                        0,
+                    ]
+                ),
+            )
+        )
 
     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))*(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)-1)*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]])
+        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))
+                * (
+                    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
+                    )
+                    - 1
+                )
+                * 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],
+            ]
+        )
 
     return f
 
+
 @njit(cache=True)
 def pid(speed, steer, current_speed, current_steer, max_sv, max_a, max_v, min_v):
     """
@@ -198,8 +356,8 @@ def pid(speed, steer, current_speed, current_steer, max_sv, max_a, max_v, min_v)
     # accl
     vel_diff = speed - current_speed
     # currently forward
-    if current_speed > 0.:
-        if (vel_diff > 0):
+    if current_speed > 0.0:
+        if vel_diff > 0:
             # accelerate
             kp = 10.0 * max_a / max_v
             accl = kp * vel_diff
@@ -209,7 +367,7 @@ def pid(speed, steer, current_speed, current_steer, max_sv, max_a, max_v, min_v)
             accl = kp * vel_diff
     # currently backwards
     else:
-        if (vel_diff > 0):
+        if vel_diff > 0:
             # braking
             kp = 2.0 * max_a / max_v
             accl = kp * vel_diff
@@ -219,208 +377,3 @@ def pid(speed, steer, current_speed, current_steer, max_sv, max_a, max_v, min_v)
             accl = kp * vel_diff
 
     return accl, sv
-
-def func_KS(x, t, 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):
-    f = vehicle_dynamics_ks(x, 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)
-    return f
-
-def func_ST(x, t, 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):
-    f = vehicle_dynamics_st(x, 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)
-    return f
-
-class DynamicsTest(unittest.TestCase):
-    def setUp(self):
-        # test params
-        self.mu = 1.0489
-        self.C_Sf = 21.92/1.0489
-        self.C_Sr = 21.92/1.0489
-        self.lf = 0.3048*3.793293
-        self.lr = 0.3048*4.667707
-        self.h = 0.3048*2.01355
-        self.m = 4.4482216152605/0.3048*74.91452
-        self.I = 4.4482216152605*0.3048*1321.416
-
-        #steering constraints
-        self.s_min = -1.066  #minimum steering angle [rad]
-        self.s_max = 1.066  #maximum steering angle [rad]
-        self.sv_min = -0.4  #minimum steering velocity [rad/s]
-        self.sv_max = 0.4  #maximum steering velocity [rad/s]
-
-        #longitudinal constraints
-        self.v_min = -13.6  #minimum velocity [m/s]
-        self.v_max = 50.8  #minimum velocity [m/s]
-        self.v_switch = 7.319  #switching velocity [m/s]
-        self.a_max = 11.5  #maximum absolute acceleration [m/s^2]
-
-    def test_derivatives(self):
-        # ground truth derivatives
-        f_ks_gt = [16.3475935934250209, 0.4819314886013121, 0.1500000000000000, 5.1464424102339752, 0.2401426578627629]
-        f_st_gt = [15.7213512030862397, 0.0925527979719355, 0.1500000000000000, 5.3536773276413925, 0.0529001056654038, 0.6435589397748606, 0.0313297971641291]
-
-        # system dynamics
-        g = 9.81
-        x_ks = np.array([3.9579422297936526, 0.0391650102771405, 0.0378491427211811, 16.3546957860883566, 0.0294717351052816])
-        x_st = np.array([2.0233348142065677, 0.0041907137716636, 0.0197545248559617, 15.7216236334290116, 0.0025857914776859, 0.0529001056654038, 0.0033012170610298])
-        v_delta = 0.15
-        acc = 0.63*g
-        u = np.array([v_delta,  acc])
-
-        f_ks = vehicle_dynamics_ks(x_ks, u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max)
-        f_st = vehicle_dynamics_st(x_st, u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max)
-
-        start = time.time()
-        for i in range(10000):
-            f_st = vehicle_dynamics_st(x_st, u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max)
-        duration = time.time() - start
-        avg_fps = 10000/duration
-
-        self.assertAlmostEqual(np.max(np.abs(f_ks_gt-f_ks)), 0.)
-        self.assertAlmostEqual(np.max(np.abs(f_st_gt-f_st)), 0.)
-        self.assertGreater(avg_fps, 5000)
-
-    def test_zeroinit_roll(self):
-        from scipy.integrate import odeint
-
-        # testing for zero initial state, zero input singularities
-        g = 9.81
-        t_start = 0.
-        t_final = 1.
-        delta0 = 0.
-        vel0 = 0.
-        Psi0 = 0.
-        dotPsi0 = 0.
-        beta0 = 0.
-        sy0 = 0.
-        initial_state = [0,sy0,delta0,vel0,Psi0,dotPsi0,beta0]
-
-        x0_KS = np.array(initial_state[0:5])
-        x0_ST = np.array(initial_state)
-
-        # time vector
-        t = np.arange(t_start, t_final, 1e-4)
-
-        # set input: rolling car (velocity should stay constant)
-        u = np.array([0., 0.])
-
-        # simulate single-track model
-        x_roll_st = odeint(func_ST, x0_ST, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
-        # simulate kinematic single-track model
-        x_roll_ks = odeint(func_KS, x0_KS, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
-
-        self.assertTrue(all(x_roll_st[-1]==x0_ST))
-        self.assertTrue(all(x_roll_ks[-1]==x0_KS))
-
-    def test_zeroinit_dec(self):
-        from scipy.integrate import odeint
-
-        # testing for zero initial state, decelerating input singularities
-        g = 9.81
-        t_start = 0.
-        t_final = 1.
-        delta0 = 0.
-        vel0 = 0.
-        Psi0 = 0.
-        dotPsi0 = 0.
-        beta0 = 0.
-        sy0 = 0.
-        initial_state = [0,sy0,delta0,vel0,Psi0,dotPsi0,beta0]
-
-        x0_KS = np.array(initial_state[0:5])
-        x0_ST = np.array(initial_state)
-
-        # time vector
-        t = np.arange(t_start, t_final, 1e-4)
-
-        # set decel input
-        u = np.array([0., -0.7*g])
-
-        # simulate single-track model
-        x_dec_st = odeint(func_ST, x0_ST, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
-        # simulate kinematic single-track model
-        x_dec_ks = odeint(func_KS, x0_KS, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
-
-        # ground truth for single-track model
-        x_dec_st_gt = [-3.4335000000000013, 0.0000000000000000, 0.0000000000000000, -6.8670000000000018, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000]
-        # ground truth for kinematic single-track model
-        x_dec_ks_gt = [-3.4335000000000013, 0.0000000000000000, 0.0000000000000000, -6.8670000000000018, 0.0000000000000000]
-
-        self.assertTrue(all(abs(x_dec_st[-1] - x_dec_st_gt) < 1e-2))
-        self.assertTrue(all(abs(x_dec_ks[-1] - x_dec_ks_gt) < 1e-2))
-
-    def test_zeroinit_acc(self):
-        from scipy.integrate import odeint
-
-        # testing for zero initial state, accelerating with left steer input singularities
-        # wheel spin and velocity should increase more wheel spin at rear
-        g = 9.81
-        t_start = 0.
-        t_final = 1.
-        delta0 = 0.
-        vel0 = 0.
-        Psi0 = 0.
-        dotPsi0 = 0.
-        beta0 = 0.
-        sy0 = 0.
-        initial_state = [0,sy0,delta0,vel0,Psi0,dotPsi0,beta0]
-
-        x0_KS = np.array(initial_state[0:5])
-        x0_ST = np.array(initial_state)
-
-        # time vector
-        t = np.arange(t_start, t_final, 1e-4)
-
-        # set decel input
-        u = np.array([0.15, 0.63*g])
-
-        # simulate single-track model
-        x_acc_st = odeint(func_ST, x0_ST, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
-        # simulate kinematic single-track model
-        x_acc_ks = odeint(func_KS, x0_KS, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
-
-        # ground truth for single-track model
-        x_acc_st_gt = [3.0731976046859715, 0.2869835398304389, 0.1500000000000000, 6.1802999999999999, 0.1097747074946325, 0.3248268063223301, 0.0697547542798040]
-        # ground truth for kinematic single-track model
-        x_acc_ks_gt = [3.0845676868494927, 0.1484249221523042, 0.1500000000000000, 6.1803000000000017, 0.1203664469224163]
-
-        self.assertTrue(all(abs(x_acc_st[-1] - x_acc_st_gt) < 1e-2))
-        self.assertTrue(all(abs(x_acc_ks[-1] - x_acc_ks_gt) < 1e-2))
-
-    def test_zeroinit_rollleft(self):
-        from scipy.integrate import odeint
-
-        # testing for zero initial state, rolling and steering left input singularities
-        g = 9.81
-        t_start = 0.
-        t_final = 1.
-        delta0 = 0.
-        vel0 = 0.
-        Psi0 = 0.
-        dotPsi0 = 0.
-        beta0 = 0.
-        sy0 = 0.
-        initial_state = [0,sy0,delta0,vel0,Psi0,dotPsi0,beta0]
-
-        x0_KS = np.array(initial_state[0:5])
-        x0_ST = np.array(initial_state)
-
-        # time vector
-        t = np.arange(t_start, t_final, 1e-4)
-
-        # set decel input
-        u = np.array([0.15, 0.])
-
-        # simulate single-track model
-        x_left_st = odeint(func_ST, x0_ST, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
-        # simulate kinematic single-track model
-        x_left_ks = odeint(func_KS, x0_KS, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
-
-        # ground truth for single-track model
-        x_left_st_gt = [0.0000000000000000, 0.0000000000000000, 0.1500000000000000, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000]
-        # ground truth for kinematic single-track model
-        x_left_ks_gt = [0.0000000000000000, 0.0000000000000000, 0.1500000000000000, 0.0000000000000000, 0.0000000000000000]
-
-        self.assertTrue(all(abs(x_left_st[-1] - x_left_st_gt) < 1e-2))
-        self.assertTrue(all(abs(x_left_ks[-1] - x_left_ks_gt) < 1e-2))
-
-if __name__ == '__main__':
-    unittest.main()
\ No newline at end of file
diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index cbf7fdf4..e8c0663a 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -27,10 +27,11 @@
 # gym imports
 import gymnasium as gym
 
+from f110_gym.envs import IntegratorType
 from f110_gym.envs.action import CarActionEnum
 
 # base classes
-from f110_gym.envs.base_classes import Simulator, Integrator, Model
+from f110_gym.envs.base_classes import Simulator, DynamicModel
 from f110_gym.envs.observation import observation_factory
 
 from f110_gym.envs.utils import deep_update
@@ -118,8 +119,8 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
         self.num_agents = self.config["num_agents"]
         self.timestep = self.config["timestep"]
         self.ego_idx = self.config["ego_idx"]
-        self.integrator = Integrator.from_string(self.config["integrator"])
-        self.model = Model.ST   # todo add from_string
+        self.integrator = IntegratorType.from_string(self.config["integrator"])
+        self.model = DynamicModel.from_string(self.config["model"])
         self.action_type = CarActionEnum.from_string(self.config["control_input"])
         self.observation_config = self.config["observation_config"]
 
@@ -243,6 +244,7 @@ def default_config(cls) -> dict:
             "timestep": 0.01,
             "ego_idx": 0,
             "integrator": "rk4",
+            "model": "st",
             "control_input": "speed",
             "observation_config": {"type": "original"},
         }
diff --git a/gym/f110_gym/envs/integrator.py b/gym/f110_gym/envs/integrator.py
new file mode 100644
index 00000000..ec048807
--- /dev/null
+++ b/gym/f110_gym/envs/integrator.py
@@ -0,0 +1,164 @@
+import warnings
+from abc import abstractmethod
+from enum import Enum
+
+
+class IntegratorType(Enum):
+    RK4 = 1
+    Euler = 2
+
+    @staticmethod
+    def from_string(integrator: str):
+        if integrator == "rk4":
+            warnings.warn(
+                f"Chosen integrator is RK4. This is different from previous versions of the gym."
+            )
+            return RK4Integrator()
+        elif integrator == "euler":
+            return EulerIntegrator()
+        else:
+            raise ValueError(f"Unknown integrator type {integrator}")
+
+
+class Integrator:
+    def __init__(self) -> None:
+        self._integrator_type = None
+
+    @abstractmethod
+    def integrate(self, f, x, u, dt, params):
+        raise NotImplementedError("integrate method not implemented")
+
+    @property
+    def type(self) -> str:
+        return self._integrator_type
+
+
+class RK4Integrator(Integrator):
+    def __init__(self) -> None:
+        super().__init__()
+        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"],
+        )
+
+        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"],
+        )
+
+        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"],
+        )
+
+        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"],
+        )
+
+        # dynamics integration
+        x = x + dt * (1 / 6) * (k1 + 2 * k2 + 2 * k3 + k4)
+        return x
+
+
+class EulerIntegrator(Integrator):
+    def __init__(self) -> None:
+        super().__init__()
+        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"],
+        )
+        x = x + dt * dstate
+        return x
diff --git a/gym/f110_gym/unittest/dynamics_test.py b/gym/f110_gym/unittest/dynamics_test.py
index 8eb16828..e2ffa72f 100644
--- a/gym/f110_gym/unittest/dynamics_test.py
+++ b/gym/f110_gym/unittest/dynamics_test.py
@@ -11,7 +11,6 @@
 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
-
 """
 Prototype of vehicle dynamics functions and classes for simulating 2D Single 
 Track dynamic model
@@ -26,226 +25,669 @@
 import unittest
 import time
 
-@njit(cache=True)
-def accl_constraints(vel, accl, 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
-            v_switch (float): switching velocity (velocity at 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:
-            accl (float): adjusted acceleration
-    """
-
-    # positive accl limit
-    if vel > v_switch:
-        pos_limit = a_max*v_switch/vel
-    else:
-        pos_limit = a_max
-
-    # accl limit reached?
-    if (vel <= v_min and accl <= 0) or (vel >= v_max and accl >= 0):
-        accl = 0.
-    elif accl <= -a_max:
-        accl = -a_max
-    elif accl >= pos_limit:
-        accl = pos_limit
-
-    return accl
-
-@njit(cache=True)
-def steering_constraint(steering_angle, steering_velocity, s_min, s_max, sv_min, sv_max):
-    """
-    Steering constraints, adjusts the steering velocity based on constraints
-
-        Args:
-            steering_angle (float): current steering_angle of the vehicle
-            steering_velocity (float): unconstraint desired steering_velocity
-            s_min (float): minimum steering angle
-            s_max (float): maximum steering angle
-            sv_min (float): minimum steering velocity
-            sv_max (float): maximum steering velocity
-
-        Returns:
-            steering_velocity (float): adjusted steering velocity
-    """
-
-    # constraint steering velocity
-    if (steering_angle <= s_min and steering_velocity <= 0) or (steering_angle >= s_max and steering_velocity >= 0):
-        steering_velocity = 0.
-    elif steering_velocity <= sv_min:
-        steering_velocity = sv_min
-    elif steering_velocity >= sv_max:
-        steering_velocity = sv_max
-
-    return steering_velocity
-
-
-@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):
-    """
-    Single Track Kinematic Vehicle Dynamics.
-
-        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])])
+from f110_gym.envs import vehicle_dynamics_ks, vehicle_dynamics_st
+
+
+def func_KS(x, t, 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):
+    f = vehicle_dynamics_ks(x, 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)
     return f
 
-@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):
-    """
-    Single Track Dynamic Vehicle Dynamics.
-
-        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
-            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
-    """
-
-    # gravity constant m/s^2
-    g = 9.81
-
-    # 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)])
-
-    # switch to kinematic model for small velocities
-    if abs(x[3]) < 0.1:
-        # wheelbase
-        lwb = lf + lr
 
-        # system dynamics
-        x_ks = x[0:5]
-        f_ks = vehicle_dynamics_ks(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)
-        f = np.hstack((f_ks, np.array([u[1]/lwb*np.tan(x[2])+x[3]/(lwb*np.cos(x[2])**2)*u[0], 
-        0])))
+def func_ST(x, t, 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):
+    f = vehicle_dynamics_st(x, 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)
+    return f
+
+class DynamicsTest(unittest.TestCase):
+    def setUp(self):
+        # test params
+        self.mu = 1.0489
+        self.C_Sf = 21.92 / 1.0489
+        self.C_Sr = 21.92 / 1.0489
+        self.lf = 0.3048 * 3.793293
+        self.lr = 0.3048 * 4.667707
+        self.h = 0.3048 * 2.01355
+        self.m = 4.4482216152605 / 0.3048 * 74.91452
+        self.I = 4.4482216152605 * 0.3048 * 1321.416
+
+        # steering constraints
+        self.s_min = -1.066  # minimum steering angle [rad]
+        self.s_max = 1.066  # maximum steering angle [rad]
+        self.sv_min = -0.4  # minimum steering velocity [rad/s]
+        self.sv_max = 0.4  # maximum steering velocity [rad/s]
+
+        # longitudinal constraints
+        self.v_min = -13.6  # minimum velocity [m/s]
+        self.v_max = 50.8  # minimum velocity [m/s]
+        self.v_switch = 7.319  # switching velocity [m/s]
+        self.a_max = 11.5  # maximum absolute acceleration [m/s^2]
+
+    def test_derivatives(self):
+        # ground truth derivatives
+        f_ks_gt = [
+            16.3475935934250209,
+            0.4819314886013121,
+            0.1500000000000000,
+            5.1464424102339752,
+            0.2401426578627629,
+        ]
+        f_st_gt = [
+            15.7213512030862397,
+            0.0925527979719355,
+            0.1500000000000000,
+            5.3536773276413925,
+            0.0529001056654038,
+            0.6435589397748606,
+            0.0313297971641291,
+        ]
 
-    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))*(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)-1)*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]])
+        g = 9.81
+        x_ks = np.array(
+            [
+                3.9579422297936526,
+                0.0391650102771405,
+                0.0378491427211811,
+                16.3546957860883566,
+                0.0294717351052816,
+            ]
+        )
+        x_st = np.array(
+            [
+                2.0233348142065677,
+                0.0041907137716636,
+                0.0197545248559617,
+                15.7216236334290116,
+                0.0025857914776859,
+                0.0529001056654038,
+                0.0033012170610298,
+            ]
+        )
+        v_delta = 0.15
+        acc = 0.63 * g
+        u = np.array([v_delta, acc])
+
+        f_ks = vehicle_dynamics_ks(
+            x_ks,
+            u,
+            self.mu,
+            self.C_Sf,
+            self.C_Sr,
+            self.lf,
+            self.lr,
+            self.h,
+            self.m,
+            self.I,
+            self.s_min,
+            self.s_max,
+            self.sv_min,
+            self.sv_max,
+            self.v_switch,
+            self.a_max,
+            self.v_min,
+            self.v_max,
+        )
+        f_st = vehicle_dynamics_st(
+            x_st,
+            u,
+            self.mu,
+            self.C_Sf,
+            self.C_Sr,
+            self.lf,
+            self.lr,
+            self.h,
+            self.m,
+            self.I,
+            self.s_min,
+            self.s_max,
+            self.sv_min,
+            self.sv_max,
+            self.v_switch,
+            self.a_max,
+            self.v_min,
+            self.v_max,
+        )
 
-    return f
+        start = time.time()
+        for i in range(10000):
+            f_st = vehicle_dynamics_st(
+                x_st,
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            )
+        duration = time.time() - start
+        avg_fps = 10000 / duration
 
-@njit(cache=True)
-def pid(speed, steer):
-    """
-    Basic controller for speed/steer -> accl./steer vel.
+        self.assertAlmostEqual(np.max(np.abs(f_ks_gt - f_ks)), 0.0)
+        self.assertAlmostEqual(np.max(np.abs(f_st_gt - f_st)), 0.0)
+        self.assertGreater(avg_fps, 5000)
 
-        Args:
-            speed (float): desired input speed
-            steer (float): desired input steering angle
+    def test_zeroinit_roll(self):
+        from scipy.integrate import odeint
 
-        Returns:
-            accl (float): desired input acceleration
-            sv (float): desired input steering velocity
-    """
-    return
+        # testing for zero initial state, zero input singularities
+        g = 9.81
+        t_start = 0.0
+        t_final = 1.0
+        delta0 = 0.0
+        vel0 = 0.0
+        Psi0 = 0.0
+        dotPsi0 = 0.0
+        beta0 = 0.0
+        sy0 = 0.0
+        initial_state = [0, sy0, delta0, vel0, Psi0, dotPsi0, beta0]
 
-def func_KS(x, t, 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):
-    f = vehicle_dynamics_ks(x, 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)
-    return f
+        x0_KS = np.array(initial_state[0:5])
+        x0_ST = np.array(initial_state)
 
-def func_ST(x, t, 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):
-    f = vehicle_dynamics_st(x, 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)
-    return f
+        # time vector
+        t = np.arange(t_start, t_final, 1e-4)
 
-class DynamicsTest(unittest.TestCase):
+        # set input: rolling car (velocity should stay constant)
+        u = np.array([0.0, 0.0])
+
+        # simulate single-track model
+        x_roll_st = odeint(
+            func_ST,
+            x0_ST,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
+        # simulate kinematic single-track model
+        x_roll_ks = odeint(
+            func_KS,
+            x0_KS,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
+
+        self.assertTrue(all(x_roll_st[-1] == x0_ST))
+        self.assertTrue(all(x_roll_ks[-1] == x0_KS))
+
+    def test_zeroinit_dec(self):
+        from scipy.integrate import odeint
+
+        # testing for zero initial state, decelerating input singularities
+        g = 9.81
+        t_start = 0.0
+        t_final = 1.0
+        delta0 = 0.0
+        vel0 = 0.0
+        Psi0 = 0.0
+        dotPsi0 = 0.0
+        beta0 = 0.0
+        sy0 = 0.0
+        initial_state = [0, sy0, delta0, vel0, Psi0, dotPsi0, beta0]
+
+        x0_KS = np.array(initial_state[0:5])
+        x0_ST = np.array(initial_state)
+
+        # time vector
+        t = np.arange(t_start, t_final, 1e-4)
+
+        # set decel input
+        u = np.array([0.0, -0.7 * g])
+
+        # simulate single-track model
+        x_dec_st = odeint(
+            func_ST,
+            x0_ST,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
+        # simulate kinematic single-track model
+        x_dec_ks = odeint(
+            func_KS,
+            x0_KS,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
+
+        # ground truth for single-track model
+        x_dec_st_gt = [
+            -3.4335000000000013,
+            0.0000000000000000,
+            0.0000000000000000,
+            -6.8670000000000018,
+            0.0000000000000000,
+            0.0000000000000000,
+            0.0000000000000000,
+        ]
+        # ground truth for kinematic single-track model
+        x_dec_ks_gt = [
+            -3.4335000000000013,
+            0.0000000000000000,
+            0.0000000000000000,
+            -6.8670000000000018,
+            0.0000000000000000,
+        ]
+
+        self.assertTrue(all(abs(x_dec_st[-1] - x_dec_st_gt) < 1e-2))
+        self.assertTrue(all(abs(x_dec_ks[-1] - x_dec_ks_gt) < 1e-2))
+
+    def test_zeroinit_acc(self):
+        from scipy.integrate import odeint
+
+        # testing for zero initial state, accelerating with left steer input singularities
+        # wheel spin and velocity should increase more wheel spin at rear
+        g = 9.81
+        t_start = 0.0
+        t_final = 1.0
+        delta0 = 0.0
+        vel0 = 0.0
+        Psi0 = 0.0
+        dotPsi0 = 0.0
+        beta0 = 0.0
+        sy0 = 0.0
+        initial_state = [0, sy0, delta0, vel0, Psi0, dotPsi0, beta0]
+
+        x0_KS = np.array(initial_state[0:5])
+        x0_ST = np.array(initial_state)
+
+        # time vector
+        t = np.arange(t_start, t_final, 1e-4)
+
+        # set decel input
+        u = np.array([0.15, 0.63 * g])
+
+        # simulate single-track model
+        x_acc_st = odeint(
+            func_ST,
+            x0_ST,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
+        # simulate kinematic single-track model
+        x_acc_ks = odeint(
+            func_KS,
+            x0_KS,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
+
+        # ground truth for single-track model
+        x_acc_st_gt = [
+            3.0731976046859715,
+            0.2869835398304389,
+            0.1500000000000000,
+            6.1802999999999999,
+            0.1097747074946325,
+            0.3248268063223301,
+            0.0697547542798040,
+        ]
+        # ground truth for kinematic single-track model
+        x_acc_ks_gt = [
+            3.0845676868494927,
+            0.1484249221523042,
+            0.1500000000000000,
+            6.1803000000000017,
+            0.1203664469224163,
+        ]
+
+        self.assertTrue(all(abs(x_acc_st[-1] - x_acc_st_gt) < 1e-2))
+        self.assertTrue(all(abs(x_acc_ks[-1] - x_acc_ks_gt) < 1e-2))
+
+    def test_zeroinit_rollleft(self):
+        from scipy.integrate import odeint
+
+        # testing for zero initial state, rolling and steering left input singularities
+        g = 9.81
+        t_start = 0.0
+        t_final = 1.0
+        delta0 = 0.0
+        vel0 = 0.0
+        Psi0 = 0.0
+        dotPsi0 = 0.0
+        beta0 = 0.0
+        sy0 = 0.0
+        initial_state = [0, sy0, delta0, vel0, Psi0, dotPsi0, beta0]
+
+        x0_KS = np.array(initial_state[0:5])
+        x0_ST = np.array(initial_state)
+
+        # time vector
+        t = np.arange(t_start, t_final, 1e-4)
+
+        # set decel input
+        u = np.array([0.15, 0.0])
+
+        # simulate single-track model
+        x_left_st = odeint(
+            func_ST,
+            x0_ST,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
+        # simulate kinematic single-track model
+        x_left_ks = odeint(
+            func_KS,
+            x0_KS,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
+
+        # ground truth for single-track model
+        x_left_st_gt = [
+            0.0000000000000000,
+            0.0000000000000000,
+            0.1500000000000000,
+            0.0000000000000000,
+            0.0000000000000000,
+            0.0000000000000000,
+            0.0000000000000000,
+        ]
+        # ground truth for kinematic single-track model
+        x_left_ks_gt = [
+            0.0000000000000000,
+            0.0000000000000000,
+            0.1500000000000000,
+            0.0000000000000000,
+            0.0000000000000000,
+        ]
+
+        self.assertTrue(all(abs(x_left_st[-1] - x_left_st_gt) < 1e-2))
+        self.assertTrue(all(abs(x_left_ks[-1] - x_left_ks_gt) < 1e-2))
+
+
+class DynamicsTest2(unittest.TestCase):
     def setUp(self):
         # test params
         self.mu = 1.0489
-        self.C_Sf = 21.92/1.0489
-        self.C_Sr = 21.92/1.0489
-        self.lf = 0.3048*3.793293
-        self.lr = 0.3048*4.667707
-        self.h = 0.3048*2.01355
-        self.m = 4.4482216152605/0.3048*74.91452
-        self.I = 4.4482216152605*0.3048*1321.416
-
-        #steering constraints
-        self.s_min = -1.066  #minimum steering angle [rad]
-        self.s_max = 1.066  #maximum steering angle [rad]
-        self.sv_min = -0.4  #minimum steering velocity [rad/s]
-        self.sv_max = 0.4  #maximum steering velocity [rad/s]
-
-        #longitudinal constraints
-        self.v_min = -13.6  #minimum velocity [m/s]
-        self.v_max = 50.8  #minimum velocity [m/s]
-        self.v_switch = 7.319  #switching velocity [m/s]
-        self.a_max = 11.5  #maximum absolute acceleration [m/s^2]
+        self.C_Sf = 21.92 / 1.0489
+        self.C_Sr = 21.92 / 1.0489
+        self.lf = 0.3048 * 3.793293
+        self.lr = 0.3048 * 4.667707
+        self.h = 0.3048 * 2.01355
+        self.m = 4.4482216152605 / 0.3048 * 74.91452
+        self.I = 4.4482216152605 * 0.3048 * 1321.416
+
+        # steering constraints
+        self.s_min = -1.066  # minimum steering angle [rad]
+        self.s_max = 1.066  # maximum steering angle [rad]
+        self.sv_min = -0.4  # minimum steering velocity [rad/s]
+        self.sv_max = 0.4  # maximum steering velocity [rad/s]
+
+        # longitudinal constraints
+        self.v_min = -13.6  # minimum velocity [m/s]
+        self.v_max = 50.8  # minimum velocity [m/s]
+        self.v_switch = 7.319  # switching velocity [m/s]
+        self.a_max = 11.5  # maximum absolute acceleration [m/s^2]
 
     def test_derivatives(self):
         # ground truth derivatives
-        f_ks_gt = [16.3475935934250209, 0.4819314886013121, 0.1500000000000000, 5.1464424102339752, 0.2401426578627629]
-        f_st_gt = [15.7213512030862397, 0.0925527979719355, 0.1500000000000000, 5.3536773276413925, 0.0529001056654038, 0.6435589397748606, 0.0313297971641291]
+        f_ks_gt = [
+            16.3475935934250209,
+            0.4819314886013121,
+            0.1500000000000000,
+            5.1464424102339752,
+            0.2401426578627629,
+        ]
+        f_st_gt = [
+            15.7213512030862397,
+            0.0925527979719355,
+            0.1500000000000000,
+            5.3536773276413925,
+            0.0529001056654038,
+            0.6435589397748606,
+            0.0313297971641291,
+        ]
 
         # system dynamics
         g = 9.81
-        x_ks = np.array([3.9579422297936526, 0.0391650102771405, 0.0378491427211811, 16.3546957860883566, 0.0294717351052816])
-        x_st = np.array([2.0233348142065677, 0.0041907137716636, 0.0197545248559617, 15.7216236334290116, 0.0025857914776859, 0.0529001056654038, 0.0033012170610298])
+        x_ks = np.array(
+            [
+                3.9579422297936526,
+                0.0391650102771405,
+                0.0378491427211811,
+                16.3546957860883566,
+                0.0294717351052816,
+            ]
+        )
+        x_st = np.array(
+            [
+                2.0233348142065677,
+                0.0041907137716636,
+                0.0197545248559617,
+                15.7216236334290116,
+                0.0025857914776859,
+                0.0529001056654038,
+                0.0033012170610298,
+            ]
+        )
         v_delta = 0.15
-        acc = 0.63*g
-        u = np.array([v_delta,  acc])
-
-        f_ks = vehicle_dynamics_ks(x_ks, u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max)
-        f_st = vehicle_dynamics_st(x_st, u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max)
+        acc = 0.63 * g
+        u = np.array([v_delta, acc])
+
+        f_ks = vehicle_dynamics_ks(
+            x_ks,
+            u,
+            self.mu,
+            self.C_Sf,
+            self.C_Sr,
+            self.lf,
+            self.lr,
+            self.h,
+            self.m,
+            self.I,
+            self.s_min,
+            self.s_max,
+            self.sv_min,
+            self.sv_max,
+            self.v_switch,
+            self.a_max,
+            self.v_min,
+            self.v_max,
+        )
+        f_st = vehicle_dynamics_st(
+            x_st,
+            u,
+            self.mu,
+            self.C_Sf,
+            self.C_Sr,
+            self.lf,
+            self.lr,
+            self.h,
+            self.m,
+            self.I,
+            self.s_min,
+            self.s_max,
+            self.sv_min,
+            self.sv_max,
+            self.v_switch,
+            self.a_max,
+            self.v_min,
+            self.v_max,
+        )
 
         start = time.time()
         for i in range(10000):
-            f_st = vehicle_dynamics_st(x_st, u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max)
+            f_st = vehicle_dynamics_st(
+                x_st,
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            )
         duration = time.time() - start
-        avg_fps = 10000/duration
+        avg_fps = 10000 / duration
 
-        self.assertAlmostEqual(np.max(np.abs(f_ks_gt-f_ks)), 0.)
-        self.assertAlmostEqual(np.max(np.abs(f_st_gt-f_st)), 0.)
+        self.assertAlmostEqual(np.max(np.abs(f_ks_gt - f_ks)), 0.0)
+        self.assertAlmostEqual(np.max(np.abs(f_st_gt - f_st)), 0.0)
         self.assertGreater(avg_fps, 5000)
 
     def test_zeroinit_roll(self):
@@ -253,15 +695,15 @@ def test_zeroinit_roll(self):
 
         # testing for zero initial state, zero input singularities
         g = 9.81
-        t_start = 0.
-        t_final = 1.
-        delta0 = 0.
-        vel0 = 0.
-        Psi0 = 0.
-        dotPsi0 = 0.
-        beta0 = 0.
-        sy0 = 0.
-        initial_state = [0,sy0,delta0,vel0,Psi0,dotPsi0,beta0]
+        t_start = 0.0
+        t_final = 1.0
+        delta0 = 0.0
+        vel0 = 0.0
+        Psi0 = 0.0
+        dotPsi0 = 0.0
+        beta0 = 0.0
+        sy0 = 0.0
+        initial_state = [0, sy0, delta0, vel0, Psi0, dotPsi0, beta0]
 
         x0_KS = np.array(initial_state[0:5])
         x0_ST = np.array(initial_state)
@@ -270,30 +712,76 @@ def test_zeroinit_roll(self):
         t = np.arange(t_start, t_final, 1e-4)
 
         # set input: rolling car (velocity should stay constant)
-        u = np.array([0., 0.])
+        u = np.array([0.0, 0.0])
 
         # simulate single-track model
-        x_roll_st = odeint(func_ST, x0_ST, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
+        x_roll_st = odeint(
+            func_ST,
+            x0_ST,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
         # simulate kinematic single-track model
-        x_roll_ks = odeint(func_KS, x0_KS, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
-
-        self.assertTrue(all(x_roll_st[-1]==x0_ST))
-        self.assertTrue(all(x_roll_ks[-1]==x0_KS))
+        x_roll_ks = odeint(
+            func_KS,
+            x0_KS,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
+
+        self.assertTrue(all(x_roll_st[-1] == x0_ST))
+        self.assertTrue(all(x_roll_ks[-1] == x0_KS))
 
     def test_zeroinit_dec(self):
         from scipy.integrate import odeint
 
         # testing for zero initial state, decelerating input singularities
         g = 9.81
-        t_start = 0.
-        t_final = 1.
-        delta0 = 0.
-        vel0 = 0.
-        Psi0 = 0.
-        dotPsi0 = 0.
-        beta0 = 0.
-        sy0 = 0.
-        initial_state = [0,sy0,delta0,vel0,Psi0,dotPsi0,beta0]
+        t_start = 0.0
+        t_final = 1.0
+        delta0 = 0.0
+        vel0 = 0.0
+        Psi0 = 0.0
+        dotPsi0 = 0.0
+        beta0 = 0.0
+        sy0 = 0.0
+        initial_state = [0, sy0, delta0, vel0, Psi0, dotPsi0, beta0]
 
         x0_KS = np.array(initial_state[0:5])
         x0_ST = np.array(initial_state)
@@ -302,17 +790,77 @@ def test_zeroinit_dec(self):
         t = np.arange(t_start, t_final, 1e-4)
 
         # set decel input
-        u = np.array([0., -0.7*g])
+        u = np.array([0.0, -0.7 * g])
 
         # simulate single-track model
-        x_dec_st = odeint(func_ST, x0_ST, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
+        x_dec_st = odeint(
+            func_ST,
+            x0_ST,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
         # simulate kinematic single-track model
-        x_dec_ks = odeint(func_KS, x0_KS, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
+        x_dec_ks = odeint(
+            func_KS,
+            x0_KS,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
 
         # ground truth for single-track model
-        x_dec_st_gt = [-3.4335000000000013, 0.0000000000000000, 0.0000000000000000, -6.8670000000000018, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000]
+        x_dec_st_gt = [
+            -3.4335000000000013,
+            0.0000000000000000,
+            0.0000000000000000,
+            -6.8670000000000018,
+            0.0000000000000000,
+            0.0000000000000000,
+            0.0000000000000000,
+        ]
         # ground truth for kinematic single-track model
-        x_dec_ks_gt = [-3.4335000000000013, 0.0000000000000000, 0.0000000000000000, -6.8670000000000018, 0.0000000000000000]
+        x_dec_ks_gt = [
+            -3.4335000000000013,
+            0.0000000000000000,
+            0.0000000000000000,
+            -6.8670000000000018,
+            0.0000000000000000,
+        ]
 
         self.assertTrue(all(abs(x_dec_st[-1] - x_dec_st_gt) < 1e-2))
         self.assertTrue(all(abs(x_dec_ks[-1] - x_dec_ks_gt) < 1e-2))
@@ -323,15 +871,15 @@ def test_zeroinit_acc(self):
         # testing for zero initial state, accelerating with left steer input singularities
         # wheel spin and velocity should increase more wheel spin at rear
         g = 9.81
-        t_start = 0.
-        t_final = 1.
-        delta0 = 0.
-        vel0 = 0.
-        Psi0 = 0.
-        dotPsi0 = 0.
-        beta0 = 0.
-        sy0 = 0.
-        initial_state = [0,sy0,delta0,vel0,Psi0,dotPsi0,beta0]
+        t_start = 0.0
+        t_final = 1.0
+        delta0 = 0.0
+        vel0 = 0.0
+        Psi0 = 0.0
+        dotPsi0 = 0.0
+        beta0 = 0.0
+        sy0 = 0.0
+        initial_state = [0, sy0, delta0, vel0, Psi0, dotPsi0, beta0]
 
         x0_KS = np.array(initial_state[0:5])
         x0_ST = np.array(initial_state)
@@ -340,17 +888,77 @@ def test_zeroinit_acc(self):
         t = np.arange(t_start, t_final, 1e-4)
 
         # set decel input
-        u = np.array([0.15, 0.63*g])
+        u = np.array([0.15, 0.63 * g])
 
         # simulate single-track model
-        x_acc_st = odeint(func_ST, x0_ST, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
+        x_acc_st = odeint(
+            func_ST,
+            x0_ST,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
         # simulate kinematic single-track model
-        x_acc_ks = odeint(func_KS, x0_KS, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
+        x_acc_ks = odeint(
+            func_KS,
+            x0_KS,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
 
         # ground truth for single-track model
-        x_acc_st_gt = [3.0731976046859715, 0.2869835398304389, 0.1500000000000000, 6.1802999999999999, 0.1097747074946325, 0.3248268063223301, 0.0697547542798040]
+        x_acc_st_gt = [
+            3.0731976046859715,
+            0.2869835398304389,
+            0.1500000000000000,
+            6.1802999999999999,
+            0.1097747074946325,
+            0.3248268063223301,
+            0.0697547542798040,
+        ]
         # ground truth for kinematic single-track model
-        x_acc_ks_gt = [3.0845676868494927, 0.1484249221523042, 0.1500000000000000, 6.1803000000000017, 0.1203664469224163]
+        x_acc_ks_gt = [
+            3.0845676868494927,
+            0.1484249221523042,
+            0.1500000000000000,
+            6.1803000000000017,
+            0.1203664469224163,
+        ]
 
         self.assertTrue(all(abs(x_acc_st[-1] - x_acc_st_gt) < 1e-2))
         self.assertTrue(all(abs(x_acc_ks[-1] - x_acc_ks_gt) < 1e-2))
@@ -360,15 +968,15 @@ def test_zeroinit_rollleft(self):
 
         # testing for zero initial state, rolling and steering left input singularities
         g = 9.81
-        t_start = 0.
-        t_final = 1.
-        delta0 = 0.
-        vel0 = 0.
-        Psi0 = 0.
-        dotPsi0 = 0.
-        beta0 = 0.
-        sy0 = 0.
-        initial_state = [0,sy0,delta0,vel0,Psi0,dotPsi0,beta0]
+        t_start = 0.0
+        t_final = 1.0
+        delta0 = 0.0
+        vel0 = 0.0
+        Psi0 = 0.0
+        dotPsi0 = 0.0
+        beta0 = 0.0
+        sy0 = 0.0
+        initial_state = [0, sy0, delta0, vel0, Psi0, dotPsi0, beta0]
 
         x0_KS = np.array(initial_state[0:5])
         x0_ST = np.array(initial_state)
@@ -377,20 +985,81 @@ def test_zeroinit_rollleft(self):
         t = np.arange(t_start, t_final, 1e-4)
 
         # set decel input
-        u = np.array([0.15, 0.])
+        u = np.array([0.15, 0.0])
 
         # simulate single-track model
-        x_left_st = odeint(func_ST, x0_ST, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
+        x_left_st = odeint(
+            func_ST,
+            x0_ST,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
         # simulate kinematic single-track model
-        x_left_ks = odeint(func_KS, x0_KS, t, args=(u, self.mu, self.C_Sf, self.C_Sr, self.lf, self.lr, self.h, self.m, self.I, self.s_min, self.s_max, self.sv_min, self.sv_max, self.v_switch, self.a_max, self.v_min, self.v_max))
+        x_left_ks = odeint(
+            func_KS,
+            x0_KS,
+            t,
+            args=(
+                u,
+                self.mu,
+                self.C_Sf,
+                self.C_Sr,
+                self.lf,
+                self.lr,
+                self.h,
+                self.m,
+                self.I,
+                self.s_min,
+                self.s_max,
+                self.sv_min,
+                self.sv_max,
+                self.v_switch,
+                self.a_max,
+                self.v_min,
+                self.v_max,
+            ),
+        )
 
         # ground truth for single-track model
-        x_left_st_gt = [0.0000000000000000, 0.0000000000000000, 0.1500000000000000, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000]
+        x_left_st_gt = [
+            0.0000000000000000,
+            0.0000000000000000,
+            0.1500000000000000,
+            0.0000000000000000,
+            0.0000000000000000,
+            0.0000000000000000,
+            0.0000000000000000,
+        ]
         # ground truth for kinematic single-track model
-        x_left_ks_gt = [0.0000000000000000, 0.0000000000000000, 0.1500000000000000, 0.0000000000000000, 0.0000000000000000]
+        x_left_ks_gt = [
+            0.0000000000000000,
+            0.0000000000000000,
+            0.1500000000000000,
+            0.0000000000000000,
+            0.0000000000000000,
+        ]
 
         self.assertTrue(all(abs(x_left_st[-1] - x_left_st_gt) < 1e-2))
         self.assertTrue(all(abs(x_left_ks[-1] - x_left_ks_gt) < 1e-2))
 
-if __name__ == '__main__':
-    unittest.main()
\ No newline at end of file
+
+if __name__ == "__main__":
+    unittest.main()

From 9786621f1e22b49bb28047e151d3cac156c37e16 Mon Sep 17 00:00:00 2001
From: Hongrui Zheng <billyzheng.bz@gmail.com>
Date: Sun, 9 Jul 2023 20:49:26 -0400
Subject: [PATCH 31/42] Added track ingestion

---
 examples/waypoint_follow.py | 10 ++++------
 gym/f110_gym/envs/track.py  | 25 ++++++++++++++++++++++++-
 setup.py                    |  3 ++-
 3 files changed, 30 insertions(+), 8 deletions(-)

diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py
index 080003bf..677c76f0 100644
--- a/examples/waypoint_follow.py
+++ b/examples/waypoint_follow.py
@@ -244,9 +244,7 @@ def _get_current_waypoint(self, waypoints, lookahead_distance, position, theta):
             return current_waypoint
         elif nearest_dist < self.max_reacquire:
             # NOTE: specify type or numba complains
-            return np.append(wpts[i, :], waypoints[i, self.conf.wpt_vind]).astype(
-                np.float32
-            )
+            return wpts[i, :]
         else:
             return None
 
@@ -301,13 +299,13 @@ def main():
         "mass": 3.463388126201571,
         "lf": 0.15597534362552312,
         "tlad": 0.82461887897713965,
-        "vgain": 1.375,
+        "vgain": 1,
     }  # 0.90338203837889}
 
     env = gym.make(
         "f110_gym:f110-v0",
         config={
-            "map": "Example",
+            "map": "Spielberg",
             "num_agents": 1,
             "observation_config": {"type": "kinematic_state"},
         },
@@ -338,7 +336,7 @@ def render_callback(env_renderer):
 
     env.add_render_callback(render_callback)
 
-    poses = np.array([[0.7, 0.0, 1.37]])
+    poses = np.array([[env.track.raceline.xs[0], env.track.raceline.ys[0], env.track.raceline.yaws[0]]])
     obs, info = env.reset(options={"poses": poses})
     done = False
     env.render()
diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
index f90377a1..c4ee5d51 100644
--- a/gym/f110_gym/envs/track.py
+++ b/gym/f110_gym/envs/track.py
@@ -1,15 +1,38 @@
+import os
 import pathlib
+import shutil
+import tarfile
 from dataclasses import dataclass
 from typing import Tuple
 
 import numpy as np
+import requests
 import yaml
+from f110_gym.envs.cubic_spline import CubicSpline2D
 from PIL import Image
 from PIL.Image import Transpose
 from yamldataclassconfig.config import YamlDataClassConfig
 
-from f110_gym.envs.cubic_spline import CubicSpline2D
+if not os.path.exists(pathlib.Path(__file__).parent.parent / "maps/convert.py"):
+    print('--- Downloading Race Tracks ---')
+    tracks_url = "http://api.f1tenth.org/f1tenth_racetracks-v1.0.0.tar.gz"
+    tracks_r = requests.get(url=tracks_url, allow_redirects=True)
+    open("/tmp/tracks.tar.gz", 'wb').write(tracks_r.content)
+
+    # extract
+    print('--- Extracting Race Tracks ---')
+    tracks_file = tarfile.open("/tmp/tracks.tar.gz")
+    tracks_file.extractall("/tmp/tracks/")
+    tracks_file.close()
+
+    # move
+    map_dir = pathlib.Path(__file__).parent.parent / "maps"
 
+    for fn in os.listdir("/tmp/tracks/f1tenth_racetracks-1.0.0/"):
+        src = "/tmp/tracks/f1tenth_racetracks-1.0.0/" + fn
+        dst = map_dir / fn
+        if fn  not in ["README.md", "LICENSE", ".gitignore", ".git"]:
+            shutil.move(src, dst)
 
 class Raceline:
     n: int
diff --git a/setup.py b/setup.py
index 7dfc0cc3..a87457ea 100644
--- a/setup.py
+++ b/setup.py
@@ -15,6 +15,7 @@
           'pyyaml>=5.3.1',
           'pyglet<1.5',
           'pyopengl',
-          'yamldataclassconfig'
+          'yamldataclassconfig',
+          'requests'
       ]
       )

From 418f1bbcba96e6754400b0e111570c1463b7f245 Mon Sep 17 00:00:00 2001
From: Hongrui Zheng <billyzheng.bz@gmail.com>
Date: Sun, 9 Jul 2023 22:10:33 -0400
Subject: [PATCH 32/42] update example to use config options

---
 examples/waypoint_follow.py | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py
index ef8751c6..415886e5 100644
--- a/examples/waypoint_follow.py
+++ b/examples/waypoint_follow.py
@@ -306,7 +306,12 @@ def main():
         config={
             "map": "Spielberg",
             "num_agents": 1,
+            "timestep": 0.01,
+            "integrator": "rk4",
+            "control_input": "speed",
+            "model": "st",
             "observation_config": {"type": "kinematic_state"},
+            "params": {"mu": 1.0},
         },
         render_mode="human",
     )

From 093c11c5d34291bc4e9004eb5f54a1aca61d9f0b Mon Sep 17 00:00:00 2001
From: Hongrui Zheng <billyzheng.bz@gmail.com>
Date: Tue, 11 Jul 2023 14:27:11 -0400
Subject: [PATCH 33/42] packaged single maps

---
 gym/f110_gym/envs/track.py | 37 ++++++++++++++++---------------------
 1 file changed, 16 insertions(+), 21 deletions(-)

diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
index c4ee5d51..460e1aab 100644
--- a/gym/f110_gym/envs/track.py
+++ b/gym/f110_gym/envs/track.py
@@ -1,6 +1,5 @@
 import os
 import pathlib
-import shutil
 import tarfile
 from dataclasses import dataclass
 from typing import Tuple
@@ -13,26 +12,6 @@
 from PIL.Image import Transpose
 from yamldataclassconfig.config import YamlDataClassConfig
 
-if not os.path.exists(pathlib.Path(__file__).parent.parent / "maps/convert.py"):
-    print('--- Downloading Race Tracks ---')
-    tracks_url = "http://api.f1tenth.org/f1tenth_racetracks-v1.0.0.tar.gz"
-    tracks_r = requests.get(url=tracks_url, allow_redirects=True)
-    open("/tmp/tracks.tar.gz", 'wb').write(tracks_r.content)
-
-    # extract
-    print('--- Extracting Race Tracks ---')
-    tracks_file = tarfile.open("/tmp/tracks.tar.gz")
-    tracks_file.extractall("/tmp/tracks/")
-    tracks_file.close()
-
-    # move
-    map_dir = pathlib.Path(__file__).parent.parent / "maps"
-
-    for fn in os.listdir("/tmp/tracks/f1tenth_racetracks-1.0.0/"):
-        src = "/tmp/tracks/f1tenth_racetracks-1.0.0/" + fn
-        dst = map_dir / fn
-        if fn  not in ["README.md", "LICENSE", ".gitignore", ".git"]:
-            shutil.move(src, dst)
 
 class Raceline:
     n: int
@@ -151,6 +130,22 @@ def find_track_dir(track_name):
     # the map dirpath, we loop over all possible maps and check if there is a matching with the current track
     map_dir = pathlib.Path(__file__).parent.parent / "maps"
 
+    if not os.path.exists(map_dir / track_name):
+        print("Downloading Files for: " + track_name)
+        tracks_url = "http://api.f1tenth.org/" + track_name + ".tar.xz"
+        tracks_r = requests.get(url=tracks_url, allow_redirects=True)
+        if tracks_r.status_code == 404:
+            raise FileNotFoundError(f"No maps exists for {track_name}.")
+
+        open("/tmp/" + track_name + ".tar.xz", "wb").write(tracks_r.content)
+
+        # extract
+        print("Extracting Files for: " + track_name)
+        map_dir = pathlib.Path(__file__).parent.parent / "maps"
+        tracks_file = tarfile.open("/tmp/" + track_name + ".tar.xz")
+        tracks_file.extractall(map_dir)
+        tracks_file.close()
+
     for base_dir in [map_dir]:
         if not base_dir.exists():
             continue

From d762e01d31c0cca38483b6e07aa04b6e8ba03629 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Tue, 11 Jul 2023 22:04:17 +0200
Subject: [PATCH 34/42] minor refactoring, remove unused line, cast length
 float

---
 gym/f110_gym/envs/track.py | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
index 460e1aab..e0388c76 100644
--- a/gym/f110_gym/envs/track.py
+++ b/gym/f110_gym/envs/track.py
@@ -50,7 +50,7 @@ def __init__(
 
         # approximate track length by linear-interpolation of x,y waypoints
         # note: we could use 'ss' but sometimes it is normalized to [0,1], so we recompute it here
-        self.length = np.sum(np.sqrt(np.diff(xs) ** 2 + np.diff(ys) ** 2))
+        self.length = float(np.sum(np.sqrt(np.diff(xs) ** 2 + np.diff(ys) ** 2)))
 
         # compute spline
         self.spline = spline if spline is not None else CubicSpline2D(xs, ys)
@@ -130,7 +130,7 @@ def find_track_dir(track_name):
     # the map dirpath, we loop over all possible maps and check if there is a matching with the current track
     map_dir = pathlib.Path(__file__).parent.parent / "maps"
 
-    if not os.path.exists(map_dir / track_name):
+    if not (map_dir / track_name).exists():
         print("Downloading Files for: " + track_name)
         tracks_url = "http://api.f1tenth.org/" + track_name + ".tar.xz"
         tracks_r = requests.get(url=tracks_url, allow_redirects=True)
@@ -141,7 +141,6 @@ def find_track_dir(track_name):
 
         # extract
         print("Extracting Files for: " + track_name)
-        map_dir = pathlib.Path(__file__).parent.parent / "maps"
         tracks_file = tarfile.open("/tmp/" + track_name + ".tar.xz")
         tracks_file.extractall(map_dir)
         tracks_file.close()

From a993673cd73ef4b3e2fc9b059dd6e2705a99794a Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Tue, 11 Jul 2023 22:19:29 +0200
Subject: [PATCH 35/42] fix unclosed file when creating tmp tar, add test
 download track

---
 gym/f110_gym/envs/track.py          |  3 ++-
 gym/f110_gym/unittest/track_test.py | 42 +++++++++++++++++++++++++++++
 2 files changed, 44 insertions(+), 1 deletion(-)

diff --git a/gym/f110_gym/envs/track.py b/gym/f110_gym/envs/track.py
index e0388c76..e5e23c8a 100644
--- a/gym/f110_gym/envs/track.py
+++ b/gym/f110_gym/envs/track.py
@@ -137,7 +137,8 @@ def find_track_dir(track_name):
         if tracks_r.status_code == 404:
             raise FileNotFoundError(f"No maps exists for {track_name}.")
 
-        open("/tmp/" + track_name + ".tar.xz", "wb").write(tracks_r.content)
+        with open("/tmp/" + track_name + ".tar.xz", "wb") as f:
+            f.write(tracks_r.content)
 
         # extract
         print("Extracting Files for: " + track_name)
diff --git a/gym/f110_gym/unittest/track_test.py b/gym/f110_gym/unittest/track_test.py
index 7feb86c3..7c3b9928 100644
--- a/gym/f110_gym/unittest/track_test.py
+++ b/gym/f110_gym/unittest/track_test.py
@@ -1,4 +1,5 @@
 import pathlib
+import time
 import unittest
 
 import numpy as np
@@ -95,3 +96,44 @@ def test_map_dir_structure(self):
                 # try to load raceline files
                 # it will raise an assertion error if the file format are not valid
                 centerline = Raceline.from_raceline_file(file_centerline)
+
+    def test_download_racetrack(self):
+        import shutil
+
+        track_name = "Spielberg"
+        track_backup = Track.from_track_name(track_name)
+
+        # rename the track dir
+        track_dir = find_track_dir(track_name)
+        tmp_dir = track_dir.parent / f"{track_name}_tmp{int(time.time())}"
+        track_dir.rename(tmp_dir)
+
+        # download the track
+        track = Track.from_track_name(track_name)
+
+        # check the two tracks' specs are the same
+        for spec_attr in ["name", "image", "resolution", "origin", "negate", "occupied_thresh", "free_thresh"]:
+            self.assertEqual(
+                getattr(track.spec, spec_attr), getattr(track_backup.spec, spec_attr)
+            )
+
+        # check the two tracks' racelines are the same
+        for raceline_attr in ["ss", "xs", "ys", "yaws", "ks", "vxs", "axs"]:
+            self.assertTrue(np.isclose(
+                getattr(track.raceline, raceline_attr), getattr(track_backup.raceline, raceline_attr)
+            ).all())
+
+        # check the two tracks' centerlines are the same
+        for centerline_attr in ["ss", "xs", "ys", "yaws", "ks", "vxs", "axs"]:
+            self.assertTrue(np.isclose(
+                getattr(track.centerline, centerline_attr), getattr(track_backup.centerline, centerline_attr)
+            ).all())
+
+        # remove the newly created track dir
+        track_dir = find_track_dir(track_name)
+        shutil.rmtree(track_dir, ignore_errors=True)
+
+        # rename the backup track dir to its original name
+        track_backup_dir = find_track_dir(tmp_dir.stem)
+        track_backup_dir.rename(track_dir)
+

From a25a90ee0dcff0fa7def51998cf7974901650fe7 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Tue, 11 Jul 2023 22:20:55 +0200
Subject: [PATCH 36/42] correct centerline loading in test mapdir structure

---
 gym/f110_gym/unittest/track_test.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gym/f110_gym/unittest/track_test.py b/gym/f110_gym/unittest/track_test.py
index 7c3b9928..2ed23a50 100644
--- a/gym/f110_gym/unittest/track_test.py
+++ b/gym/f110_gym/unittest/track_test.py
@@ -95,7 +95,7 @@ def test_map_dir_structure(self):
             if file_centerline.exists():
                 # try to load raceline files
                 # it will raise an assertion error if the file format are not valid
-                centerline = Raceline.from_raceline_file(file_centerline)
+                centerline = Raceline.from_centerline_file(file_centerline)
 
     def test_download_racetrack(self):
         import shutil

From 47fe6275cc52476d9cc2ba1961367276f9a2004a Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Tue, 11 Jul 2023 22:24:21 +0200
Subject: [PATCH 37/42] add maps dir to gitignore and add gitkeep file in maps
 dir

---
 .gitignore                 | 2 ++
 gym/f110_gym/maps/.gitkeep | 0
 2 files changed, 2 insertions(+)
 create mode 100644 gym/f110_gym/maps/.gitkeep

diff --git a/.gitignore b/.gitignore
index 2d07a8ea..94b5a670 100644
--- a/.gitignore
+++ b/.gitignore
@@ -171,3 +171,5 @@ cython_debug/
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
 #.idea/
 *.zip
+
+/gym/f110_gym/maps/*
diff --git a/gym/f110_gym/maps/.gitkeep b/gym/f110_gym/maps/.gitkeep
new file mode 100644
index 00000000..e69de29b

From 5b007abeef28045a7e26bc01ffd6826b97780ac1 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Tue, 11 Jul 2023 23:44:57 +0200
Subject: [PATCH 38/42] add space in action type, avoid initialization default
 CarAction as input argument of Simulator and Racecar because depends on
 params, update f110 env to use action space from action_type, recompute
 action space on configure, add test configure for action space.

---
 gym/f110_gym/envs/action.py            | 55 +++++++++++++++++++++----
 gym/f110_gym/envs/base_classes.py      | 14 +++----
 gym/f110_gym/envs/f110_env.py          | 32 ++++++---------
 gym/f110_gym/unittest/f110_env_test.py | 56 ++++++++++++++++++++++++++
 4 files changed, 123 insertions(+), 34 deletions(-)

diff --git a/gym/f110_gym/envs/action.py b/gym/f110_gym/envs/action.py
index f300b6ca..a4372f3b 100644
--- a/gym/f110_gym/envs/action.py
+++ b/gym/f110_gym/envs/action.py
@@ -2,6 +2,7 @@
 from enum import Enum
 from typing import Any, Tuple, Dict
 
+import gymnasium as gym
 import numpy as np
 from f110_gym.envs.dynamic_models import pid
 
@@ -13,12 +14,13 @@ class CarActionEnum(Enum):
     @staticmethod
     def from_string(action: str):
         if action == "accl":
-            return AcclAction()
+            return AcclAction
         elif action == "speed":
-            return SpeedAction()
+            return SpeedAction
         else:
             raise ValueError(f"Unknown action type {action}")
 
+
 class CarAction:
     def __init__(self) -> None:
         self._action_type = None
@@ -31,24 +33,43 @@ def act(self, action: Any, **kwargs) -> Tuple[float, float]:
     def type(self) -> str:
         return self._action_type
 
+    @property
+    def space(self) -> gym.Space:
+        return NotImplementedError(
+            f"space method not implemented for action type {self.type}"
+        )
+
 
 class AcclAction(CarAction):
-    def __init__(self) -> None:
+    def __init__(self, params: Dict) -> None:
         super().__init__()
         self._action_type = "accl"
 
-    def act(
-        self, action: Tuple[float, float], state, params
-    ) -> Tuple[float, float]:
+        self.steering_low, self.steering_high = params["sv_min"], params["sv_max"]
+        self.acc_low, self.acc_high = -params["a_max"], params["a_max"]
+
+    def act(self, action: Tuple[float, float], state, params) -> Tuple[float, float]:
         return action
 
+    @property
+    def space(self) -> gym.Space:
+        low = np.array([self.steering_low, self.acc_low]).astype(np.float32)
+        high = np.array([self.steering_high, self.acc_high]).astype(np.float32)
+
+        return gym.spaces.Box(low=low, high=high, shape=(2,), dtype=np.float32)
+
 
 class SpeedAction(CarAction):
-    def __init__(self) -> None:
+    def __init__(self, params: Dict) -> None:
         super().__init__()
         self._action_type = "speed"
 
-    def act(self, action: Tuple[float, float], state: np.ndarray, params: Dict) -> Tuple[float, float]:
+        self.steering_low, self.steering_high = params["s_min"], params["s_max"]
+        self.velocity_low, self.velocity_high = params["v_min"], params["v_max"]
+
+    def act(
+        self, action: Tuple[float, float], state: np.ndarray, params: Dict
+    ) -> Tuple[float, float]:
         accl, sv = pid(
             action[0],
             action[1],
@@ -60,3 +81,21 @@ def act(self, action: Tuple[float, float], state: np.ndarray, params: Dict) -> T
             params["v_min"],
         )
         return accl, sv
+
+    @property
+    def space(self) -> gym.Space:
+        low = np.array([self.steering_low, self.velocity_low]).astype(np.float32)
+        high = np.array([self.steering_high, self.velocity_high]).astype(np.float32)
+
+        return gym.spaces.Box(low=low, high=high, shape=(2,), dtype=np.float32)
+
+
+def from_single_to_multi_action_space(
+    single_agent_action_space: gym.spaces.Box, num_agents: int
+) -> gym.spaces.Box:
+    return gym.spaces.Box(
+        low=single_agent_action_space.low[None].repeat(num_agents, 0),
+        high=single_agent_action_space.high[None].repeat(num_agents, 0),
+        shape=(num_agents, single_agent_action_space.shape[0]),
+        dtype=np.float32,
+    )
diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index ed0b230f..0a6ac0f4 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -29,7 +29,7 @@
 import numpy as np
 
 from f110_gym.envs import DynamicModel
-from f110_gym.envs.action import SpeedAction
+from f110_gym.envs.action import CarAction
 from f110_gym.envs.integrator import EulerIntegrator, IntegratorType
 from f110_gym.envs.laser_models import ScanSimulator2D, check_ttc_jit, ray_cast
 from f110_gym.envs.collision_models import get_vertices, collision_multiple
@@ -62,13 +62,13 @@ def __init__(
         self,
         params,
         seed,
+        action_type: CarAction,
+        integrator=EulerIntegrator(),
+        model=DynamicModel.ST,
         is_ego=False,
         time_step=0.01,
         num_beams=1080,
         fov=4.7,
-        integrator=EulerIntegrator(),
-        model=DynamicModel.ST,
-        action_type=SpeedAction(),
     ):
         """
         TODO rewrite it
@@ -380,11 +380,11 @@ def __init__(
         params,
         num_agents,
         seed,
-        time_step=0.01,
-        ego_idx=0,
+        action_type: CarAction,
         integrator=IntegratorType.RK4,
         model=DynamicModel.ST,
-        action_type=SpeedAction(),
+        time_step=0.01,
+        ego_idx=0,
     ):
         """
         Init function
diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index e8c0663a..af449251 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -28,7 +28,7 @@
 import gymnasium as gym
 
 from f110_gym.envs import IntegratorType
-from f110_gym.envs.action import CarActionEnum
+from f110_gym.envs.action import CarActionEnum, from_single_to_multi_action_space
 
 # base classes
 from f110_gym.envs.base_classes import Simulator, DynamicModel
@@ -121,8 +121,9 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
         self.ego_idx = self.config["ego_idx"]
         self.integrator = IntegratorType.from_string(self.config["integrator"])
         self.model = DynamicModel.from_string(self.config["model"])
-        self.action_type = CarActionEnum.from_string(self.config["control_input"])
         self.observation_config = self.config["observation_config"]
+        action_type_fn = CarActionEnum.from_string(self.config["control_input"])
+        self.action_type = action_type_fn(params=self.params)
 
         # radius to consider done
         self.start_thresh = 0.5  # 10cm
@@ -132,8 +133,6 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
         self.poses_y = []
         self.poses_theta = []
         self.collisions = np.zeros((self.num_agents,))
-        # TODO: collision_idx not used yet
-        # self.collision_idx = -1 * np.ones((self.num_agents, ))
 
         # loop completion
         self.near_start = True
@@ -176,21 +175,8 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs):
         self.observation_space = self.observation_type.space()
 
         # action space
-        # NOTE: keep original structure of action space (box space), just define bounds
-        steering_low, steering_high = self.sim.params["s_min"], self.sim.params["s_max"]
-        velocity_low, velocity_high = self.sim.params["v_min"], self.sim.params["v_max"]
-        low = (
-            np.array([[steering_low, velocity_low]])
-            .repeat(self.num_agents, 0)
-            .astype(np.float32)
-        )
-        high = (
-            np.array([[steering_high, velocity_high]])
-            .repeat(self.num_agents, 0)
-            .astype(np.float32)
-        )
-        self.action_space = gym.spaces.Box(
-            low=low, high=high, shape=(self.num_agents, 2), dtype=np.float32
+        self.action_space = from_single_to_multi_action_space(
+            self.action_type.space, self.num_agents
         )
 
         # stateful observations for rendering
@@ -255,6 +241,14 @@ def configure(self, config: dict) -> None:
             if hasattr(self, "sim"):
                 self.sim.update_params(self.config["params"])
 
+            if hasattr(self, "action_space"):
+                # if some parameters changed, recompute action space
+                action_type_fn = CarActionEnum.from_string(self.config["control_input"])
+                self.action_type = action_type_fn(params=self.params)
+                self.action_space = from_single_to_multi_action_space(
+                    self.action_type.space, self.num_agents
+                )
+
     def _check_done(self):
         """
         Check if the current rollout is done
diff --git a/gym/f110_gym/unittest/f110_env_test.py b/gym/f110_gym/unittest/f110_env_test.py
index 3f04eb11..2358b3f3 100644
--- a/gym/f110_gym/unittest/f110_env_test.py
+++ b/gym/f110_gym/unittest/f110_env_test.py
@@ -105,3 +105,59 @@ def test_configure_method(self):
 
         base_env.close()
         extended_env.close()
+
+    def test_configure_action_space(self):
+        """
+        Try to change the upper bound of the action space, and check that the
+        action space is correctly updated.
+        """
+        base_env = self._make_env()
+        action_space_low = base_env.action_space.low
+        action_space_high = base_env.action_space.high
+
+        params = base_env.sim.params
+        new_v_max = 5.0
+        params["v_max"] = new_v_max
+
+        base_env.configure(config={"params": params})
+        new_action_space_low = base_env.action_space.low
+        new_action_space_high = base_env.action_space.high
+
+        self.assertTrue(
+            (action_space_low == new_action_space_low).all(),
+            "Steering action space should be the same",
+        )
+        self.assertTrue(
+            action_space_high[0][0] == new_action_space_high[0][0],
+            "Steering action space should be the same",
+        )
+        self.assertTrue(
+            new_action_space_high[0][1] == new_v_max,
+            f"Speed action high should be {new_v_max}",
+        )
+
+    def test_acceleration_action_space(self):
+        """
+        Test that the acceleration action space is correctly configured.
+        """
+        base_env = self._make_env(config={"control_input": "accl"})
+        params = base_env.sim.params
+        action_space_low = base_env.action_space.low
+        action_space_high = base_env.action_space.high
+
+        self.assertTrue(
+            (action_space_low[0][0] - params["sv_min"]) < 1e-6,
+            "lower sv does not match min steering velocity",
+        )
+        self.assertTrue(
+            (action_space_high[0][0] - params["sv_max"]) < 1e-6,
+            "upper sv does not match max steering velocity",
+        )
+        self.assertTrue(
+            (action_space_low[0][1] + params["a_max"]) < 1e-6,
+            "lower acceleration bound does not match a_min",
+        )
+        self.assertTrue(
+            (action_space_high[0][1] - params["a_max"]) < 1e-6,
+            "upper acceleration bound does not match a_max",
+        )

From 1bd837cf79ec01b1d89189b1f6cb509773cb743b Mon Sep 17 00:00:00 2001
From: Hongrui Zheng <billyzheng.bz@gmail.com>
Date: Tue, 11 Jul 2023 20:28:17 -0400
Subject: [PATCH 39/42] fix slow speed portion of st model cog

---
 gym/f110_gym/envs/dynamic_models.py | 82 +++++++++++++++++++++++++++--
 1 file changed, 78 insertions(+), 4 deletions(-)

diff --git a/gym/f110_gym/envs/dynamic_models.py b/gym/f110_gym/envs/dynamic_models.py
index f2d244cf..f4a60e32 100644
--- a/gym/f110_gym/envs/dynamic_models.py
+++ b/gym/f110_gym/envs/dynamic_models.py
@@ -197,6 +197,69 @@ def vehicle_dynamics_ks(
     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
+            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
+    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,
+        ]
+    )
+    return f
+
+
 @njit(cache=True)
 def vehicle_dynamics_st(
     x,
@@ -256,7 +319,7 @@ def vehicle_dynamics_st(
 
         # system dynamics
         x_ks = x[0:5]
-        f_ks = vehicle_dynamics_ks(
+        f_ks = vehicle_dynamics_ks_cog(
             x_ks,
             u,
             mu,
@@ -276,14 +339,25 @@ def vehicle_dynamics_st(
             v_min,
             v_max,
         )
+        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.hstack(
             (
                 f_ks,
                 np.array(
                     [
-                        u[1] / lwb * np.tan(x[2])
-                        + x[3] / (lwb * np.cos(x[2]) ** 2) * u[0],
-                        0,
+                        dd_psi,
+                        d_beta,
                     ]
                 ),
             )

From 48d676a81a0bd5b98b00d567d60cd4f220a20589 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Wed, 12 Jul 2023 21:44:58 +0200
Subject: [PATCH 40/42] bugfix: update params in configure, fix test.

---
 gym/f110_gym/envs/f110_env.py          | 2 ++
 gym/f110_gym/unittest/f110_env_test.py | 2 +-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py
index 163cc8dc..ce5c933e 100644
--- a/gym/f110_gym/envs/f110_env.py
+++ b/gym/f110_gym/envs/f110_env.py
@@ -237,6 +237,8 @@ def default_config(cls) -> dict:
     def configure(self, config: dict) -> None:
         if config:
             self.config = deep_update(self.config, config)
+            self.params = self.config["params"]
+
             if hasattr(self, "sim"):
                 self.sim.update_params(self.config["params"])
 
diff --git a/gym/f110_gym/unittest/f110_env_test.py b/gym/f110_gym/unittest/f110_env_test.py
index 758f74fb..e47f97d8 100644
--- a/gym/f110_gym/unittest/f110_env_test.py
+++ b/gym/f110_gym/unittest/f110_env_test.py
@@ -107,7 +107,7 @@ def test_configure_action_space(self):
         action_space_low = base_env.action_space.low
         action_space_high = base_env.action_space.high
 
-        params = base_env.sim.params
+        params = base_env.sim.params.copy()
         new_v_max = 5.0
         params["v_max"] = new_v_max
 

From 20ce77f5623b856cdaf1675fe751cb87f28e9747 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Wed, 12 Jul 2023 23:12:48 +0200
Subject: [PATCH 41/42] general: code formatting, tests: rename module, rename
 test file "test_*" for auto test discovery, update test scan simulator and
 collision checks.

---
 examples/waypoint_follow.py                   |  10 +-
 gym/f110_gym/__init__.py                      |   6 +-
 gym/f110_gym/envs/action.py                   |   2 -
 gym/f110_gym/envs/base_classes.py             |   1 -
 gym/f110_gym/envs/collision_models.py         | 124 +++++----
 gym/f110_gym/envs/f110_env_backup.py          | 191 ++++++++-----
 gym/f110_gym/envs/laser_models.py             | 253 ++++++++++++------
 gym/f110_gym/envs/rendering.py                | 131 +++++----
 gym/f110_gym/{unittest => test}/__init__.py   |   0
 .../{unittest => test}/legacy_scan.npz        | Bin
 .../{unittest => test}/legacy_scan_gen.py     |  73 ++---
 .../pyglet_test.py => test/pyglet_test_.py}   |  76 ++++--
 gym/f110_gym/test/pyglet_test_camera.py       | 127 +++++++++
 .../test_collision_checks.py}                 |  87 +++---
 .../test_dynamics.py}                         |  91 ++++++-
 .../test_f110_env.py}                         |   0
 .../test_observation.py}                      |   0
 .../scan_sim.py => test/test_scan_sim.py}     | 187 ++-----------
 .../track_test.py => test/test_track.py}      |  29 +-
 .../utils_test.py => test/test_utils.py}      |   0
 gym/f110_gym/unittest/pyglet_test_camera.py   | 131 ---------
 setup.py                                      |  39 +--
 22 files changed, 872 insertions(+), 686 deletions(-)
 rename gym/f110_gym/{unittest => test}/__init__.py (100%)
 rename gym/f110_gym/{unittest => test}/legacy_scan.npz (100%)
 rename gym/f110_gym/{unittest => test}/legacy_scan_gen.py (58%)
 rename gym/f110_gym/{unittest/pyglet_test.py => test/pyglet_test_.py} (72%)
 create mode 100644 gym/f110_gym/test/pyglet_test_camera.py
 rename gym/f110_gym/{unittest/collision_checks.py => test/test_collision_checks.py} (82%)
 rename gym/f110_gym/{unittest/dynamics_test.py => test/test_dynamics.py} (96%)
 rename gym/f110_gym/{unittest/f110_env_test.py => test/test_f110_env.py} (100%)
 rename gym/f110_gym/{unittest/observation_test.py => test/test_observation.py} (100%)
 rename gym/f110_gym/{unittest/scan_sim.py => test/test_scan_sim.py} (65%)
 rename gym/f110_gym/{unittest/track_test.py => test/test_track.py} (89%)
 rename gym/f110_gym/{unittest/utils_test.py => test/test_utils.py} (100%)
 delete mode 100644 gym/f110_gym/unittest/pyglet_test_camera.py

diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py
index 415886e5..5a0ffcea 100644
--- a/examples/waypoint_follow.py
+++ b/examples/waypoint_follow.py
@@ -340,7 +340,15 @@ def render_callback(env_renderer):
 
     env.add_render_callback(render_callback)
 
-    poses = np.array([[env.track.raceline.xs[0], env.track.raceline.ys[0], env.track.raceline.yaws[0]]])
+    poses = np.array(
+        [
+            [
+                env.track.raceline.xs[0],
+                env.track.raceline.ys[0],
+                env.track.raceline.yaws[0],
+            ]
+        ]
+    )
     obs, info = env.reset(options={"poses": poses})
     done = False
     env.render()
diff --git a/gym/f110_gym/__init__.py b/gym/f110_gym/__init__.py
index 362e9035..20052799 100644
--- a/gym/f110_gym/__init__.py
+++ b/gym/f110_gym/__init__.py
@@ -1,6 +1,6 @@
 import gymnasium as gym
 
 gym.register(
-	id='f110-v0',
-	entry_point='f110_gym.envs:F110Env',
-)
\ No newline at end of file
+    id="f110-v0",
+    entry_point="f110_gym.envs:F110Env",
+)
diff --git a/gym/f110_gym/envs/action.py b/gym/f110_gym/envs/action.py
index e168cd74..a4372f3b 100644
--- a/gym/f110_gym/envs/action.py
+++ b/gym/f110_gym/envs/action.py
@@ -48,7 +48,6 @@ def __init__(self, params: Dict) -> None:
         self.steering_low, self.steering_high = params["sv_min"], params["sv_max"]
         self.acc_low, self.acc_high = -params["a_max"], params["a_max"]
 
-
     def act(self, action: Tuple[float, float], state, params) -> Tuple[float, float]:
         return action
 
@@ -68,7 +67,6 @@ def __init__(self, params: Dict) -> None:
         self.steering_low, self.steering_high = params["s_min"], params["s_max"]
         self.velocity_low, self.velocity_high = params["v_min"], params["v_max"]
 
-
     def act(
         self, action: Tuple[float, float], state: np.ndarray, params: Dict
     ) -> Tuple[float, float]:
diff --git a/gym/f110_gym/envs/base_classes.py b/gym/f110_gym/envs/base_classes.py
index 112e84ce..b2156d1d 100644
--- a/gym/f110_gym/envs/base_classes.py
+++ b/gym/f110_gym/envs/base_classes.py
@@ -303,7 +303,6 @@ def update_pose(self, raw_steer, vel):
 
         u_np = np.array([sv, accl])
 
-
         f_dynamics = self.model.f_dynamics
         self.state = self.integrator.integrate(
             f=f_dynamics, x=self.state, u=u_np, dt=self.time_step, params=self.params
diff --git a/gym/f110_gym/envs/collision_models.py b/gym/f110_gym/envs/collision_models.py
index 45885e7c..8961cdc8 100644
--- a/gym/f110_gym/envs/collision_models.py
+++ b/gym/f110_gym/envs/collision_models.py
@@ -21,7 +21,6 @@
 # SOFTWARE.
 
 
-
 """
 Prototype of Utility functions and GJK algorithm for Collision checks between vehicles
 Originally from https://github.com/kroitor/gjk.c
@@ -31,6 +30,7 @@
 import numpy as np
 from numba import njit
 
+
 @njit(cache=True)
 def perpendicular(pt):
     """
@@ -44,7 +44,7 @@ def perpendicular(pt):
     """
     temp = pt[0]
     pt[0] = pt[1]
-    pt[1] = -1*temp
+    pt[1] = -1 * temp
     return pt
 
 
@@ -61,7 +61,7 @@ def tripleProduct(a, b, c):
     """
     ac = a.dot(c)
     bc = b.dot(c)
-    return b*ac - a*bc
+    return b * ac - a * bc
 
 
 @njit(cache=True)
@@ -75,7 +75,7 @@ def avgPoint(vertices):
     Returns:
         avg (np.ndarray, (2,)): average point of the vertices
     """
-    return np.sum(vertices, axis=0)/vertices.shape[0]
+    return np.sum(vertices, axis=0) / vertices.shape[0]
 
 
 @njit(cache=True)
@@ -153,7 +153,7 @@ def collision(vertices1, vertices2):
 
         if index < 2:
             b = simplex[0, :]
-            ab = b-a
+            ab = b - a
             d = tripleProduct(ab, ao, ab)
             if np.linalg.norm(d) < 1e-10:
                 d = perpendicular(ab)
@@ -161,8 +161,8 @@ def collision(vertices1, vertices2):
 
         b = simplex[1, :]
         c = simplex[0, :]
-        ab = b-a
-        ac = c-a
+        ab = b - a
+        ac = c - a
 
         acperp = tripleProduct(ab, ac, ac)
 
@@ -181,6 +181,7 @@ def collision(vertices1, vertices2):
         iter_count += 1
     return False
 
+
 @njit(cache=True)
 def collision_multiple(vertices):
     """
@@ -193,28 +194,30 @@ def collision_multiple(vertices):
         collisions (np.ndarray (num_vertices, )): whether each body is in collision
         collision_idx (np.ndarray (num_vertices, )): which index of other body is each index's body is in collision, -1 if not in collision
     """
-    collisions = np.zeros((vertices.shape[0], ))
-    collision_idx = -1 * np.ones((vertices.shape[0], ))
+    collisions = np.zeros((vertices.shape[0],))
+    collision_idx = -1 * np.ones((vertices.shape[0],))
     # looping over all pairs
-    for i in range(vertices.shape[0]-1):
-        for j in range(i+1, vertices.shape[0]):
+    for i in range(vertices.shape[0] - 1):
+        for j in range(i + 1, vertices.shape[0]):
             # check collision
             vi = np.ascontiguousarray(vertices[i, :, :])
             vj = np.ascontiguousarray(vertices[j, :, :])
             ij_collision = collision(vi, vj)
             # fill in results
             if ij_collision:
-                collisions[i] = 1.
-                collisions[j] = 1.
+                collisions[i] = 1.0
+                collisions[j] = 1.0
                 collision_idx[i] = j
                 collision_idx[j] = i
 
     return collisions, collision_idx
 
+
 """
 Utility functions for getting vertices by pose and shape
 """
 
+
 @njit(cache=True)
 def get_trmtx(pose):
     """
@@ -231,9 +234,17 @@ def get_trmtx(pose):
     th = pose[2]
     cos = np.cos(th)
     sin = np.sin(th)
-    H = np.array([[cos, -sin, 0., x], [sin, cos, 0., y], [0., 0., 1., 0.], [0., 0., 0., 1.]])
+    H = np.array(
+        [
+            [cos, -sin, 0.0, x],
+            [sin, cos, 0.0, y],
+            [0.0, 0.0, 1.0, 0.0],
+            [0.0, 0.0, 0.0, 1.0],
+        ]
+    )
     return H
 
+
 @njit(cache=True)
 def get_vertices(pose, length, width):
     """
@@ -248,48 +259,52 @@ def get_vertices(pose, length, width):
         vertices (np.ndarray, (4, 2)): corner vertices of the vehicle body
     """
     H = get_trmtx(pose)
-    rl = H.dot(np.asarray([[-length/2],[width/2],[0.], [1.]])).flatten()
-    rr = H.dot(np.asarray([[-length/2],[-width/2],[0.], [1.]])).flatten()
-    fl = H.dot(np.asarray([[length/2],[width/2],[0.], [1.]])).flatten()
-    fr = H.dot(np.asarray([[length/2],[-width/2],[0.], [1.]])).flatten()
-    rl = rl/rl[3]
-    rr = rr/rr[3]
-    fl = fl/fl[3]
-    fr = fr/fr[3]
-    vertices = np.asarray([[rl[0], rl[1]], [rr[0], rr[1]], [fr[0], fr[1]], [fl[0], fl[1]]])
+    rl = H.dot(np.asarray([[-length / 2], [width / 2], [0.0], [1.0]])).flatten()
+    rr = H.dot(np.asarray([[-length / 2], [-width / 2], [0.0], [1.0]])).flatten()
+    fl = H.dot(np.asarray([[length / 2], [width / 2], [0.0], [1.0]])).flatten()
+    fr = H.dot(np.asarray([[length / 2], [-width / 2], [0.0], [1.0]])).flatten()
+    rl = rl / rl[3]
+    rr = rr / rr[3]
+    fl = fl / fl[3]
+    fr = fr / fr[3]
+    vertices = np.asarray(
+        [[rl[0], rl[1]], [rr[0], rr[1]], [fr[0], fr[1]], [fl[0], fl[1]]]
+    )
     return vertices
 
 
 """
-Unit tests for GJK collision checks
+Unit test for GJK collision checks
 Author: Hongrui Zheng
 """
 
 import time
 import unittest
 
+
 class CollisionTests(unittest.TestCase):
     def setUp(self):
         # test params
         np.random.seed(1234)
 
         # Collision check body
-        self.vertices1 = np.asarray([[4,11.],[5,5],[9,9],[10,10]])
+        self.vertices1 = np.asarray([[4, 11.0], [5, 5], [9, 9], [10, 10]])
 
         # car size
         self.length = 0.32
         self.width = 0.22
-    
+
     def test_get_vert(self):
         test_pose = np.array([2.3, 6.7, 0.8])
         vertices = get_vertices(test_pose, self.length, self.width)
-        rect = np.vstack((vertices, vertices[0,:]))
+        rect = np.vstack((vertices, vertices[0, :]))
         import matplotlib.pyplot as plt
-        plt.scatter(test_pose[0], test_pose[1], c='red')
+
+        plt.scatter(test_pose[0], test_pose[1], c="red")
         plt.plot(rect[:, 0], rect[:, 1])
         plt.xlim([1, 4])
         plt.ylim([5, 8])
-        plt.axes().set_aspect('equal')
+        plt.axes().set_aspect("equal")
         plt.show()
         self.assertTrue(vertices.shape == (4, 2))
 
@@ -299,41 +314,46 @@ def test_get_vert_fps(self):
         for _ in range(1000):
             vertices = get_vertices(test_pose, self.length, self.width)
         elapsed = time.time() - start
-        fps = 1000/elapsed
-        print('get vertices fps:', fps)
-        self.assertTrue(fps>500)
+        fps = 1000 / elapsed
+        print("get vertices fps:", fps)
+        self.assertTrue(fps > 500)
 
     def test_random_collision(self):
         # perturb the body by a small amount and make sure it all collides with the original body
         for _ in range(1000):
-            a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-            b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-            self.assertTrue(collision(a,b))
+            a = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+            b = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+            self.assertTrue(collision(a, b))
 
     def test_multiple_collisions(self):
-        a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-        b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-        c = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-        d = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-        e = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-        f = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-        g = self.vertices1 + 10.
-        allv = np.stack((a,b,c,d,e,f,g))
+        a = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+        b = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+        c = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+        d = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+        e = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+        f = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+        g = self.vertices1 + 10.0
+        allv = np.stack((a, b, c, d, e, f, g))
         collisions, collision_idx = collision_multiple(allv)
-        self.assertTrue(np.all(collisions == np.array([1., 1., 1., 1., 1., 1., 0.])))
-        self.assertTrue(np.all(collision_idx == np.array([5., 5., 5., 5., 5., 4., -1.])))
+        self.assertTrue(
+            np.all(collisions == np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0]))
+        )
+        self.assertTrue(
+            np.all(collision_idx == np.array([5.0, 5.0, 5.0, 5.0, 5.0, 4.0, -1.0]))
+        )
 
     def test_fps(self):
         # also perturb the body but mainly want to test GJK speed
         start = time.time()
         for _ in range(1000):
-            a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-            b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
+            a = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+            b = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
             collision(a, b)
         elapsed = time.time() - start
-        fps = 1000/elapsed
-        print('gjk fps:', fps)
-        self.assertTrue(fps>500)
+        fps = 1000 / elapsed
+        print("gjk fps:", fps)
+        self.assertTrue(fps > 500)
+
 
-if __name__ == '__main__':
-    unittest.main()
\ No newline at end of file
+if __name__ == "__main__":
+    unittest.main()
diff --git a/gym/f110_gym/envs/f110_env_backup.py b/gym/f110_gym/envs/f110_env_backup.py
index 6744d0a0..5fd37721 100644
--- a/gym/f110_gym/envs/f110_env_backup.py
+++ b/gym/f110_gym/envs/f110_env_backup.py
@@ -20,9 +20,9 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 
-'''
+"""
 Author: Hongrui Zheng
-'''
+"""
 
 # gym imports
 import gym
@@ -53,6 +53,7 @@
 # from matplotlib.pyplot import imshow
 # import matplotlib.pyplot as plt
 
+
 class F110Env(gym.Env, utils.EzPickle):
     """
     OpenAI gym environment for F1/10 simulator
@@ -62,7 +63,8 @@ class F110Env(gym.Env, utils.EzPickle):
 
     should be initialized with a map, a timestep, and number of agents
     """
-    metadata = {'render.modes': []}
+
+    metadata = {"render.modes": []}
 
     def __init__(self):
         # simualtor params
@@ -152,7 +154,7 @@ def __init__(self):
         self.socket = self.context.socket(zmq.PAIR)
         while tries < max_tries:
             try:
-                self.socket.bind('tcp://*:%s' % str(min_port + tries))
+                self.socket.bind("tcp://*:%s" % str(min_port + tries))
                 # self.socket.connect('tcp://localhost:6666')
                 self.port = min_port + tries
                 break
@@ -160,7 +162,7 @@ def __init__(self):
                 tries = tries + 1
                 # print('Gym env - retrying for ' + str(tries) + ' times')
 
-        print('Gym env - Connected env to port: ' + str(self.port))
+        print("Gym env - Connected env to port: " + str(self.port))
 
         # create cpp instance if create then need to pass port number
         # subprocess call assumes directory structure
@@ -190,7 +192,19 @@ def _start_executable(self, path):
         cs_r = self.params[4]
         I_z = self.params[5]
         mass = self.params[6]
-        args = [path+'sim_server', str(self.timestep), str(self.num_agents), str(self.port), str(mu), str(h_cg), str(l_r), str(cs_f), str(cs_r), str(I_z), str(mass)]
+        args = [
+            path + "sim_server",
+            str(self.timestep),
+            str(self.num_agents),
+            str(self.port),
+            str(mu),
+            str(h_cg),
+            str(l_r),
+            str(cs_f),
+            str(cs_r),
+            str(I_z),
+            str(mass),
+        ]
         self.sim_p = subprocess.Popen(args)
 
     def _set_map(self):
@@ -198,11 +212,13 @@ def _set_map(self):
         Sets the map for the simulator instance
         """
         if not self.map_inited:
-            print('Gym env - Sim map not initialized, call env.init_map() to init map.')
+            print("Gym env - Sim map not initialized, call env.init_map() to init map.")
         # create and fill in protobuf
         map_request_proto = sim_requests_pb2.SimRequest()
         map_request_proto.type = 1
-        map_request_proto.map_request.map.extend((1. - self.map_img/255.).flatten().tolist())
+        map_request_proto.map_request.map.extend(
+            (1.0 - self.map_img / 255.0).flatten().tolist()
+        )
         map_request_proto.map_request.origin_x = self.origin[0]
         map_request_proto.map_request.origin_y = self.origin[1]
         map_request_proto.map_request.map_resolution = self.map_resolution
@@ -224,7 +240,7 @@ def _set_map(self):
         # get results
         set_map_result = sim_response_proto.map_result.result
         if set_map_result == 1:
-            print('Gym env - Set map failed, exiting...')
+            print("Gym env - Set map failed, exiting...")
             sys.exit()
 
     def _check_done(self):
@@ -243,17 +259,17 @@ def _check_done(self):
         right_t = 2
         timeout = self.current_time >= self.timeout
         if self.double_finish:
-            poses_x = np.array(self.all_x)-self.start_xs
-            poses_y = np.array(self.all_y)-self.start_ys
+            poses_x = np.array(self.all_x) - self.start_xs
+            poses_y = np.array(self.all_y) - self.start_ys
             delta_pt = np.dot(self.start_rot, np.stack((poses_x, poses_y), axis=0))
-            temp_y = delta_pt[1,:]
+            temp_y = delta_pt[1, :]
             idx1 = temp_y > left_t
             idx2 = temp_y < -right_t
             temp_y[idx1] -= left_t
             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]:
@@ -262,7 +278,7 @@ def _check_done(self):
                 elif not closes[i] and self.near_starts[i]:
                     self.near_starts[i] = False
                     self.toggle_list[i] += 1
-            done = (self.in_collision | (timeout) | np.all(self.toggle_list >= 4))
+            done = self.in_collision | (timeout) | np.all(self.toggle_list >= 4)
             # only for two cars atm
             self.lap_counts[0] = np.floor(self.toggle_list[0] / 2)
             self.lap_counts[1] = np.floor(self.toggle_list[1] / 2)
@@ -272,14 +288,16 @@ def _check_done(self):
                 self.lap_times[1] = self.current_time
             return done, self.toggle_list >= 4
 
-        delta_pt = np.dot(self.start_rot, np.array([self.x-self.start_x, self.y-self.start_y]))
-        if delta_pt[1] > left_t: # left
-            temp_y = delta_pt[1]-left_t
-        elif delta_pt[1] < -right_t: # right
+        delta_pt = np.dot(
+            self.start_rot, np.array([self.x - self.start_x, self.y - self.start_y])
+        )
+        if delta_pt[1] > left_t:  # left
+            temp_y = delta_pt[1] - left_t
+        elif delta_pt[1] < -right_t:  # right
             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:
@@ -288,7 +306,7 @@ def _check_done(self):
         elif not close and self.near_start:
             self.near_start = False
             self.num_toggles += 1
-        done = (self.in_collision | (timeout) | (self.num_toggles >= 4))
+        done = self.in_collision | (timeout) | (self.num_toggles >= 4)
         return done
 
     def _check_passed(self):
@@ -302,15 +320,15 @@ def _update_state(self, obs_dict):
         Update the env's states according to observations
         obs is observation dictionary
         """
-        self.x = obs_dict['poses_x'][obs_dict['ego_idx']]
-        self.y = obs_dict['poses_y'][obs_dict['ego_idx']]
+        self.x = obs_dict["poses_x"][obs_dict["ego_idx"]]
+        self.y = obs_dict["poses_y"][obs_dict["ego_idx"]]
         if self.double_finish:
-            self.all_x = obs_dict['poses_x']
-            self.all_y = obs_dict['poses_y']
+            self.all_x = obs_dict["poses_x"]
+            self.all_y = obs_dict["poses_y"]
 
-        self.theta = obs_dict['poses_theta'][obs_dict['ego_idx']]
-        self.in_collision = obs_dict['collisions'][obs_dict['ego_idx']]
-        self.collision_angle = obs_dict['collision_angles'][obs_dict['ego_idx']]
+        self.theta = obs_dict["poses_theta"][obs_dict["ego_idx"]]
+        self.in_collision = obs_dict["collisions"][obs_dict["ego_idx"]]
+        self.collision_angle = obs_dict["collision_angles"][obs_dict["ego_idx"]]
 
     # TODO: do we do the ray casting here or in C++?
     # if speed is a concern do it in C++?
@@ -327,16 +345,18 @@ def _raycast_opponents(self, obs_dict):
     def step(self, action):
         # can't step if params not set
         if not self.params_set:
-            print('ERROR - Gym Env - Params not set, call update params before stepping.')
+            print(
+                "ERROR - Gym Env - Params not set, call update params before stepping."
+            )
             sys.exit()
         # action is a list of steering angles + command velocities
         # also a ego car index
         # action should a DICT with {'ego_idx': int, 'speed':[], 'steer':[]}
         step_request_proto = sim_requests_pb2.SimRequest()
         step_request_proto.type = 0
-        step_request_proto.step_request.ego_idx = action['ego_idx']
-        step_request_proto.step_request.requested_vel.extend(action['speed'])
-        step_request_proto.step_request.requested_ang.extend(action['steer'])
+        step_request_proto.step_request.ego_idx = action["ego_idx"]
+        step_request_proto.step_request.requested_vel.extend(action["speed"])
+        step_request_proto.step_request.requested_ang.extend(action["steer"])
         # serialization
         step_request_string = step_request_proto.SerializeToString()
         # send step request
@@ -352,34 +372,47 @@ def step(self, action):
         response_type = sim_response_proto.type
         # TODO: also check for stepping fail
         if not response_type == 0:
-            print('Gym env - Wrong response type for stepping, exiting...')
+            print("Gym env - Wrong response type for stepping, exiting...")
             sys.exit()
         observations_proto = sim_response_proto.sim_obs
         # make sure the ego idx matches
-        if not observations_proto.ego_idx == action['ego_idx']:
-            print('Gym env - Ego index mismatch, exiting...')
+        if not observations_proto.ego_idx == action["ego_idx"]:
+            print("Gym env - Ego index mismatch, exiting...")
             sys.exit()
         # get observations
         carobs_list = observations_proto.observations
         # construct observation dict
         # Observation DICT, assume indices consistent: {'ego_idx':int, 'scans':[[]], 'poses_x':[], 'poses_y':[], 'poses_theta':[], 'linear_vels_x':[], 'linear_vels_y':[], 'ang_vels_z':[], 'collisions':[], 'collision_angles':[]}
-        obs = {'ego_idx': observations_proto.ego_idx, 'scans': [], 'poses_x': [], 'poses_y': [], 'poses_theta': [], 'linear_vels_x': [], 'linear_vels_y': [], 'ang_vels_z': [], 'collisions': [], 'collision_angles': [], 'lap_times': [], 'lap_counts': []}
+        obs = {
+            "ego_idx": observations_proto.ego_idx,
+            "scans": [],
+            "poses_x": [],
+            "poses_y": [],
+            "poses_theta": [],
+            "linear_vels_x": [],
+            "linear_vels_y": [],
+            "ang_vels_z": [],
+            "collisions": [],
+            "collision_angles": [],
+            "lap_times": [],
+            "lap_counts": [],
+        }
         for car_obs in carobs_list:
-            obs['scans'].append(car_obs.scan)
-            obs['poses_x'].append(car_obs.pose_x)
-            obs['poses_y'].append(car_obs.pose_y)
+            obs["scans"].append(car_obs.scan)
+            obs["poses_x"].append(car_obs.pose_x)
+            obs["poses_y"].append(car_obs.pose_y)
             if abs(car_obs.theta) < np.pi:
-                obs['poses_theta'].append(car_obs.theta)
+                obs["poses_theta"].append(car_obs.theta)
             else:
-                obs['poses_theta'].append(-((2 * np.pi) - car_obs.theta))
-            obs['linear_vels_x'].append(car_obs.linear_vel_x)
-            obs['linear_vels_y'].append(car_obs.linear_vel_y)
-            obs['ang_vels_z'].append(car_obs.ang_vel_z)
-            obs['collisions'].append(car_obs.collision)
-            obs['collision_angles'].append(car_obs.collision_angle)
+                obs["poses_theta"].append(-((2 * np.pi) - car_obs.theta))
+            obs["linear_vels_x"].append(car_obs.linear_vel_x)
+            obs["linear_vels_y"].append(car_obs.linear_vel_y)
+            obs["ang_vels_z"].append(car_obs.ang_vel_z)
+            obs["collisions"].append(car_obs.collision)
+            obs["collision_angles"].append(car_obs.collision_angle)
 
-        obs['lap_times'] = self.lap_times
-        obs['lap_counts'] = self.lap_counts
+        obs["lap_times"] = self.lap_times
+        obs["lap_counts"] = self.lap_counts
 
         # TODO: do we need step reward?
         reward = self.timestep
@@ -389,7 +422,7 @@ def step(self, action):
         self._update_state(obs)
         if self.double_finish:
             done, temp = self._check_done()
-            info = {'checkpoint_done': temp}
+            info = {"checkpoint_done": temp}
         else:
             done = self._check_done()
             info = {}
@@ -398,26 +431,29 @@ def step(self, action):
         return obs, reward, done, info
 
     def reset(self, poses=None):
-
         self.current_time = 0.0
         self.in_collision = False
         self.collision_angles = None
         self.num_toggles = 0
         self.near_start = True
-        self.near_starts = np.array([True]*self.num_agents)
+        self.near_starts = np.array([True] * self.num_agents)
         self.toggle_list = np.zeros((self.num_agents,))
         if poses:
-            pose_x = poses['x']
-            pose_y = poses['y']
-            pose_theta = poses['theta']
+            pose_x = poses["x"]
+            pose_y = poses["y"]
+            pose_theta = poses["theta"]
             self.start_x = pose_x[0]
             self.start_y = pose_y[0]
             self.start_theta = pose_theta[0]
             self.start_xs = np.array(pose_x)
             self.start_ys = np.array(pose_y)
             self.start_thetas = np.array(pose_theta)
-            self.start_rot = np.array([[np.cos(-self.start_theta), -np.sin(-self.start_theta)],
-                                        [np.sin(-self.start_theta), np.cos(-self.start_theta)]])
+            self.start_rot = np.array(
+                [
+                    [np.cos(-self.start_theta), -np.sin(-self.start_theta)],
+                    [np.sin(-self.start_theta), np.cos(-self.start_theta)],
+                ]
+            )
             # create reset by pose proto
             reset_request_proto = sim_requests_pb2.SimRequest()
             reset_request_proto.type = 4
@@ -433,8 +469,12 @@ def reset(self, poses=None):
             self.start_x = 0.0
             self.start_y = 0.0
             self.start_theta = 0.0
-            self.start_rot = np.array([[np.cos(-self.start_theta), -np.sin(-self.start_theta)],
-                                        [np.sin(-self.start_theta), np.cos(-self.start_theta)]])
+            self.start_rot = np.array(
+                [
+                    [np.cos(-self.start_theta), -np.sin(-self.start_theta)],
+                    [np.sin(-self.start_theta), np.cos(-self.start_theta)],
+                ]
+            )
             reset_request_proto = sim_requests_pb2.SimRequest()
             reset_request_proto.type = 2
             reset_request_proto.reset_request.num_cars = self.num_agents
@@ -448,13 +488,13 @@ def reset(self, poses=None):
         reset_response_proto = sim_requests_pb2.SimResponse()
         reset_response_proto.ParseFromString(reset_response_string)
         if reset_response_proto.reset_resp.result:
-            print('Gym env - Reset failed')
+            print("Gym env - Reset failed")
             # TODO: failure handling
             return None
         # TODO: return with gym convention, one step?
         vels = [0.0] * self.num_agents
         angs = [0.0] * self.num_agents
-        action = {'ego_idx': self.ego_idx, 'speed': vels, 'steer': angs}
+        action = {"ego_idx": self.ego_idx, "speed": vels, "steer": angs}
         # print('Gym env - Reset done')
         obs, reward, done, info = self.step(action)
         # print('Gym env - step done for reset')
@@ -462,21 +502,23 @@ def reset(self, poses=None):
 
     def init_map(self, map_path, img_ext, rgb, flip):
         """
-            init a map for the gym env
-            map_path: full path for the yaml, same as ROS, img and yaml in same dir
-            rgb: map grayscale or rgb
-            flip: if map needs flipping
+        init a map for the gym env
+        map_path: full path for the yaml, same as ROS, img and yaml in same dir
+        rgb: map grayscale or rgb
+        flip: if map needs flipping
         """
 
         self.map_path = map_path
-        if not map_path.endswith('.yaml'):
-            print('Gym env - Please use a yaml file for map initialization.')
-            print('Exiting...')
+        if not map_path.endswith(".yaml"):
+            print("Gym env - Please use a yaml file for map initialization.")
+            print("Exiting...")
             sys.exit()
 
         # split yaml ext name
         map_img_path = os.path.splitext(self.map_path)[0] + img_ext
-        self.map_img = np.array(Image.open(map_img_path).transpose(Image.FLIP_TOP_BOTTOM))
+        self.map_img = np.array(
+            Image.open(map_img_path).transpose(Image.FLIP_TOP_BOTTOM)
+        )
         self.map_img = self.map_img.astype(np.float64)
         if flip:
             self.map_img = self.map_img[::-1]
@@ -488,11 +530,11 @@ def init_map(self, map_path, img_ext, rgb, flip):
         self.map_height = self.map_img.shape[0]
         self.map_width = self.map_img.shape[1]
         self.free_thresh = 0.6  # TODO: double check
-        with open(self.map_path, 'r') as yaml_stream:
+        with open(self.map_path, "r") as yaml_stream:
             try:
                 map_metadata = yaml.safe_load(yaml_stream)
-                self.map_resolution = map_metadata['resolution']
-                self.origin = map_metadata['origin']
+                self.map_resolution = map_metadata["resolution"]
+                self.origin = map_metadata["origin"]
             except yaml.YAMLError as ex:
                 print(ex)
         self.map_inited = True
@@ -504,8 +546,7 @@ def init_map(self, map_path, img_ext, rgb, flip):
         #     # waypoints are [x, y, speed, theta]
         #     self.waypoints = np.array([(float(pt[0]), float(pt[1]), float(pt[2]), float(pt[3])) for pt in self.waypoints])
 
-
-    def render(self, mode='human', close=False):
+    def render(self, mode="human", close=False):
         return
 
     # def get_min_dist(self, position):
@@ -538,7 +579,9 @@ def render(self, mode='human', close=False):
     #     min_dist_segment = np.argmin(dists)
     #     return projections[min_dist_segment], dists[min_dist_segment], t[min_dist_segment], min_dist_segment
 
-    def update_params(self, mu, h_cg, l_r, cs_f, cs_r, I_z, mass, exe_path, double_finish=False):
+    def update_params(
+        self, mu, h_cg, l_r, cs_f, cs_r, I_z, mass, exe_path, double_finish=False
+    ):
         # if not self.sim_p is None:
         #     print('Gym env - Sim server exists, killing...')
         #     self.socket.send(b'dead')
@@ -577,7 +620,7 @@ def update_params(self, mu, h_cg, l_r, cs_f, cs_r, I_z, mass, exe_path, double_f
         update_response_proto = sim_requests_pb2.SimResponse()
         update_response_proto.ParseFromString(update_response_string)
         if update_response_proto.update_resp.result:
-            print('Gym env - Update param failed')
+            print("Gym env - Update param failed")
             return None
 
         # print('Gym env - params updated.')
diff --git a/gym/f110_gym/envs/laser_models.py b/gym/f110_gym/envs/laser_models.py
index 616653c0..450a2967 100644
--- a/gym/f110_gym/envs/laser_models.py
+++ b/gym/f110_gym/envs/laser_models.py
@@ -21,7 +21,6 @@
 # SOFTWARE.
 
 
-
 """
 Prototype of Utility functions and classes for simulating 2D LIDAR scans
 Author: Hongrui Zheng
@@ -55,6 +54,7 @@ def get_dt(bitmap, resolution):
     dt = resolution * edt(bitmap)
     return dt
 
+
 @njit(cache=True)
 def xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution):
     """
@@ -65,7 +65,7 @@ def xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution):
             y (float): coordinate in y (m)
             orig_x (float): x coordinate of the map origin (m)
             orig_y (float): y coordinate of the map origin (m)
-        
+
         Returns:
             r (int): row number in the transform matrix of the given point
             c (int): column number in the transform matrix of the given point
@@ -79,17 +79,25 @@ def xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution):
     y_rot = -x_trans * orig_s + y_trans * orig_c
 
     # clip the state to be a cell
-    if x_rot < 0 or x_rot >= width * resolution or y_rot < 0 or y_rot >= height * resolution:
+    if (
+        x_rot < 0
+        or x_rot >= width * resolution
+        or y_rot < 0
+        or y_rot >= height * resolution
+    ):
         c = -1
         r = -1
     else:
-        c = int(x_rot/resolution)
-        r = int(y_rot/resolution)
+        c = int(x_rot / resolution)
+        r = int(y_rot / resolution)
 
     return r, c
 
+
 @njit(cache=True)
-def distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt):
+def distance_transform(
+    x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt
+):
     """
     Look up corresponding distance in the distance matrix
 
@@ -106,8 +114,25 @@ def distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, reso
     distance = dt[r, c]
     return distance
 
+
 @njit(cache=True)
-def trace_ray(x, y, theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt, max_range):
+def trace_ray(
+    x,
+    y,
+    theta_index,
+    sines,
+    cosines,
+    eps,
+    orig_x,
+    orig_y,
+    orig_c,
+    orig_s,
+    height,
+    width,
+    resolution,
+    dt,
+    max_range,
+):
     """
     Find the length of a specific ray at a specific scan angle theta
     Purely math calculation and loops, should be JITted.
@@ -122,14 +147,16 @@ def trace_ray(x, y, theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, or
         Returns:
             total_distance (float): the distance to first obstacle on the current scan beam
     """
-    
+
     # int casting, and index precal trigs
     theta_index_ = int(theta_index)
     s = sines[theta_index_]
     c = cosines[theta_index_]
 
     # distance to nearest initialization
-    dist_to_nearest = distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt)
+    dist_to_nearest = distance_transform(
+        x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt
+    )
     total_dist = dist_to_nearest
 
     # ray tracing iterations
@@ -140,16 +167,37 @@ def trace_ray(x, y, theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, or
 
         # update dist_to_nearest for current point on ray
         # also keeps track of total ray length
-        dist_to_nearest = distance_transform(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt)
+        dist_to_nearest = distance_transform(
+            x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt
+        )
         total_dist += dist_to_nearest
 
     if total_dist > max_range:
         total_dist = max_range
-    
+
     return total_dist
 
+
 @njit(cache=True)
-def get_scan(pose, theta_dis, fov, num_beams, theta_index_increment, sines, cosines, eps, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt, max_range):
+def get_scan(
+    pose,
+    theta_dis,
+    fov,
+    num_beams,
+    theta_index_increment,
+    sines,
+    cosines,
+    eps,
+    orig_x,
+    orig_y,
+    orig_c,
+    orig_s,
+    height,
+    width,
+    resolution,
+    dt,
+    max_range,
+):
     """
     Perform the scan for each discretized angle of each beam of the laser, loop heavy, should be JITted
 
@@ -167,17 +215,33 @@ def get_scan(pose, theta_dis, fov, num_beams, theta_index_increment, sines, cosi
     scan = np.empty((num_beams,))
 
     # make theta discrete by mapping the range [-pi, pi] onto [0, theta_dis]
-    theta_index = theta_dis * (pose[2] - fov/2.)/(2. * np.pi)
+    theta_index = theta_dis * (pose[2] - fov / 2.0) / (2.0 * np.pi)
 
     # make sure it's wrapped properly
     theta_index = np.fmod(theta_index, theta_dis)
-    while (theta_index < 0):
+    while theta_index < 0:
         theta_index += theta_dis
 
     # sweep through each beam
     for i in range(0, num_beams):
         # trace the current beam
-        scan[i] = trace_ray(pose[0], pose[1], theta_index, sines, cosines, eps, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt, max_range)
+        scan[i] = trace_ray(
+            pose[0],
+            pose[1],
+            theta_index,
+            sines,
+            cosines,
+            eps,
+            orig_x,
+            orig_y,
+            orig_c,
+            orig_s,
+            height,
+            width,
+            resolution,
+            dt,
+            max_range,
+        )
 
         # increment the beam index
         theta_index += theta_index_increment
@@ -188,7 +252,8 @@ def get_scan(pose, theta_dis, fov, num_beams, theta_index_increment, sines, cosi
 
     return scan
 
-@njit(cache=True, error_model='numpy')
+
+@njit(cache=True, error_model="numpy")
 def check_ttc_jit(scan, vel, scan_angles, cosines, side_distances, ttc_thresh):
     """
     Checks the iTTC of each beam in a scan for collision with environment
@@ -209,8 +274,8 @@ def check_ttc_jit(scan, vel, scan_angles, cosines, side_distances, ttc_thresh):
     if vel != 0.0:
         num_beams = scan.shape[0]
         for i in range(num_beams):
-            proj_vel = vel*cosines[i]
-            ttc = (scan[i] - side_distances[i])/proj_vel
+            proj_vel = vel * cosines[i]
+            ttc = (scan[i] - side_distances[i]) / proj_vel
             if (ttc < ttc_thresh) and (ttc >= 0.0):
                 in_collision = True
                 break
@@ -219,6 +284,7 @@ def check_ttc_jit(scan, vel, scan_angles, cosines, side_distances, ttc_thresh):
 
     return in_collision
 
+
 @njit(cache=True)
 def cross(v1, v2):
     """
@@ -230,7 +296,8 @@ def cross(v1, v2):
     Returns:
         crossproduct (float): cross product
     """
-    return v1[0]*v2[1]-v1[1]*v2[0]
+    return v1[0] * v2[1] - v1[1] * v2[0]
+
 
 @njit(cache=True)
 def are_collinear(pt_a, pt_b, pt_c):
@@ -249,6 +316,7 @@ def are_collinear(pt_a, pt_b, pt_c):
     col = np.fabs(cross(ba, ca)) < tol
     return col
 
+
 @njit(cache=True)
 def get_range(pose, beam_theta, va, vb):
     """
@@ -265,7 +333,7 @@ def get_range(pose, beam_theta, va, vb):
     o = pose[0:2]
     v1 = o - va
     v2 = vb - va
-    v3 = np.array([np.cos(beam_theta + np.pi/2.), np.sin(beam_theta + np.pi/2.)])
+    v3 = np.array([np.cos(beam_theta + np.pi / 2.0), np.sin(beam_theta + np.pi / 2.0)])
 
     denom = v2.dot(v3)
     distance = np.inf
@@ -282,6 +350,7 @@ def get_range(pose, beam_theta, va, vb):
 
     return distance
 
+
 @njit(cache=True)
 def get_blocked_view_indices(pose, vertices, scan_angles):
     """
@@ -300,14 +369,16 @@ def get_blocked_view_indices(pose, vertices, scan_angles):
 
     # find angles between all four and pose vector
     ego_x_vec = np.array([[np.cos(pose[2])], [np.sin(pose[2])]])
-    
-    angles_with_x = np.empty((4, ))
+
+    angles_with_x = np.empty((4,))
     for i in range(4):
-        angle = np.arctan2(ego_x_vec[1], ego_x_vec[0]) - np.arctan2(unit_vecs[i, 1], unit_vecs[i, 0])
+        angle = np.arctan2(ego_x_vec[1], ego_x_vec[0]) - np.arctan2(
+            unit_vecs[i, 1], unit_vecs[i, 0]
+        )
         if angle > np.pi:
-            angle = angle - 2*np.pi
+            angle = angle - 2 * np.pi
         elif angle < -np.pi:
-            angle = angle + 2*np.pi
+            angle = angle + 2 * np.pi
         angles_with_x[i] = -angle[0]
 
     ind1 = int(np.argmin(np.abs(scan_angles - angles_with_x[0])))
@@ -328,7 +399,7 @@ def ray_cast(pose, scan, scan_angles, vertices):
         scan (np.ndarray(num_beams, )): original scan to modify
         scan_angles (np.ndarray(num_beams, )): corresponding beam angles
         vertices (np.ndarray(4, 2)): four vertices of a vehicle pose
-    
+
     Returns:
         new_scan (np.ndarray(num_beams, )): modified scan
     """
@@ -343,11 +414,17 @@ def ray_cast(pose, scan, scan_angles, vertices):
         # looping over vertices
         for j in range(4):
             # check if original scan is longer than ray casted distance
-            scan_range = get_range(pose, pose[2]+scan_angles[i], looped_vertices[j,:], looped_vertices[j+1,:])
+            scan_range = get_range(
+                pose,
+                pose[2] + scan_angles[i],
+                looped_vertices[j, :],
+                looped_vertices[j + 1, :],
+            )
             if scan_range < scan[i]:
                 scan[i] = scan_range
     return scan
 
+
 class ScanSimulator2D(object):
     """
     2D LIDAR scan simulator class
@@ -361,14 +438,14 @@ class ScanSimulator2D(object):
     """
 
     def __init__(self, num_beams, fov, eps=0.0001, theta_dis=2000, max_range=30.0):
-        # initialization 
+        # initialization
         self.num_beams = num_beams
         self.fov = fov
         self.eps = eps
         self.theta_dis = theta_dis
         self.max_range = max_range
         self.angle_increment = self.fov / (self.num_beams - 1)
-        self.theta_index_increment = theta_dis * self.angle_increment / (2. * np.pi)
+        self.theta_index_increment = theta_dis * self.angle_increment / (2.0 * np.pi)
         self.orig_c = None
         self.orig_s = None
         self.orig_x = None
@@ -377,12 +454,12 @@ def __init__(self, num_beams, fov, eps=0.0001, theta_dis=2000, max_range=30.0):
         self.map_width = None
         self.map_resolution = None
         self.dt = None
-        
+
         # precomputing corresponding cosines and sines of the angle array
-        theta_arr = np.linspace(0.0, 2*np.pi, num=theta_dis)
+        theta_arr = np.linspace(0.0, 2 * np.pi, num=theta_dis)
         self.sines = np.sin(theta_arr)
         self.cosines = np.cos(theta_arr)
-    
+
     def set_map(self, map_name: str):
         """
         Set the bitmap of the scan simulator by path
@@ -429,16 +506,34 @@ def scan(self, pose, rng, std_dev=0.01):
             Raises:
                 ValueError: when scan is called before a map is set
         """
-        
+
         if self.map_height is None:
-            raise ValueError('Map is not set for scan simulator.')
-        
-        scan = get_scan(pose, self.theta_dis, self.fov, self.num_beams, self.theta_index_increment, self.sines, self.cosines, self.eps, self.orig_x, self.orig_y, self.orig_c, self.orig_s, self.map_height, self.map_width, self.map_resolution, self.dt, self.max_range)
+            raise ValueError("Map is not set for scan simulator.")
+
+        scan = get_scan(
+            pose,
+            self.theta_dis,
+            self.fov,
+            self.num_beams,
+            self.theta_index_increment,
+            self.sines,
+            self.cosines,
+            self.eps,
+            self.orig_x,
+            self.orig_y,
+            self.orig_c,
+            self.orig_s,
+            self.map_height,
+            self.map_width,
+            self.map_resolution,
+            self.dt,
+            self.max_range,
+        )
 
         if rng is not None:
-            noise = rng.normal(0., std_dev, size=self.num_beams)
+            noise = rng.normal(0.0, std_dev, size=self.num_beams)
             scan += noise
-            
+
         return scan
 
     def get_increment(self):
@@ -446,7 +541,7 @@ def get_increment(self):
 
 
 """
-Unit tests for the 2D scan simulator class
+Unit test for the 2D scan simulator class
 Author: Hongrui Zheng
 
 Test cases:
@@ -463,7 +558,7 @@ def setUp(self):
 
         self.num_test = 10
         self.test_poses = np.zeros((self.num_test, 3))
-        self.test_poses[:, 2] = np.linspace(-1., 1., num=self.num_test)
+        self.test_poses[:, 2] = np.linspace(-1.0, 1.0, num=self.num_test)
 
         # # legacy gym data
         # sample_scan = np.load('legacy_scan.npz')
@@ -524,44 +619,45 @@ def test_fps(self):
 
         scan_rng = np.random.default_rng(seed=12345)
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
-        map_path = '../maps/Berlin/Berlin_map.yaml'
-        map_ext = '.png'
+        map_path = "../maps/Berlin/Berlin_map.yaml"
+        map_ext = ".png"
         scan_sim.set_map(map_path, map_ext)
-        
+
         import time
+
         start = time.time()
         for i in range(10000):
-            x_test = i/10000
-            scan = scan_sim.scan(np.array([x_test, 0., 0.]), scan_rng)
+            x_test = i / 10000
+            scan = scan_sim.scan(np.array([x_test, 0.0, 0.0]), scan_rng)
         end = time.time()
-        fps = 10000/(end-start)
+        fps = 10000 / (end - start)
         # print('FPS test')
         # print('Elapsed time: ' + str(end-start) + ' , FPS: ' + str(1/fps))
-        self.assertGreater(fps, 500.)
+        self.assertGreater(fps, 500.0)
 
     def test_rng(self):
         num_beams = 1080
         fov = 4.7
-        map_path = '../maps/Berlin/Berlin_map.yaml'
-        map_ext = '.png'
+        map_path = "../maps/Berlin/Berlin_map.yaml"
+        map_ext = ".png"
         it = 100
 
         scan_rng = np.random.default_rng(seed=12345)
         scan_sim = ScanSimulator2D(num_beams, fov)
         scan_sim.set_map(map_path, map_ext)
-        scan1 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng)
-        scan2 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng)
+        scan1 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng)
+        scan2 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng)
         for i in range(it):
-            scan3 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng)
-        scan4 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng)
-        
+            scan3 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng)
+        scan4 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng)
+
         scan_rng = np.random.default_rng(seed=12345)
-        scan5 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng)
-        scan2 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng)
+        scan5 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng)
+        scan2 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng)
         for i in range(it):
-            _ = scan_sim.scan(np.array([0., 0., 0.]), scan_rng)
-        scan6 = scan_sim.scan(np.array([0., 0., 0.]), scan_rng)
-        
+            _ = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng)
+        scan6 = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng)
+
         self.assertTrue(np.allclose(scan1, scan5))
         self.assertFalse(np.allclose(scan1, scan2))
         self.assertFalse(np.allclose(scan1, scan3))
@@ -572,49 +668,54 @@ def main():
     num_beams = 1080
     fov = 4.7
     # map_path = '../envs/maps/Berlin_map.yaml'
-    map_path = '../maps/Example/Example_map.yaml'
-    map_ext = '.png'
+    map_path = "../maps/Example/Example_map.yaml"
+    map_ext = ".png"
     scan_rng = np.random.default_rng(seed=12345)
     scan_sim = ScanSimulator2D(num_beams, fov)
     scan_sim.set_map(map_path, map_ext)
-    scan = scan_sim.scan(np.array([0., 0., 0.]), scan_rng)
+    scan = scan_sim.scan(np.array([0.0, 0.0, 0.0]), scan_rng)
 
     # fps test
     import time
+
     start = time.time()
     for i in range(10000):
-        x_test = i/10000
-        scan = scan_sim.scan(np.array([x_test, 0., 0.]), scan_rng)
+        x_test = i / 10000
+        scan = scan_sim.scan(np.array([x_test, 0.0, 0.0]), scan_rng)
     end = time.time()
-    fps = (end-start)/10000
-    print('FPS test')
-    print('Elapsed time: ' + str(end-start) + ' , FPS: ' + str(1/fps))
+    fps = (end - start) / 10000
+    print("FPS test")
+    print("Elapsed time: " + str(end - start) + " , FPS: " + str(1 / fps))
 
     # visualization
     import matplotlib.pyplot as plt
     from matplotlib.animation import FuncAnimation
+
     num_iter = 100
-    theta = np.linspace(-fov/2., fov/2., num=num_beams)
+    theta = np.linspace(-fov / 2.0, fov / 2.0, num=num_beams)
     fig = plt.figure()
-    ax = fig.add_subplot(111, projection='polar')
+    ax = fig.add_subplot(111, projection="polar")
     ax.set_ylim(0, 31)
-    line, = ax.plot([], [], '.', lw=0)
+    (line,) = ax.plot([], [], ".", lw=0)
+
     def update(i):
         # x_ani = i * 3. / num_iter
         theta_ani = -i * 2 * np.pi / num_iter
-        x_ani = 0.
-        current_scan = scan_sim.scan(np.array([x_ani, 0., theta_ani]), scan_rng)
+        x_ani = 0.0
+        current_scan = scan_sim.scan(np.array([x_ani, 0.0, theta_ani]), scan_rng)
         print(np.max(current_scan))
         line.set_data(theta, current_scan)
-        return line, 
+        return (line,)
+
     ani = FuncAnimation(fig, update, frames=num_iter, blit=True)
     plt.show()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     unittest.main()
-    #main()
+    # main()
 
-    # import time 
+    # import time
     # pt_a = np.array([1., 1.])
     # pt_b = np.array([1., 2.])
     # pt_c = np.array([1., 3.])
diff --git a/gym/f110_gym/envs/rendering.py b/gym/f110_gym/envs/rendering.py
index 374c3a98..44e06e4c 100644
--- a/gym/f110_gym/envs/rendering.py
+++ b/gym/f110_gym/envs/rendering.py
@@ -21,7 +21,6 @@
 # SOFTWARE.
 
 
-
 """
 Rendering engine for f1tenth gym env based on pyglet and OpenGL
 Author: Hongrui Zheng
@@ -37,21 +36,24 @@
 import yaml
 
 from f110_gym.envs.track import Track
+
 # helpers
 from f110_gym.envs.collision_models import get_vertices
 
 # zooming constants
 ZOOM_IN_FACTOR = 1.2
-ZOOM_OUT_FACTOR = 1/ZOOM_IN_FACTOR
+ZOOM_OUT_FACTOR = 1 / ZOOM_IN_FACTOR
 
 # vehicle shape constants
 CAR_LENGTH = 0.58
 CAR_WIDTH = 0.31
 
+
 class EnvRenderer(pyglet.window.Window):
     """
     A window class inherited from pyglet.window.Window, handles the camera/projection interaction, resizing window, and rendering the environment
     """
+
     def __init__(self, width, height, *args, **kwargs):
         """
         Class constructor
@@ -63,20 +65,19 @@ def __init__(self, width, height, *args, **kwargs):
         Returns:
             None
         """
-        conf = Config(sample_buffers=1,
-                      samples=4,
-                      depth_size=16,
-                      double_buffer=True)
-        super().__init__(width, height, config=conf, resizable=True, vsync=False, *args, **kwargs)
+        conf = Config(sample_buffers=1, samples=4, depth_size=16, double_buffer=True)
+        super().__init__(
+            width, height, config=conf, resizable=True, vsync=False, *args, **kwargs
+        )
 
         # gl init
-        glClearColor(9/255, 32/255, 87/255, 1.)
+        glClearColor(9 / 255, 32 / 255, 87 / 255, 1.0)
 
         # initialize camera values
-        self.left = -width/2
-        self.right = width/2
-        self.bottom = -height/2
-        self.top = height/2
+        self.left = -width / 2
+        self.right = width / 2
+        self.bottom = -height / 2
+        self.top = height / 2
         self.zoom_level = 1.2
         self.zoomed_width = width
         self.zoomed_height = height
@@ -86,7 +87,7 @@ def __init__(self, width, height, *args, **kwargs):
 
         # current env map
         self.map_points = None
-        
+
         # current env agent poses, (num_agents, 3), columns are (x, y, theta)
         self.poses = None
 
@@ -95,17 +96,19 @@ def __init__(self, width, height, *args, **kwargs):
 
         # current score label
         self.score_label = pyglet.text.Label(
-                'Lap Time: {laptime:.2f}, Ego Lap Count: {count:.0f}'.format(
-                    laptime=0.0, count=0.0),
-                font_size=36,
-                x=0,
-                y=-800,
-                anchor_x='center',
-                anchor_y='center',
-                # width=0.01,
-                # height=0.01,
-                color=(255, 255, 255, 255),
-                batch=self.batch)
+            "Lap Time: {laptime:.2f}, Ego Lap Count: {count:.0f}".format(
+                laptime=0.0, count=0.0
+            ),
+            font_size=36,
+            x=0,
+            y=-800,
+            anchor_x="center",
+            anchor_y="center",
+            # width=0.01,
+            # height=0.01,
+            color=(255, 255, 255, 255),
+            batch=self.batch,
+        )
 
         self.fps_display = pyglet.window.FPSDisplay(self)
 
@@ -142,9 +145,15 @@ def update_map(self, track: Track):
         # mask and only leave the obstacle points
         map_mask = map_img == 0.0
         map_mask_flat = map_mask.flatten()
-        map_points = 50. * map_coords[:, map_mask_flat].T
+        map_points = 50.0 * map_coords[:, map_mask_flat].T
         for i in range(map_points.shape[0]):
-            self.batch.add(1, GL_POINTS, None, ('v3f/stream', [map_points[i, 0], map_points[i, 1], map_points[i, 2]]), ('c3B/stream', [183, 193, 222]))
+            self.batch.add(
+                1,
+                GL_POINTS,
+                None,
+                ("v3f/stream", [map_points[i, 0], map_points[i, 1], map_points[i, 2]]),
+                ("c3B/stream", [183, 193, 222]),
+            )
         self.map_points = map_points
 
     def on_resize(self, width, height):
@@ -166,10 +175,10 @@ def on_resize(self, width, height):
 
         # update camera value
         (width, height) = self.get_size()
-        self.left = -self.zoom_level * width/2
-        self.right = self.zoom_level * width/2
-        self.bottom = -self.zoom_level * height/2
-        self.top = self.zoom_level * height/2
+        self.left = -self.zoom_level * width / 2
+        self.right = self.zoom_level * width / 2
+        self.bottom = -self.zoom_level * height / 2
+        self.top = self.zoom_level * height / 2
         self.zoomed_width = self.zoom_level * width
         self.zoomed_height = self.zoom_level * height
 
@@ -213,17 +222,16 @@ def on_mouse_scroll(self, x, y, dx, dy):
         f = ZOOM_IN_FACTOR if dy > 0 else ZOOM_OUT_FACTOR if dy < 0 else 1
 
         # If zoom_level is in the proper range
-        if .01 < self.zoom_level * f < 10:
-
+        if 0.01 < self.zoom_level * f < 10:
             self.zoom_level *= f
 
             (width, height) = self.get_size()
 
-            mouse_x = x/width
-            mouse_y = y/height
+            mouse_x = x / width
+            mouse_y = y / height
 
-            mouse_x_in_world = self.left + mouse_x*self.zoomed_width
-            mouse_y_in_world = self.bottom + mouse_y*self.zoomed_height
+            mouse_x_in_world = self.left + mouse_x * self.zoomed_width
+            mouse_y_in_world = self.bottom + mouse_y * self.zoomed_height
 
             self.zoomed_width *= f
             self.zoomed_height *= f
@@ -248,12 +256,12 @@ def on_close(self):
         """
 
         super().on_close()
-        raise Exception('Rendering window was closed.')
+        raise Exception("Rendering window was closed.")
 
     def on_draw(self):
         """
         Function when the pyglet is drawing. The function draws the batch created that includes the map points, the agent polygons, and the information text, and the fps display.
-        
+
         Args:
             None
 
@@ -263,9 +271,9 @@ def on_draw(self):
 
         # if map and poses doesn't exist, raise exception
         if self.map_points is None:
-            raise Exception('Map not set for renderer.')
+            raise Exception("Map not set for renderer.")
         if self.poses is None:
-            raise Exception('Agent poses not updated for renderer.')
+            raise Exception("Agent poses not updated for renderer.")
 
         # Initialize Projection matrix
         glMatrixMode(GL_PROJECTION)
@@ -300,31 +308,54 @@ def update_obs(self, obs):
             None
         """
 
-        self.ego_idx = obs['ego_idx']
-        poses_x = obs['poses_x']
-        poses_y = obs['poses_y']
-        poses_theta = obs['poses_theta']
+        self.ego_idx = obs["ego_idx"]
+        poses_x = obs["poses_x"]
+        poses_y = obs["poses_y"]
+        poses_theta = obs["poses_theta"]
 
         num_agents = len(poses_x)
         if self.poses is None:
             self.cars = []
             for i in range(num_agents):
                 if i == self.ego_idx:
-                    vertices_np = get_vertices(np.array([0., 0., 0.]), CAR_LENGTH, CAR_WIDTH)
+                    vertices_np = get_vertices(
+                        np.array([0.0, 0.0, 0.0]), CAR_LENGTH, CAR_WIDTH
+                    )
                     vertices = list(vertices_np.flatten())
-                    car = self.batch.add(4, GL_QUADS, None, ('v2f', vertices), ('c3B', [172, 97, 185, 172, 97, 185, 172, 97, 185, 172, 97, 185]))
+                    car = self.batch.add(
+                        4,
+                        GL_QUADS,
+                        None,
+                        ("v2f", vertices),
+                        (
+                            "c3B",
+                            [172, 97, 185, 172, 97, 185, 172, 97, 185, 172, 97, 185],
+                        ),
+                    )
                     self.cars.append(car)
                 else:
-                    vertices_np = get_vertices(np.array([0., 0., 0.]), CAR_LENGTH, CAR_WIDTH)
+                    vertices_np = get_vertices(
+                        np.array([0.0, 0.0, 0.0]), CAR_LENGTH, CAR_WIDTH
+                    )
                     vertices = list(vertices_np.flatten())
-                    car = self.batch.add(4, GL_QUADS, None, ('v2f', vertices), ('c3B', [99, 52, 94, 99, 52, 94, 99, 52, 94, 99, 52, 94]))
+                    car = self.batch.add(
+                        4,
+                        GL_QUADS,
+                        None,
+                        ("v2f", vertices),
+                        ("c3B", [99, 52, 94, 99, 52, 94, 99, 52, 94, 99, 52, 94]),
+                    )
                     self.cars.append(car)
 
         poses = np.stack((poses_x, poses_y, poses_theta)).T
         for j in range(poses.shape[0]):
-            vertices_np = 50. * get_vertices(poses[j, :], CAR_LENGTH, CAR_WIDTH)
+            vertices_np = 50.0 * get_vertices(poses[j, :], CAR_LENGTH, CAR_WIDTH)
             vertices = list(vertices_np.flatten())
             self.cars[j].vertices = vertices
         self.poses = poses
 
-        self.score_label.text = 'Lap Time: {laptime:.2f}, Ego Lap Count: {count:.0f}'.format(laptime=obs['lap_times'][0], count=obs['lap_counts'][obs['ego_idx']])
\ No newline at end of file
+        self.score_label.text = (
+            "Lap Time: {laptime:.2f}, Ego Lap Count: {count:.0f}".format(
+                laptime=obs["lap_times"][0], count=obs["lap_counts"][obs["ego_idx"]]
+            )
+        )
diff --git a/gym/f110_gym/unittest/__init__.py b/gym/f110_gym/test/__init__.py
similarity index 100%
rename from gym/f110_gym/unittest/__init__.py
rename to gym/f110_gym/test/__init__.py
diff --git a/gym/f110_gym/unittest/legacy_scan.npz b/gym/f110_gym/test/legacy_scan.npz
similarity index 100%
rename from gym/f110_gym/unittest/legacy_scan.npz
rename to gym/f110_gym/test/legacy_scan.npz
diff --git a/gym/f110_gym/unittest/legacy_scan_gen.py b/gym/f110_gym/test/legacy_scan_gen.py
similarity index 58%
rename from gym/f110_gym/unittest/legacy_scan_gen.py
rename to gym/f110_gym/test/legacy_scan_gen.py
index 20aa69db..96beed5a 100644
--- a/gym/f110_gym/unittest/legacy_scan_gen.py
+++ b/gym/f110_gym/test/legacy_scan_gen.py
@@ -21,12 +21,11 @@
 # SOFTWARE.
 
 
-
 """
 Utility functions to generate sample scan data from legacy C++ backend
 Author: Hongrui Zheng
 
-The script generates sample scan data for 3 different maps used in the unit tests.
+The script generates sample scan data for 3 different maps used in the unit test.
 
 Map 1: Levine
 
@@ -42,8 +41,8 @@
 thetas = np.linspace(-2.35, 2.35, num=1080)
 
 # init
-executable_dir = '../../../build/'
-mass= 3.74
+executable_dir = "../../../build/"
+mass = 3.74
 l_r = 0.17145
 I_z = 0.04712
 mu = 0.523
@@ -54,49 +53,57 @@
 # test poses
 num_test = 10
 test_poses = np.zeros((num_test, 3))
-test_poses[:, 2] = np.linspace(-1., 1., num=num_test)
+test_poses[:, 2] = np.linspace(-1.0, 1.0, num=num_test)
 
 # map 1: vegas
-map_path = '../../../maps/Vegas_map.yaml'
-map_ext = '.png'
-racecar_env = gym.make('f110_gym:f110-v0')
+map_path = "../../../maps/Vegas_map.yaml"
+map_ext = ".png"
+racecar_env = gym.make("f110_gym:f110-v0")
 racecar_env.init_map(map_path, map_ext, False, False)
-racecar_env.update_params(mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True)
+racecar_env.update_params(
+    mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True
+)
 vegas_scan = np.empty((num_test, 1080))
 for i in range(test_poses.shape[0]):
-    x = [test_poses[i, 0], 200.]
-    y = [test_poses[i, 1], 200.]
-    theta = [test_poses[i, 2], 0.]
-    obs,_,_,_ = racecar_env.reset({'x': x, 'y': y, 'theta': theta})
-    vegas_scan[i,:] = obs['scans'][0]
+    x = [test_poses[i, 0], 200.0]
+    y = [test_poses[i, 1], 200.0]
+    theta = [test_poses[i, 2], 0.0]
+    obs, _, _, _ = racecar_env.reset({"x": x, "y": y, "theta": theta})
+    vegas_scan[i, :] = obs["scans"][0]
 
 # map 2: berlin
-map_path = '../../../maps/Berlin_map.yaml'
-map_ext = '.png'
-racecar_env = gym.make('f110_gym:f110-v0')
+map_path = "../../../maps/Berlin_map.yaml"
+map_ext = ".png"
+racecar_env = gym.make("f110_gym:f110-v0")
 racecar_env.init_map(map_path, map_ext, False, False)
-racecar_env.update_params(mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True)
+racecar_env.update_params(
+    mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True
+)
 berlin_scan = np.empty((num_test, 1080))
 for i in range(test_poses.shape[0]):
-    x = [test_poses[i, 0], 200.]
-    y = [test_poses[i, 1], 200.]
-    theta = [test_poses[i, 2], 0.]
-    obs,_,_,_ = racecar_env.reset({'x': x, 'y': y, 'theta': theta})
-    berlin_scan[i,:] = obs['scans'][0]
+    x = [test_poses[i, 0], 200.0]
+    y = [test_poses[i, 1], 200.0]
+    theta = [test_poses[i, 2], 0.0]
+    obs, _, _, _ = racecar_env.reset({"x": x, "y": y, "theta": theta})
+    berlin_scan[i, :] = obs["scans"][0]
 
 # map 3: skirk
-map_path = '../../../maps/Skirk_map.yaml'
-map_ext = '.png'
-racecar_env = gym.make('f110_gym:f110-v0')
+map_path = "../../../maps/Skirk_map.yaml"
+map_ext = ".png"
+racecar_env = gym.make("f110_gym:f110-v0")
 racecar_env.init_map(map_path, map_ext, False, False)
-racecar_env.update_params(mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True)
+racecar_env.update_params(
+    mu, h_cg, l_r, cs_f, cs_r, I_z, mass, executable_dir, double_finish=True
+)
 skirk_scan = np.empty((num_test, 1080))
 for i in range(test_poses.shape[0]):
-    x = [test_poses[i, 0], 200.]
-    y = [test_poses[i, 1], 200.]
-    theta = [test_poses[i, 2], 0.]
-    obs,_,_,_ = racecar_env.reset({'x': x, 'y': y, 'theta': theta})
-    skirk_scan[i,:] = obs['scans'][0]
+    x = [test_poses[i, 0], 200.0]
+    y = [test_poses[i, 1], 200.0]
+    theta = [test_poses[i, 2], 0.0]
+    obs, _, _, _ = racecar_env.reset({"x": x, "y": y, "theta": theta})
+    skirk_scan[i, :] = obs["scans"][0]
 
 # package data
-np.savez_compressed('legacy_scan.npz', vegas=vegas_scan, berlin=berlin_scan, skirk=skirk_scan)
\ No newline at end of file
+np.savez_compressed(
+    "legacy_scan.npz", vegas=vegas_scan, berlin=berlin_scan, skirk=skirk_scan
+)
diff --git a/gym/f110_gym/unittest/pyglet_test.py b/gym/f110_gym/test/pyglet_test_.py
similarity index 72%
rename from gym/f110_gym/unittest/pyglet_test.py
rename to gym/f110_gym/test/pyglet_test_.py
index 43449019..8d0d355d 100644
--- a/gym/f110_gym/unittest/pyglet_test.py
+++ b/gym/f110_gym/test/pyglet_test_.py
@@ -8,11 +8,16 @@
 
 import argparse
 
-class Camera:
-    """ A simple 2D camera that contains the speed and offset."""
 
-    def __init__(self, window: pyglet.window.Window, scroll_speed=1, min_zoom=1, max_zoom=4):
-        assert min_zoom <= max_zoom, "Minimum zoom must not be greater than maximum zoom"
+class Camera:
+    """A simple 2D camera that contains the speed and offset."""
+
+    def __init__(
+        self, window: pyglet.window.Window, scroll_speed=1, min_zoom=1, max_zoom=4
+    ):
+        assert (
+            min_zoom <= max_zoom
+        ), "Minimum zoom must not be greater than maximum zoom"
         self._window = window
         self.scroll_speed = scroll_speed
         self.max_zoom = max_zoom
@@ -27,7 +32,7 @@ def zoom(self):
 
     @zoom.setter
     def zoom(self, value):
-        """ Here we set zoom, clamp value to minimum of min_zoom and max of max_zoom."""
+        """Here we set zoom, clamp value to minimum of min_zoom and max of max_zoom."""
         self._zoom = max(min(value, self.max_zoom), self.min_zoom)
 
     @property
@@ -41,9 +46,9 @@ def position(self, value):
         self.offset_x, self.offset_y = value
 
     def move(self, axis_x, axis_y):
-        """ Move axis direction with scroll_speed.
-            Example: Move left -> move(-1, 0)
-         """
+        """Move axis direction with scroll_speed.
+        Example: Move left -> move(-1, 0)
+        """
         self.offset_x += self.scroll_speed * axis_x
         self.offset_y += self.scroll_speed * axis_y
 
@@ -51,7 +56,9 @@ def begin(self):
         # Set the current camera offset so you can draw your scene.
 
         # Translate using the offset.
-        view_matrix = self._window.view.translate(-self.offset_x * self._zoom, -self.offset_y * self._zoom, 0)
+        view_matrix = self._window.view.translate(
+            -self.offset_x * self._zoom, -self.offset_y * self._zoom, 0
+        )
         # Scale by zoom level.
         view_matrix = view_matrix.scale(self._zoom, self._zoom, 1)
 
@@ -64,7 +71,9 @@ def end(self):
         # Reverse scale, since that was the last transform.
         view_matrix = self._window.view.scale(1 / self._zoom, 1 / self._zoom, 1)
         # Reverse translate.
-        view_matrix = view_matrix.translate(self.offset_x * self._zoom, self.offset_y * self._zoom, 0)
+        view_matrix = view_matrix.translate(
+            self.offset_x * self._zoom, self.offset_y * self._zoom, 0
+        )
 
         self._window.view = view_matrix
 
@@ -95,25 +104,30 @@ def end(self):
         self._window.view = view_matrix
 
 
-
 parser = argparse.ArgumentParser()
-parser.add_argument('--map_path', type=str, required=True, help='Path to the map without extensions')
-parser.add_argument('--map_ext', type=str, required=True, help='Extension of the map image file')
+parser.add_argument(
+    "--map_path", type=str, required=True, help="Path to the map without extensions"
+)
+parser.add_argument(
+    "--map_ext", type=str, required=True, help="Extension of the map image file"
+)
 args = parser.parse_args()
 
 # load map yaml
-with open(args.map_path + '.yaml', 'r') as yaml_stream:
+with open(args.map_path + ".yaml", "r") as yaml_stream:
     try:
         map_metada = yaml.safe_load(yaml_stream)
-        map_resolution = map_metada['resolution']
-        origin = map_metada['origin']
+        map_resolution = map_metada["resolution"]
+        origin = map_metada["origin"]
         origin_x = origin[0]
         origin_y = origin[1]
     except yaml.YAMLError as ex:
         print(ex)
 
 # load map image
-map_img = np.array(Image.open(args.map_path + args.map_ext).transpose(Image.FLIP_TOP_BOTTOM)).astype(np.float64)
+map_img = np.array(
+    Image.open(args.map_path + args.map_ext).transpose(Image.FLIP_TOP_BOTTOM)
+).astype(np.float64)
 map_height = map_img.shape[0]
 map_width = map_img.shape[1]
 
@@ -134,47 +148,61 @@ def end(self):
 # prep opengl
 try:
     # Try and create a window with multisampling (antialiasing)
-    config = Config(sample_buffers=1, samples=4,
-                    depth_size=16, double_buffer=True, )
+    config = Config(
+        sample_buffers=1,
+        samples=4,
+        depth_size=16,
+        double_buffer=True,
+    )
     window = window.Window(resizable=True, config=config)
 except window.NoSuchConfigException:
     # Fall back to no multisampling for old hardware
     window = window.Window(resizable=True)
 
-glClearColor(18/255, 4/255, 88/255, 1.)
+glClearColor(18 / 255, 4 / 255, 88 / 255, 1.0)
 glEnable(GL_DEPTH_TEST)
 glTranslatef(25, -5, -60)
 
 cam = Camera(window)
 
+
 @window.event
 def on_resize(width, height):
     # Override the default on_resize handler to create a 3D projection
     glViewport(0, 0, width, height)
     glMatrixMode(GL_PROJECTION)
     glLoadIdentity()
-    gluPerspective(60., width / float(height), .1, 1000.)
+    gluPerspective(60.0, width / float(height), 0.1, 1000.0)
     glMatrixMode(GL_MODELVIEW)
     return pyglet.event.EVENT_HANDLED
 
+
 batch = graphics.Batch()
 
 points = []
 for i in range(map_points.shape[0]):
-    particle = batch.add(1, GL_POINTS, None, ('v3f/stream', [map_points[i, 0], map_points[i, 1], map_points[i, 2]]))
+    particle = batch.add(
+        1,
+        GL_POINTS,
+        None,
+        ("v3f/stream", [map_points[i, 0], map_points[i, 1], map_points[i, 2]]),
+    )
     points.append(particle)
 
+
 def loop(dt):
     print(pyglet.clock.get_fps())
     pass
 
+
 @window.event
 def on_draw():
     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
-    glColor3f(254/255, 117/255, 254/255)
+    glColor3f(254 / 255, 117 / 255, 254 / 255)
     cam.begin()
     batch.draw()
     cam.end()
 
+
 pyglet.clock.schedule(loop)
-pyglet.app.run()
\ No newline at end of file
+pyglet.app.run()
diff --git a/gym/f110_gym/test/pyglet_test_camera.py b/gym/f110_gym/test/pyglet_test_camera.py
new file mode 100644
index 00000000..6f5f973c
--- /dev/null
+++ b/gym/f110_gym/test/pyglet_test_camera.py
@@ -0,0 +1,127 @@
+import pyglet
+from pyglet.gl import *
+
+# Zooming constants
+ZOOM_IN_FACTOR = 1.2
+ZOOM_OUT_FACTOR = 1 / ZOOM_IN_FACTOR
+
+
+class App(pyglet.window.Window):
+    def __init__(self, width, height, *args, **kwargs):
+        conf = Config(sample_buffers=1, samples=4, depth_size=16, double_buffer=True)
+        super().__init__(width, height, config=conf, *args, **kwargs)
+
+        # Initialize camera values
+        self.left = 0
+        self.right = width
+        self.bottom = 0
+        self.top = height
+        self.zoom_level = 1
+        self.zoomed_width = width
+        self.zoomed_height = height
+
+    def init_gl(self, width, height):
+        # Set clear color
+        glClearColor(0 / 255, 0 / 255, 0 / 255, 0 / 255)
+
+        # Set antialiasing
+        glEnable(GL_LINE_SMOOTH)
+        glEnable(GL_POLYGON_SMOOTH)
+        glHint(GL_LINE_SMOOTH_HINT, GL_NICEST)
+
+        # Set alpha blending
+        glEnable(GL_BLEND)
+        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
+
+        # Set viewport
+        glViewport(0, 0, width, height)
+
+    def on_resize(self, width, height):
+        super().on_resize(width, height)
+        size = self.get_size()
+        self.left = 0
+        self.right = size[0]
+        self.bottom = 0
+        self.top = size[1]
+        self.zoomed_width = size[0]
+        self.zoomed_height = size[1]
+
+        # # Set window values
+        # self.width  = width
+        # self.height = height
+        # # Initialize OpenGL context
+        # self.init_gl(width, height)
+        # self.width = width
+        # self.height = height
+        # pass
+
+    def on_mouse_drag(self, x, y, dx, dy, buttons, modifiers):
+        # Move camera
+        self.left -= dx * self.zoom_level
+        self.right -= dx * self.zoom_level
+        self.bottom -= dy * self.zoom_level
+        self.top -= dy * self.zoom_level
+
+    def on_mouse_scroll(self, x, y, dx, dy):
+        # Get scale factor
+        f = ZOOM_IN_FACTOR if dy > 0 else ZOOM_OUT_FACTOR if dy < 0 else 1
+        # If zoom_level is in the proper range
+        if 0.2 < self.zoom_level * f < 5:
+            self.zoom_level *= f
+
+            size = self.get_size()
+
+            mouse_x = x / size[0]
+            mouse_y = y / size[1]
+
+            mouse_x_in_world = self.left + mouse_x * self.zoomed_width
+            mouse_y_in_world = self.bottom + mouse_y * self.zoomed_height
+
+            self.zoomed_width *= f
+            self.zoomed_height *= f
+
+            self.left = mouse_x_in_world - mouse_x * self.zoomed_width
+            self.right = mouse_x_in_world + (1 - mouse_x) * self.zoomed_width
+            self.bottom = mouse_y_in_world - mouse_y * self.zoomed_height
+            self.top = mouse_y_in_world + (1 - mouse_y) * self.zoomed_height
+
+    def on_draw(self):
+        # Initialize Projection matrix
+        glMatrixMode(GL_PROJECTION)
+        glLoadIdentity()
+
+        # Initialize Modelview matrix
+        glMatrixMode(GL_MODELVIEW)
+        glLoadIdentity()
+        # Save the default modelview matrix
+        glPushMatrix()
+
+        # Clear window with ClearColor
+        glClear(GL_COLOR_BUFFER_BIT)
+
+        # Set orthographic projection matrix
+        glOrtho(self.left, self.right, self.bottom, self.top, 1, -1)
+
+        # Draw quad
+        glBegin(GL_QUADS)
+        glColor3ub(0xFF, 0, 0)
+        glVertex2i(10, 10)
+
+        glColor3ub(0xFF, 0xFF, 0)
+        glVertex2i(110, 10)
+
+        glColor3ub(0, 0xFF, 0)
+        glVertex2i(110, 110)
+
+        glColor3ub(0, 0, 0xFF)
+        glVertex2i(10, 110)
+        glEnd()
+
+        # Remove default modelview matrix
+        glPopMatrix()
+
+    def run(self):
+        pyglet.app.run()
+
+
+App(800, 800, resizable=True).run()
diff --git a/gym/f110_gym/unittest/collision_checks.py b/gym/f110_gym/test/test_collision_checks.py
similarity index 82%
rename from gym/f110_gym/unittest/collision_checks.py
rename to gym/f110_gym/test/test_collision_checks.py
index c5c86f0e..14d9cb9a 100644
--- a/gym/f110_gym/unittest/collision_checks.py
+++ b/gym/f110_gym/test/test_collision_checks.py
@@ -21,7 +21,6 @@
 # SOFTWARE.
 
 
-
 """
 Prototype of Utility functions and GJK algorithm for Collision checks between vehicles
 Originally from https://github.com/kroitor/gjk.c
@@ -31,6 +30,7 @@
 import numpy as np
 from numba import njit
 
+
 @njit(cache=True)
 def perpendicular(pt):
     """
@@ -44,7 +44,7 @@ def perpendicular(pt):
     """
     temp = pt[0]
     pt[0] = pt[1]
-    pt[1] = -1*temp
+    pt[1] = -1 * temp
     return pt
 
 
@@ -61,7 +61,7 @@ def tripleProduct(a, b, c):
     """
     ac = a.dot(c)
     bc = b.dot(c)
-    return b*ac - a*bc
+    return b * ac - a * bc
 
 
 @njit(cache=True)
@@ -75,7 +75,7 @@ def avgPoint(vertices):
     Returns:
         avg (np.ndarray, (2,)): average point of the vertices
     """
-    return np.sum(vertices, axis=0)/vertices.shape[0]
+    return np.sum(vertices, axis=0) / vertices.shape[0]
 
 
 @njit(cache=True)
@@ -153,7 +153,7 @@ def collision(vertices1, vertices2):
 
         if index < 2:
             b = simplex[0, :]
-            ab = b-a
+            ab = b - a
             d = tripleProduct(ab, ao, ab)
             if np.linalg.norm(d) < 1e-10:
                 d = perpendicular(ab)
@@ -161,8 +161,8 @@ def collision(vertices1, vertices2):
 
         b = simplex[1, :]
         c = simplex[0, :]
-        ab = b-a
-        ac = c-a
+        ab = b - a
+        ac = c - a
 
         acperp = tripleProduct(ab, ac, ac)
 
@@ -181,10 +181,12 @@ def collision(vertices1, vertices2):
         iter_count += 1
     return False
 
+
 """
 Utility functions for getting vertices by pose and shape
 """
 
+
 @njit(cache=True)
 def get_trmtx(pose):
     """
@@ -201,9 +203,17 @@ def get_trmtx(pose):
     th = pose[2]
     cos = np.cos(th)
     sin = np.sin(th)
-    H = np.array([[cos, -sin, 0., x], [sin, cos, 0., y], [0., 0., 1., 0.], [0., 0., 0., 1.]])
+    H = np.array(
+        [
+            [cos, -sin, 0.0, x],
+            [sin, cos, 0.0, y],
+            [0.0, 0.0, 1.0, 0.0],
+            [0.0, 0.0, 0.0, 1.0],
+        ]
+    )
     return H
 
+
 @njit(cache=True)
 def get_vertices(pose, length, width):
     """
@@ -218,48 +228,52 @@ def get_vertices(pose, length, width):
         vertices (np.ndarray, (4, 2)): corner vertices of the vehicle body
     """
     H = get_trmtx(pose)
-    rl = H.dot(np.asarray([[-length/2],[width/2],[0.], [1.]])).flatten()
-    rr = H.dot(np.asarray([[-length/2],[-width/2],[0.], [1.]])).flatten()
-    fl = H.dot(np.asarray([[length/2],[width/2],[0.], [1.]])).flatten()
-    fr = H.dot(np.asarray([[length/2],[-width/2],[0.], [1.]])).flatten()
-    rl = rl/rl[3]
-    rr = rr/rr[3]
-    fl = fl/fl[3]
-    fr = fr/fr[3]
-    vertices = np.asarray([[rl[0], rl[1]], [rr[0], rr[1]], [fr[0], fr[1]], [fl[0], fl[1]]])
+    rl = H.dot(np.asarray([[-length / 2], [width / 2], [0.0], [1.0]])).flatten()
+    rr = H.dot(np.asarray([[-length / 2], [-width / 2], [0.0], [1.0]])).flatten()
+    fl = H.dot(np.asarray([[length / 2], [width / 2], [0.0], [1.0]])).flatten()
+    fr = H.dot(np.asarray([[length / 2], [-width / 2], [0.0], [1.0]])).flatten()
+    rl = rl / rl[3]
+    rr = rr / rr[3]
+    fl = fl / fl[3]
+    fr = fr / fr[3]
+    vertices = np.asarray(
+        [[rl[0], rl[1]], [rr[0], rr[1]], [fr[0], fr[1]], [fl[0], fl[1]]]
+    )
     return vertices
 
 
 """
-Unit tests for GJK collision checks
+Unit test for GJK collision checks
 Author: Hongrui Zheng
 """
 
 import time
 import unittest
 
+
 class CollisionTests(unittest.TestCase):
     def setUp(self):
         # test params
         np.random.seed(1234)
 
         # Collision check body
-        self.vertices1 = np.asarray([[4,11.],[5,5],[9,9],[10,10]])
+        self.vertices1 = np.asarray([[4, 11.0], [5, 5], [9, 9], [10, 10]])
 
         # car size
         self.length = 0.32
         self.width = 0.22
-    
+
     def test_get_vert(self):
         test_pose = np.array([2.3, 6.7, 0.8])
         vertices = get_vertices(test_pose, self.length, self.width)
-        rect = np.vstack((vertices, vertices[0,:]))
+        rect = np.vstack((vertices, vertices[0, :]))
         import matplotlib.pyplot as plt
-        plt.scatter(test_pose[0], test_pose[1], c='red')
+
+        plt.scatter(test_pose[0], test_pose[1], c="red")
         plt.plot(rect[:, 0], rect[:, 1])
         plt.xlim([1, 4])
         plt.ylim([5, 8])
-        plt.axes().set_aspect('equal')
+        plt.axes().set_aspect("equal")
         plt.show()
         self.assertTrue(vertices.shape == (4, 2))
 
@@ -269,28 +283,29 @@ def test_get_vert_fps(self):
         for _ in range(1000):
             vertices = get_vertices(test_pose, self.length, self.width)
         elapsed = time.time() - start
-        fps = 1000/elapsed
-        print('get vertices fps:', fps)
-        self.assertTrue(fps>500)
+        fps = 1000 / elapsed
+        print("get vertices fps:", fps)
+        self.assertTrue(fps > 500)
 
     def test_random_collision(self):
         # perturb the body by a small amount and make sure it all collides with the original body
         for _ in range(1000):
-            a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-            b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-            self.assertTrue(collision(a,b))
+            a = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+            b = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+            self.assertTrue(collision(a, b))
 
     def test_fps(self):
         # also perturb the body but mainly want to test GJK speed
         start = time.time()
         for _ in range(1000):
-            a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
-            b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100.
+            a = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
+            b = self.vertices1 + np.random.normal(size=(self.vertices1.shape)) / 100.0
             collision(a, b)
         elapsed = time.time() - start
-        fps = 1000/elapsed
-        print('gjk fps:', fps)
-        self.assertTrue(fps>500)
+        fps = 1000 / elapsed
+        print("gjk fps:", fps)
+        self.assertTrue(fps > 500)
+
 
-if __name__ == '__main__':
-    unittest.main()
\ No newline at end of file
+if __name__ == "__main__":
+    unittest.main()
diff --git a/gym/f110_gym/unittest/dynamics_test.py b/gym/f110_gym/test/test_dynamics.py
similarity index 96%
rename from gym/f110_gym/unittest/dynamics_test.py
rename to gym/f110_gym/test/test_dynamics.py
index e2ffa72f..7bce8f43 100644
--- a/gym/f110_gym/unittest/dynamics_test.py
+++ b/gym/f110_gym/test/test_dynamics.py
@@ -28,19 +28,94 @@
 from f110_gym.envs import vehicle_dynamics_ks, vehicle_dynamics_st
 
 
-def func_KS(x, t, 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):
-    f = vehicle_dynamics_ks(x, 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)
+def func_KS(
+    x,
+    t,
+    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,
+):
+    f = vehicle_dynamics_ks(
+        x,
+        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,
+    )
     return f
 
 
-def func_ST(x, t, 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):
-    f = vehicle_dynamics_st(x, 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)
+def func_ST(
+    x,
+    t,
+    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,
+):
+    f = vehicle_dynamics_st(
+        x,
+        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,
+    )
     return f
 
+
 class DynamicsTest(unittest.TestCase):
     def setUp(self):
         # test params
diff --git a/gym/f110_gym/unittest/f110_env_test.py b/gym/f110_gym/test/test_f110_env.py
similarity index 100%
rename from gym/f110_gym/unittest/f110_env_test.py
rename to gym/f110_gym/test/test_f110_env.py
diff --git a/gym/f110_gym/unittest/observation_test.py b/gym/f110_gym/test/test_observation.py
similarity index 100%
rename from gym/f110_gym/unittest/observation_test.py
rename to gym/f110_gym/test/test_observation.py
diff --git a/gym/f110_gym/unittest/scan_sim.py b/gym/f110_gym/test/test_scan_sim.py
similarity index 65%
rename from gym/f110_gym/unittest/scan_sim.py
rename to gym/f110_gym/test/test_scan_sim.py
index bb1ca344..41d8b49c 100644
--- a/gym/f110_gym/unittest/scan_sim.py
+++ b/gym/f110_gym/test/test_scan_sim.py
@@ -36,6 +36,8 @@
 import unittest
 import timeit
 
+from f110_gym.envs import ScanSimulator2D
+
 
 def get_dt(bitmap, resolution):
     """
@@ -248,152 +250,13 @@ def get_scan(
     return scan
 
 
-class ScanSimulator2D(object):
-    """
-    2D LIDAR scan simulator class
-
-    Init params:
-        num_beams (int): number of beams in the scan
-        fov (float): field of view of the laser scan
-        std_dev (float, default=0.01): standard deviation of the generated whitenoise in the scan
-        eps (float, default=0.0001): ray tracing iteration termination condition
-        theta_dis (int, default=2000): number of steps to discretize the angles between 0 and 2pi for look up
-        max_range (float, default=30.0): maximum range of the laser
-        seed (int, default=123): seed for random number generator for the whitenoise in scan
-    """
-
-    def __init__(
-        self,
-        num_beams,
-        fov,
-        std_dev=0.01,
-        eps=0.0001,
-        theta_dis=2000,
-        max_range=30.0,
-        seed=123,
-    ):
-        # initialization
-        self.num_beams = num_beams
-        self.fov = fov
-        self.std_dev = std_dev
-        self.eps = eps
-        self.theta_dis = theta_dis
-        self.max_range = max_range
-        self.angle_increment = self.fov / (self.num_beams - 1)
-        self.theta_index_increment = theta_dis * self.angle_increment / (2.0 * np.pi)
-        self.orig_c = None
-        self.orig_s = None
-        self.orig_x = None
-        self.orig_y = None
-        self.map_height = None
-        self.map_width = None
-        self.map_resolution = None
-        self.dt = None
-
-        # white noise generator
-        self.rng = np.random.default_rng(seed=seed)
-
-        # precomputing corresponding cosines and sines of the angle array
-        theta_arr = np.linspace(0.0, 2 * np.pi, num=theta_dis)
-        self.sines = np.sin(theta_arr)
-        self.cosines = np.cos(theta_arr)
-
-    def set_map(self, map_path, map_ext):
-        """
-        Set the bitmap of the scan simulator by path
-
-            Args:
-                map_path (str): path to the map yaml file
-                map_ext (str): extension (image type) of the map image
-
-            Returns:
-                flag (bool): if image reading and loading is successful
-        """
-        # TODO: do we open the option to flip the images, and turn rgb into grayscale? or specify the exact requirements in documentation.
-        # TODO: throw error if image specification isn't met
-
-        # load map image
-        map_img_path = os.path.splitext(map_path)[0] + map_ext
-        self.map_img = np.array(
-            Image.open(map_img_path).transpose(Image.FLIP_TOP_BOTTOM)
-        )
-        self.map_img = self.map_img.astype(np.float64)
-
-        # grayscale -> binary
-        self.map_img[self.map_img <= 128.0] = 0.0
-        self.map_img[self.map_img > 128.0] = 255.0
-
-        self.map_height = self.map_img.shape[0]
-        self.map_width = self.map_img.shape[1]
-
-        # load map yaml
-        with open(map_path, "r") as yaml_stream:
-            try:
-                map_metadata = yaml.safe_load(yaml_stream)
-                self.map_resolution = map_metadata["resolution"]
-                self.origin = map_metadata["origin"]
-            except yaml.YAMLError as ex:
-                print(ex)
-
-        # calculate map parameters
-        self.orig_x = self.origin[0]
-        self.orig_y = self.origin[1]
-        self.orig_s = np.sin(self.origin[2])
-        self.orig_c = np.cos(self.origin[2])
-
-        # get the distance transform
-        self.dt = get_dt(self.map_img, self.map_resolution)
-
-        return True
-
-    def scan(self, pose):
-        """
-        Perform simulated 2D scan by pose on the given map
-
-            Args:
-                pose (numpy.ndarray (3, )): pose of the scan frame (x, y, theta)
-
-            Returns:
-                scan (numpy.ndarray (n, )): data array of the laserscan, n=num_beams
-
-            Raises:
-                ValueError: when scan is called before a map is set
-        """
-        if self.map_height is None:
-            raise ValueError("Map is not set for scan simulator.")
-        scan = get_scan(
-            pose,
-            self.theta_dis,
-            self.fov,
-            self.num_beams,
-            self.theta_index_increment,
-            self.sines,
-            self.cosines,
-            self.eps,
-            self.orig_x,
-            self.orig_y,
-            self.orig_c,
-            self.orig_s,
-            self.map_height,
-            self.map_width,
-            self.map_resolution,
-            self.dt,
-            self.max_range,
-        )
-        noise = self.rng.normal(0.0, self.std_dev, size=self.num_beams)
-        final_scan = scan + noise
-        return final_scan
-
-    def get_increment(self):
-        return self.angle_increment
-
-
 """
-Unit tests for the 2D scan simulator class
+Unit test for the 2D scan simulator class
 Author: Hongrui Zheng
 
 Test cases:
-    1, 2: Comparison between generated scan array of the new simulator and the legacy C++ simulator, generated data used, MSE is used as the metric
+    1, 2: Comparison between generated scan array of the new simulator and the legacy C++ simulator, 
+    generated data used, MSE is used as the metric
     2. FPS test, should be greater than 500
 """
 
@@ -415,18 +278,14 @@ def setUp(self):
         self.skirk_scan = sample_scan["skirk"]
 
     def test_map_berlin(self):
+        scan_rng = np.random.default_rng(seed=12345)
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
         new_berlin = np.empty((self.num_test, self.num_beams))
-        map_path = (
-            os.path.dirname(os.path.abspath(__file__))
-            + "/../maps/Berlin/Berlin_map.yaml"
-        )
-        map_ext = ".png"
-        scan_sim.set_map(map_path, map_ext)
+        scan_sim.set_map(map_name="Berlin")
         # scan gen loop
         for i in range(self.num_test):
             test_pose = self.test_poses[i]
-            new_berlin[i, :] = scan_sim.scan(test_pose)
+            new_berlin[i, :] = scan_sim.scan(pose=test_pose, rng=scan_rng)
         diff = self.berlin_scan - new_berlin
         mse = np.mean(diff**2)
         # print('Levine distance test, norm: ' + str(norm))
@@ -442,18 +301,15 @@ def test_map_berlin(self):
         self.assertLess(mse, 2.0)
 
     def test_map_skirk(self):
+        scan_rng = np.random.default_rng(seed=12345)
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
         new_skirk = np.empty((self.num_test, self.num_beams))
-        map_path = (
-            os.path.dirname(os.path.abspath(__file__)) + "/../maps/Skirk/Skirk_map.yaml"
-        )
-        map_ext = ".png"
-        scan_sim.set_map(map_path, map_ext)
+        scan_sim.set_map(map_name="Skirk")
         print("map set")
         # scan gen loop
         for i in range(self.num_test):
             test_pose = self.test_poses[i]
-            new_skirk[i, :] = scan_sim.scan(test_pose)
+            new_skirk[i, :] = scan_sim.scan(pose=test_pose, rng=scan_rng)
         diff = self.skirk_scan - new_skirk
         mse = np.mean(diff**2)
         print("skirk distance test, mse: " + str(mse))
@@ -470,23 +326,19 @@ def test_map_skirk(self):
 
     def test_fps(self):
         # scan fps should be greater than 500
+        scan_rng = np.random.default_rng(seed=12345)
         scan_sim = ScanSimulator2D(self.num_beams, self.fov)
-        map_path = (
-            os.path.dirname(os.path.abspath(__file__)) + "/../maps/Skirk/Skirk_map.yaml"
-        )
-        map_ext = ".png"
-        scan_sim.set_map(map_path, map_ext)
+        scan_sim.set_map(map_name="Skirk")
 
         import time
 
         start = time.time()
         for i in range(10000):
             x_test = i / 10000
-            scan = scan_sim.scan(np.array([x_test, 0.0, 0.0]))
+            scan = scan_sim.scan(pose=np.array([x_test, 0.0, 0.0]), rng=scan_rng)
         end = time.time()
         fps = 10000 / (end - start)
-        # print('FPS test')
-        # print('Elapsed time: ' + str(end-start) + ' , FPS: ' + str(1/fps))
+
         self.assertGreater(fps, 500.0)
 
 
@@ -494,10 +346,9 @@ def main():
     num_beams = 1080
     fov = 4.7
     # map_path = '../envs/maps/Berlin_map.yaml'
-    map_path = "/home/f1tenth-eval/tunercar/es/maps/map0.yaml"
-    map_ext = ".png"
+    scan_rng = np.random.default_rng(seed=12345)
     scan_sim = ScanSimulator2D(num_beams, fov)
-    scan_sim.set_map(map_path, map_ext)
+    scan_sim.set_map(map_name="Example")
     scan = scan_sim.scan(np.array([0.0, 0.0, 0.0]))
 
     # fps test
@@ -506,7 +357,7 @@ def main():
     start = time.time()
     for i in range(10000):
         x_test = i / 10000
-        scan = scan_sim.scan(np.array([x_test, 0.0, 0.0]))
+        scan = scan_sim.scan(np.array([x_test, 0.0, 0.0]), rng=scan_rng)
     end = time.time()
     fps = (end - start) / 10000
     print("FPS test")
@@ -537,5 +388,5 @@ def update(i):
 
 
 if __name__ == "__main__":
-    # unittest.main()
+    # test.main()
     main()
diff --git a/gym/f110_gym/unittest/track_test.py b/gym/f110_gym/test/test_track.py
similarity index 89%
rename from gym/f110_gym/unittest/track_test.py
rename to gym/f110_gym/test/test_track.py
index 2ed23a50..06c66d58 100644
--- a/gym/f110_gym/unittest/track_test.py
+++ b/gym/f110_gym/test/test_track.py
@@ -112,22 +112,36 @@ def test_download_racetrack(self):
         track = Track.from_track_name(track_name)
 
         # check the two tracks' specs are the same
-        for spec_attr in ["name", "image", "resolution", "origin", "negate", "occupied_thresh", "free_thresh"]:
+        for spec_attr in [
+            "name",
+            "image",
+            "resolution",
+            "origin",
+            "negate",
+            "occupied_thresh",
+            "free_thresh",
+        ]:
             self.assertEqual(
                 getattr(track.spec, spec_attr), getattr(track_backup.spec, spec_attr)
             )
 
         # check the two tracks' racelines are the same
         for raceline_attr in ["ss", "xs", "ys", "yaws", "ks", "vxs", "axs"]:
-            self.assertTrue(np.isclose(
-                getattr(track.raceline, raceline_attr), getattr(track_backup.raceline, raceline_attr)
-            ).all())
+            self.assertTrue(
+                np.isclose(
+                    getattr(track.raceline, raceline_attr),
+                    getattr(track_backup.raceline, raceline_attr),
+                ).all()
+            )
 
         # check the two tracks' centerlines are the same
         for centerline_attr in ["ss", "xs", "ys", "yaws", "ks", "vxs", "axs"]:
-            self.assertTrue(np.isclose(
-                getattr(track.centerline, centerline_attr), getattr(track_backup.centerline, centerline_attr)
-            ).all())
+            self.assertTrue(
+                np.isclose(
+                    getattr(track.centerline, centerline_attr),
+                    getattr(track_backup.centerline, centerline_attr),
+                ).all()
+            )
 
         # remove the newly created track dir
         track_dir = find_track_dir(track_name)
@@ -136,4 +150,3 @@ def test_download_racetrack(self):
         # rename the backup track dir to its original name
         track_backup_dir = find_track_dir(tmp_dir.stem)
         track_backup_dir.rename(track_dir)
-
diff --git a/gym/f110_gym/unittest/utils_test.py b/gym/f110_gym/test/test_utils.py
similarity index 100%
rename from gym/f110_gym/unittest/utils_test.py
rename to gym/f110_gym/test/test_utils.py
diff --git a/gym/f110_gym/unittest/pyglet_test_camera.py b/gym/f110_gym/unittest/pyglet_test_camera.py
deleted file mode 100644
index 46ba26a3..00000000
--- a/gym/f110_gym/unittest/pyglet_test_camera.py
+++ /dev/null
@@ -1,131 +0,0 @@
-import pyglet
-from pyglet.gl import *
-
-# Zooming constants
-ZOOM_IN_FACTOR = 1.2
-ZOOM_OUT_FACTOR = 1/ZOOM_IN_FACTOR
-
-class App(pyglet.window.Window):
-
-    def __init__(self, width, height, *args, **kwargs):
-        conf = Config(sample_buffers=1,
-                      samples=4,
-                      depth_size=16,
-                      double_buffer=True)
-        super().__init__(width, height, config=conf, *args, **kwargs)
-
-        #Initialize camera values
-        self.left   = 0
-        self.right  = width
-        self.bottom = 0
-        self.top    = height
-        self.zoom_level = 1
-        self.zoomed_width  = width
-        self.zoomed_height = height
-
-    def init_gl(self, width, height):
-        # Set clear color
-        glClearColor(0/255, 0/255, 0/255, 0/255)
-
-        # Set antialiasing
-        glEnable( GL_LINE_SMOOTH )
-        glEnable( GL_POLYGON_SMOOTH )
-        glHint( GL_LINE_SMOOTH_HINT, GL_NICEST )
-
-        # Set alpha blending
-        glEnable( GL_BLEND )
-        glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA )
-
-        # Set viewport
-        glViewport( 0, 0, width, height )
-
-    def on_resize(self, width, height):
-        super().on_resize(width, height)
-        size = self.get_size()
-        self.left = 0
-        self.right = size[0]
-        self.bottom = 0
-        self.top = size[1]
-        self.zoomed_width = size[0]
-        self.zoomed_height = size[1]
-        
-        # # Set window values
-        # self.width  = width
-        # self.height = height
-        # # Initialize OpenGL context
-        # self.init_gl(width, height)
-        # self.width = width
-        # self.height = height
-        # pass
-
-    def on_mouse_drag(self, x, y, dx, dy, buttons, modifiers):
-        # Move camera
-        self.left   -= dx*self.zoom_level
-        self.right  -= dx*self.zoom_level
-        self.bottom -= dy*self.zoom_level
-        self.top    -= dy*self.zoom_level
-
-    def on_mouse_scroll(self, x, y, dx, dy):
-        # Get scale factor
-        f = ZOOM_IN_FACTOR if dy > 0 else ZOOM_OUT_FACTOR if dy < 0 else 1
-        # If zoom_level is in the proper range
-        if .2 < self.zoom_level*f < 5:
-
-            self.zoom_level *= f
-
-            size = self.get_size()
-
-            mouse_x = x/size[0]
-            mouse_y = y/size[1]
-
-            mouse_x_in_world = self.left   + mouse_x*self.zoomed_width
-            mouse_y_in_world = self.bottom + mouse_y*self.zoomed_height
-
-            self.zoomed_width  *= f
-            self.zoomed_height *= f
-
-            self.left   = mouse_x_in_world - mouse_x*self.zoomed_width
-            self.right  = mouse_x_in_world + (1 - mouse_x)*self.zoomed_width
-            self.bottom = mouse_y_in_world - mouse_y*self.zoomed_height
-            self.top    = mouse_y_in_world + (1 - mouse_y)*self.zoomed_height
-
-    def on_draw(self):
-        # Initialize Projection matrix
-        glMatrixMode( GL_PROJECTION )
-        glLoadIdentity()
-
-        # Initialize Modelview matrix
-        glMatrixMode( GL_MODELVIEW )
-        glLoadIdentity()
-        # Save the default modelview matrix
-        glPushMatrix()
-
-        # Clear window with ClearColor
-        glClear( GL_COLOR_BUFFER_BIT )
-
-        # Set orthographic projection matrix
-        glOrtho( self.left, self.right, self.bottom, self.top, 1, -1 )
-
-        # Draw quad
-        glBegin( GL_QUADS )
-        glColor3ub( 0xFF, 0, 0 )
-        glVertex2i( 10, 10 )
-
-        glColor3ub( 0xFF, 0xFF, 0 )
-        glVertex2i( 110, 10 )
-
-        glColor3ub( 0, 0xFF, 0 )
-        glVertex2i( 110, 110 )
-
-        glColor3ub( 0, 0, 0xFF )
-        glVertex2i( 10, 110 )
-        glEnd()
-
-        # Remove default modelview matrix
-        glPopMatrix()
-
-    def run(self):
-        pyglet.app.run()
-
-
-App(800, 800, resizable=True).run()
\ No newline at end of file
diff --git a/setup.py b/setup.py
index a87457ea..85b33204 100644
--- a/setup.py
+++ b/setup.py
@@ -1,21 +1,22 @@
 from setuptools import setup
 
-setup(name='f110_gym',
-      version='0.2.1',
-      author='Hongrui Zheng',
-      author_email='billyzheng.bz@gmail.com',
-      url='https://f1tenth.org',
-      package_dir={'': 'gym'},
-      install_requires=[
-          'gymnasium',
-          'numpy<=1.22.0,>=1.18.0',
-          'Pillow>=9.0.1',
-          'scipy>=1.7.3',
-          'numba>=0.55.2',
-          'pyyaml>=5.3.1',
-          'pyglet<1.5',
-          'pyopengl',
-          'yamldataclassconfig',
-          'requests'
-      ]
-      )
+setup(
+    name="f110_gym",
+    version="0.2.1",
+    author="Hongrui Zheng",
+    author_email="billyzheng.bz@gmail.com",
+    url="https://f1tenth.org",
+    package_dir={"": "gym"},
+    install_requires=[
+        "gymnasium",
+        "numpy<=1.22.0,>=1.18.0",
+        "Pillow>=9.0.1",
+        "scipy>=1.7.3",
+        "numba>=0.55.2",
+        "pyyaml>=5.3.1",
+        "pyglet<1.5",
+        "pyopengl",
+        "yamldataclassconfig",
+        "requests",
+    ],
+)

From 364334e1eb5da97b72a73658c2715bd788047454 Mon Sep 17 00:00:00 2001
From: luigiberducci <luigi.berd@gmail.com>
Date: Wed, 12 Jul 2023 23:42:21 +0200
Subject: [PATCH 42/42] refactoring random track generator, move it to examples
 dir

---
 .gitignore                               |   3 +-
 examples/random_trackgen.py              | 260 +++++++++++++++++++++++
 gym/f110_gym/unittest/random_trackgen.py | 234 --------------------
 3 files changed, 261 insertions(+), 236 deletions(-)
 create mode 100644 examples/random_trackgen.py
 delete mode 100644 gym/f110_gym/unittest/random_trackgen.py

diff --git a/.gitignore b/.gitignore
index 94b5a670..8a0205b3 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,8 +4,6 @@ __pycache__
 *.pyc
 .DS_Store
 .vscode/
-gym/f110_gym/unittest/maps/
-gym/f110_gym/unittest/centerline/
 docs/_build/
 *.mypy_cache/
 flycheck_*
@@ -173,3 +171,4 @@ cython_debug/
 *.zip
 
 /gym/f110_gym/maps/*
+/examples/maps
diff --git a/examples/random_trackgen.py b/examples/random_trackgen.py
new file mode 100644
index 00000000..2130e9c4
--- /dev/null
+++ b/examples/random_trackgen.py
@@ -0,0 +1,260 @@
+# MIT License
+
+# Copyright (c) 2020 Joseph Auckley, Matthew O'Kelly, Aman Sinha, Hongrui Zheng
+
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+
+"""
+Generates random tracks.
+Adapted from https://gym.openai.com/envs/CarRacing-v0
+Author: Hongrui Zheng
+
+Note: additional requirements
+    - shapely
+    - opencv-python
+"""
+import pathlib
+
+import cv2
+import math
+import numpy as np
+import shapely.geometry as shp
+import matplotlib.pyplot as plt
+
+
+def main(args):
+    n_maps = args.n_maps
+    outdir = args.outdir
+    seed = args.seed
+
+    np.random.seed(seed)
+
+    outdir.mkdir(parents=True, exist_ok=True)
+
+    for i in range(n_maps):
+        try:
+            print(f"[info] creating track {i}")
+            track, track_int, track_ext = create_track()
+            convert_track(track, track_int, track_ext, i, outdir)
+            print(f"[info] saved track {i} in {outdir}/")
+        except:
+            print("[error] failed to create track. Retrying...")
+            continue
+        print()
+
+
+def create_track():
+    CHECKPOINTS = 16
+    SCALE = 6.0
+    TRACK_RAD = 900 / SCALE
+    TRACK_DETAIL_STEP = 21 / SCALE
+    TRACK_TURN_RATE = 0.31
+    WIDTH = 10.0
+
+    start_alpha = 0.0
+
+    # Create checkpoints
+    checkpoints = []
+    for c in range(CHECKPOINTS):
+        alpha = 2 * math.pi * c / CHECKPOINTS + np.random.uniform(
+            0, 2 * math.pi * 1 / CHECKPOINTS
+        )
+        rad = np.random.uniform(TRACK_RAD / 3, TRACK_RAD)
+        if c == 0:
+            alpha = 0
+            rad = 1.5 * TRACK_RAD
+        if c == CHECKPOINTS - 1:
+            alpha = 2 * math.pi * c / CHECKPOINTS
+            start_alpha = 2 * math.pi * (-0.5) / CHECKPOINTS
+            rad = 1.5 * TRACK_RAD
+        checkpoints.append((alpha, rad * math.cos(alpha), rad * math.sin(alpha)))
+
+    # Go from one checkpoint to another to create track
+    x, y, beta = 1.5 * TRACK_RAD, 0, 0
+    dest_i = 0
+    laps = 0
+    track = []
+    no_freeze = 2500
+    visited_other_side = False
+    while True:
+        alpha = math.atan2(y, x)
+        if visited_other_side and alpha > 0:
+            laps += 1
+            visited_other_side = False
+        if alpha < 0:
+            visited_other_side = True
+            alpha += 2 * math.pi
+        while True:
+            failed = True
+            while True:
+                dest_alpha, dest_x, dest_y = checkpoints[dest_i % len(checkpoints)]
+                if alpha <= dest_alpha:
+                    failed = False
+                    break
+                dest_i += 1
+                if dest_i % len(checkpoints) == 0:
+                    break
+            if not failed:
+                break
+            alpha -= 2 * math.pi
+            continue
+        r1x = math.cos(beta)
+        r1y = math.sin(beta)
+        p1x = -r1y
+        p1y = r1x
+        dest_dx = dest_x - x
+        dest_dy = dest_y - y
+        proj = r1x * dest_dx + r1y * dest_dy
+        while beta - alpha > 1.5 * math.pi:
+            beta -= 2 * math.pi
+        while beta - alpha < -1.5 * math.pi:
+            beta += 2 * math.pi
+        prev_beta = beta
+        proj *= SCALE
+        if proj > 0.3:
+            beta -= min(TRACK_TURN_RATE, abs(0.001 * proj))
+        if proj < -0.3:
+            beta += min(TRACK_TURN_RATE, abs(0.001 * proj))
+        x += p1x * TRACK_DETAIL_STEP
+        y += p1y * TRACK_DETAIL_STEP
+        track.append((alpha, prev_beta * 0.5 + beta * 0.5, x, y))
+        if laps > 4:
+            break
+        no_freeze -= 1
+        if no_freeze == 0:
+            break
+
+    # Find closed loop
+    i1, i2 = -1, -1
+    i = len(track)
+    while True:
+        i -= 1
+        if i == 0:
+            return False
+        pass_through_start = (
+            track[i][0] > start_alpha and track[i - 1][0] <= start_alpha
+        )
+        if pass_through_start and i2 == -1:
+            i2 = i
+        elif pass_through_start and i1 == -1:
+            i1 = i
+            break
+
+    print("[info] track generation: %i..%i -> %i-tiles track" % (i1, i2, i2 - i1))
+
+    assert i1 != -1
+    assert i2 != -1
+
+    track = track[i1 : i2 - 1]
+    first_beta = track[0][1]
+    first_perp_x = math.cos(first_beta)
+    first_perp_y = math.sin(first_beta)
+
+    # Length of perpendicular jump to put together head and tail
+    well_glued_together = np.sqrt(
+        np.square(first_perp_x * (track[0][2] - track[-1][2]))
+        + np.square(first_perp_y * (track[0][3] - track[-1][3]))
+    )
+    if well_glued_together > TRACK_DETAIL_STEP:
+        return False
+
+    # Converting to numpy array
+    track_xy = [(x, y) for (a1, b1, x, y) in track]
+    track_xy = np.asarray(track_xy)
+    track_poly = shp.Polygon(track_xy)
+
+    # Finding interior and exterior walls
+    track_xy_offset_in = track_poly.buffer(WIDTH)
+    track_xy_offset_out = track_poly.buffer(-WIDTH)
+    track_xy_offset_in_np = np.array(track_xy_offset_in.exterior.xy).T
+    track_xy_offset_out_np = np.array(track_xy_offset_out.exterior.xy).T
+
+    return track_xy, track_xy_offset_in_np, track_xy_offset_out_np
+
+
+def convert_track(track, track_int, track_ext, track_id, outdir):
+    # converts track to image and saves the centerline as waypoints
+    fig, ax = plt.subplots()
+    fig.set_size_inches(20, 20)
+    ax.plot(track_int[:, 0], track_int[:, 1], color="black", linewidth=3)
+    ax.plot(track_ext[:, 0], track_ext[:, 1], color="black", linewidth=3)
+    plt.tight_layout()
+    ax.set_aspect("equal")
+    ax.set_xlim(-180, 300)
+    ax.set_ylim(-300, 300)
+    plt.axis("off")
+
+    track_filepath = outdir / f"map{track_id}_map.png"
+    plt.savefig(track_filepath, dpi=80)
+    plt.close()
+
+    map_width, map_height = fig.canvas.get_width_height()
+    print("[info] map image size: ", map_width, map_height)
+
+    # Transform the track center line into pixel coordinates
+    xy_pixels = ax.transData.transform(track)
+    origin_x_pix = xy_pixels[0, 0]
+    origin_y_pix = xy_pixels[0, 1]
+
+    xy_pixels = xy_pixels - np.array([[origin_x_pix, origin_y_pix]])
+
+    map_origin_x = -origin_x_pix * 0.05
+    map_origin_y = -origin_y_pix * 0.05
+
+    # Convert image using cv2
+    cv_img = cv2.imread(str(track_filepath), -1)
+    cv_img_bw = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
+    # Save image
+    cv2.imwrite(str(track_filepath), cv_img_bw)
+    cv2.imwrite(str(track_filepath.with_suffix(".pgm")), cv_img_bw)
+
+    # create yaml file
+    with open(track_filepath.with_suffix(".yaml"), "w") as yaml:
+        yaml.write(f"image: map{track_id}.pgm\n")
+        yaml.write("resolution: 0.062500\n")
+        yaml.write(f"origin: [{map_origin_x},{map_origin_y},0.000000]\n")
+        yaml.write("negate: 0\n")
+        yaml.write("occupied_thresh: 0.45\n")
+        yaml.write("free_thresh: 0.196")
+
+    # Saving centerline as a csv
+    centerline_filepath = outdir / f"map{track_id}_centerline.csv"
+    with open(centerline_filepath, "w") as waypoints_csv:
+        waypoints_csv.write("#x,y\n")
+        for row in xy_pixels:
+            waypoints_csv.write(f"{0.05 * row[0]}, {0.05 * row[1]}\n")
+
+
+if __name__ == "__main__":
+    import argparse
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--seed", type=int, default=123, help="The seed for the numpy rng"
+    )
+    parser.add_argument(
+        "--n_maps", type=int, default=3, help="Number of maps to create"
+    )
+    parser.add_argument(
+        "--outdir", type=pathlib.Path, default="./maps", help="Out directory"
+    )
+    args = parser.parse_args()
+
+    main(args)
diff --git a/gym/f110_gym/unittest/random_trackgen.py b/gym/f110_gym/unittest/random_trackgen.py
deleted file mode 100644
index cb88e3f4..00000000
--- a/gym/f110_gym/unittest/random_trackgen.py
+++ /dev/null
@@ -1,234 +0,0 @@
-# MIT License
-
-# Copyright (c) 2020 Joseph Auckley, Matthew O'Kelly, Aman Sinha, Hongrui Zheng
-
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-# SOFTWARE.
-
-
-"""
-Generates random tracks.
-Adapted from https://gym.openai.com/envs/CarRacing-v0
-Author: Hongrui Zheng  
-"""
-
-import cv2
-import os
-import math
-import numpy as np
-import shapely.geometry as shp
-import matplotlib.pyplot as plt
-from matplotlib.patches import Polygon
-from matplotlib.collections import PatchCollection
-import argparse
-
-parser = argparse.ArgumentParser()
-parser.add_argument('--seed', type=int, default=123, help='Seed for the numpy rng.')
-parser.add_argument('--num_maps', type=int, default=1, help='Number of maps to generate.')
-args = parser.parse_args()
-
-np.random.seed(args.seed)
-
-if not os.path.exists('maps'):
-    print('Creating maps/ directory.')
-    os.makedirs('maps')
-if not os.path.exists('centerline'):
-    print('Creating centerline/ directory.')
-    os.makedirs('centerline')
-
-NUM_MAPS = args.num_maps
-WIDTH = 10.0
-def create_track():
-    CHECKPOINTS = 16
-    SCALE = 6.0
-    TRACK_RAD = 900/SCALE
-    TRACK_DETAIL_STEP = 21/SCALE
-    TRACK_TURN_RATE = 0.31
-
-    start_alpha = 0.
-
-    # Create checkpoints
-    checkpoints = []
-    for c in range(CHECKPOINTS):
-        alpha = 2*math.pi*c/CHECKPOINTS + np.random.uniform(0, 2*math.pi*1/CHECKPOINTS)
-        rad = np.random.uniform(TRACK_RAD/3, TRACK_RAD)
-        if c==0:
-            alpha = 0
-            rad = 1.5*TRACK_RAD
-        if c==CHECKPOINTS-1:
-            alpha = 2*math.pi*c/CHECKPOINTS
-            start_alpha = 2*math.pi*(-0.5)/CHECKPOINTS
-            rad = 1.5*TRACK_RAD
-        checkpoints.append( (alpha, rad*math.cos(alpha), rad*math.sin(alpha)) )
-    road = []
-
-    # Go from one checkpoint to another to create track
-    x, y, beta = 1.5*TRACK_RAD, 0, 0
-    dest_i = 0
-    laps = 0
-    track = []
-    no_freeze = 2500
-    visited_other_side = False
-    while True:
-        alpha = math.atan2(y, x)
-        if visited_other_side and alpha > 0:
-            laps += 1
-            visited_other_side = False
-        if alpha < 0:
-            visited_other_side = True
-            alpha += 2*math.pi
-        while True:
-            failed = True
-            while True:
-                dest_alpha, dest_x, dest_y = checkpoints[dest_i % len(checkpoints)]
-                if alpha <= dest_alpha:
-                    failed = False
-                    break
-                dest_i += 1
-                if dest_i % len(checkpoints) == 0:
-                    break
-            if not failed:
-                break
-            alpha -= 2*math.pi
-            continue
-        r1x = math.cos(beta)
-        r1y = math.sin(beta)
-        p1x = -r1y
-        p1y = r1x
-        dest_dx = dest_x - x
-        dest_dy = dest_y - y
-        proj = r1x*dest_dx + r1y*dest_dy
-        while beta - alpha >  1.5*math.pi:
-             beta -= 2*math.pi
-        while beta - alpha < -1.5*math.pi:
-             beta += 2*math.pi
-        prev_beta = beta
-        proj *= SCALE
-        if proj >  0.3:
-             beta -= min(TRACK_TURN_RATE, abs(0.001*proj))
-        if proj < -0.3:
-             beta += min(TRACK_TURN_RATE, abs(0.001*proj))
-        x += p1x*TRACK_DETAIL_STEP
-        y += p1y*TRACK_DETAIL_STEP
-        track.append( (alpha,prev_beta*0.5 + beta*0.5,x,y) )
-        if laps > 4:
-             break
-        no_freeze -= 1
-        if no_freeze==0:
-             break
-
-    # Find closed loop
-    i1, i2 = -1, -1
-    i = len(track)
-    while True:
-        i -= 1
-        if i==0:
-            return False
-        pass_through_start = track[i][0] > start_alpha and track[i-1][0] <= start_alpha
-        if pass_through_start and i2==-1:
-            i2 = i
-        elif pass_through_start and i1==-1:
-            i1 = i
-            break
-    print("Track generation: %i..%i -> %i-tiles track" % (i1, i2, i2-i1))
-    assert i1!=-1
-    assert i2!=-1
-
-    track = track[i1:i2-1]
-    first_beta = track[0][1]
-    first_perp_x = math.cos(first_beta)
-    first_perp_y = math.sin(first_beta)
-
-    # Length of perpendicular jump to put together head and tail
-    well_glued_together = np.sqrt(
-        np.square( first_perp_x*(track[0][2] - track[-1][2]) ) +
-        np.square( first_perp_y*(track[0][3] - track[-1][3]) ))
-    if well_glued_together > TRACK_DETAIL_STEP:
-        return False
-
-    # post processing, converting to numpy, finding exterior and interior walls
-    track_xy = [(x, y) for (a1, b1, x, y) in track]
-    track_xy = np.asarray(track_xy)
-    track_poly = shp.Polygon(track_xy)
-    track_xy_offset_in = track_poly.buffer(WIDTH)
-    track_xy_offset_out = track_poly.buffer(-WIDTH)
-    track_xy_offset_in_np = np.array(track_xy_offset_in.exterior)
-    track_xy_offset_out_np = np.array(track_xy_offset_out.exterior)
-    return track_xy, track_xy_offset_in_np, track_xy_offset_out_np
-
-
-def convert_track(track, track_int, track_ext, iter):
-
-    # converts track to image and saves the centerline as waypoints
-    fig, ax = plt.subplots()
-    fig.set_size_inches(20, 20)
-    ax.plot(*track_int.T, color='black', linewidth=3)
-    ax.plot(*track_ext.T, color='black', linewidth=3)
-    plt.tight_layout()
-    ax.set_aspect('equal')
-    ax.set_xlim(-180, 300)
-    ax.set_ylim(-300, 300)
-    plt.axis('off')
-    plt.savefig('maps/map' + str(iter) + '.png', dpi=80)
-
-    map_width, map_height = fig.canvas.get_width_height()
-    print('map size: ', map_width, map_height)
-
-    # transform the track center line into pixel coordinates
-    xy_pixels = ax.transData.transform(track)
-    origin_x_pix = xy_pixels[0, 0]
-    origin_y_pix = xy_pixels[0, 1]
-
-    xy_pixels = xy_pixels - np.array([[origin_x_pix, origin_y_pix]])
-
-    map_origin_x = -origin_x_pix*0.05
-    map_origin_y = -origin_y_pix*0.05
-
-    # convert image using cv2
-    cv_img = cv2.imread('maps/map' + str(iter) + '.png', -1)
-    # convert to bw
-    cv_img_bw = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
-    # saving to img
-    cv2.imwrite('maps/map' + str(iter) + '.png', cv_img_bw)
-    cv2.imwrite('maps/map' + str(iter) + '.pgm', cv_img_bw)
-
-    # create yaml file
-    yaml = open('maps/map' + str(iter) + '.yaml', 'w')
-    yaml.write('image: map' + str(iter) + '.pgm\n')
-    yaml.write('resolution: 0.062500\n')
-    yaml.write('origin: [' + str(map_origin_x) + ',' + str(map_origin_y) + ', 0.000000]\n')
-    yaml.write('negate: 0\noccupied_thresh: 0.45\nfree_thresh: 0.196')
-    yaml.close()
-    plt.close()
-
-    # saving track centerline as a csv in ros coords
-    waypoints_csv = open('centerline/map' + str(iter) + '.csv', 'w')
-    for row in xy_pixels:
-        waypoints_csv.write(str(0.05*row[0]) + ', ' + str(0.05*row[1]) + '\n')
-    waypoints_csv.close()
-
-
-
-if __name__ == '__main__':
-    for i in range(NUM_MAPS):
-        try:
-            track, track_int, track_ext = create_track()
-        except:
-            print('Random generator failed, retrying')
-            continue
-        convert_track(track, track_int, track_ext, i)
\ No newline at end of file