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