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

@@ -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()

gym/f110_gym/__init__.py

@@ -1,6 +1,6 @@
 import gymnasium as gym
 
 gym.register(
-	id='f110-v0',
-	entry_point='f110_gym.envs:F110Env',
-)
+    id="f110-v0",
+    entry_point="f110_gym.envs:F110Env",
+)

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]:

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

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,16 +153,16 @@ 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)
             continue
 
         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()
+if __name__ == "__main__":
+    unittest.main()