def test_reset_scenario(self):
main_sphere = sphere(mass=1, radius=0.5, name='sphere')
cc = CollisionChecker()
self.assertEqual(cc.n_fixed_models, 0)
self.assertEqual(cc.n_meshes, 0)
cc.add_model(main_sphere)
self.assertEqual(cc.n_fixed_models, 0)
self.assertEqual(cc.n_meshes, 1)
cc.reset_scenario()
self.assertEqual(cc.n_fixed_models, 0)
self.assertEqual(cc.n_meshes, 0)
cc.add_fixed_model(main_sphere)
self.assertEqual(cc.n_fixed_models, 1)
self.assertEqual(cc.n_meshes, 1)
cc.reset_scenario()
self.assertEqual(cc.n_fixed_models, 1)
self.assertEqual(cc.n_meshes, 0)
cc.reset_to_fixed_model_scenario()
self.assertEqual(cc.n_fixed_models, 1)
self.assertEqual(cc.n_meshes, 1)
# Try to add a ground plane model
main_sphere.is_ground_plane = True
cc.add_fixed_model(main_sphere)
self.assertEqual(cc.n_fixed_models, 1)
self.assertEqual(cc.n_meshes, 1)
def test_mesh_around_another(self):
main_sphere = sphere(mass=1, radius=0.1, name='sphere')
centers = np.random.uniform(0, 0.2, size=(20, 3))
cc = CollisionChecker()
cc.add_fixed_model(main_sphere)
self.assertEqual(cc.n_fixed_models, 1)
self.assertEqual(cc.n_meshes, 1)
for k in range(centers.shape[0]):
c = centers[k, :]
radius = np.random.uniform(0.5, 1.0)
model = sphere(mass=1,
radius=radius,
name='test',
pose=[i for i in c] + [0, 0, 0])
self.assertIsNotNone(model)
self.assertTrue(cc.check_collision_with_current_scene(model))
def test_singleton_collision_checker(self):
cc1 = SingletonCollisionChecker.get_instance()
self.assertIsNotNone(cc1)
cc2 = SingletonCollisionChecker.get_instance()
self.assertIsNotNone(cc2)
self.assertEqual(cc1.n_fixed_models, 0)
self.assertEqual(cc2.n_fixed_models, 0)
main_sphere = sphere(mass=1, radius=0.5, name='sphere')
cc1.add_fixed_model(main_sphere)
self.assertEqual(cc1.n_fixed_models, 1)
self.assertEqual(cc2.n_fixed_models, 1)
self.assertEqual(cc1.fixed_models[0].to_sdf(),
cc2.fixed_models[0].to_sdf())