diff --git a/gym/f110_gym/envs/collision_models.py b/gym/f110_gym/envs/collision_models.py index 8961cdc8..87547ab6 100644 --- a/gym/f110_gym/envs/collision_models.py +++ b/gym/f110_gym/envs/collision_models.py @@ -271,89 +271,3 @@ def get_vertices(pose, length, width): [[rl[0], rl[1]], [rr[0], rr[1]], [fr[0], fr[1]], [fl[0], fl[1]]] ) return vertices - - -""" -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.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, :])) - import matplotlib.pyplot as plt - - 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.show() - self.assertTrue(vertices.shape == (4, 2)) - - def test_get_vert_fps(self): - test_pose = np.array([2.3, 6.7, 0.8]) - start = time.time() - 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) - - 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.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.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.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.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) - - -if __name__ == "__main__": - unittest.main()