def test_exception_if_contact_buffer_always_full(self): max_attempts_per_prop = 2 radius = 0.1 num_spheres = 5 physics, spheres = _make_spheres(num_spheres=num_spheres, radius=radius, nconmax=1) candidate_positions = np.multiply.outer( np.arange(num_spheres * max_attempts_per_prop), [radius * 2.01, 0, 0]) # If we only place the first sphere then the others will all be overlapping # at the origin, so we get an error due to filling the contact buffer. prop_placer_failure = prop_initializer.PropPlacer( props=[spheres[0]], position=deterministic.Sequence(candidate_positions), ignore_collisions=False, max_attempts_per_prop=max_attempts_per_prop) with self.assertRaises(control.PhysicsError): prop_placer_failure(physics, random_state=np.random.RandomState(0)) physics, spheres = _make_spheres(num_spheres=num_spheres, radius=radius, nconmax=1) # If we place all of the spheres then we can find a configuration where they # are non-colliding, so the contact buffer is not full when the initializer # returns. prop_placer = prop_initializer.PropPlacer( props=spheres, position=deterministic.Sequence(candidate_positions), ignore_collisions=False, max_attempts_per_prop=max_attempts_per_prop) prop_placer(physics, random_state=np.random.RandomState(0))
def build_placer(): return prop_initializer.PropPlacer( props=spheres[:1], position=positions_dist, settle_physics=True, max_settle_physics_time=1e-6, # To ensure that settling FAILS. max_settle_physics_attempts=max_settle_physics_attempts)
def test_settle_physics(self, settle_physics): radius = 0.1 physics, spheres = _make_spheres(num_spheres=2, radius=radius, nconmax=1) # Only place the first sphere. prop_placer = prop_initializer.PropPlacer( props=spheres[:1], position=np.array([2.01 * radius, 0., 0.]), settle_physics=settle_physics) prop_placer(physics, random_state=np.random.RandomState(0)) first_position, first_quaternion = spheres[0].get_pose(physics) del first_quaternion # Unused. # If we allowed the physics to settle then the first sphere should be # resting on the ground, otherwise it should be at the target height. expected_first_z_pos = -radius if settle_physics else 0. self.assertAlmostEqual(first_position[2], expected_first_z_pos, places=3) second_position, second_quaternion = spheres[1].get_pose(physics) del second_quaternion # Unused. # The sphere that we were not placing should not have moved. self.assertEqual(second_position[2], 0.)
def test_ignore_contacts_with_entities(self): physics, spheres = _make_spheres(num_spheres=2, radius=0.01, nconmax=1000) # Target position of both spheres (non-colliding). fixed_positions = [(0, 0, 0.1), (0, 0.1, 0.1)] # Placer that initializes both spheres to (0, 0, 0.1), ignoring contacts. prop_placer_init = prop_initializer.PropPlacer( props=spheres, position=fixed_positions[0], ignore_collisions=True, max_attempts_per_prop=1) # Sequence of placers that will move the spheres to their target positions. prop_placer_seq = [] for prop, target_position in zip(spheres, fixed_positions): placer = prop_initializer.PropPlacer(props=[prop], position=target_position, ignore_collisions=False, max_attempts_per_prop=1) prop_placer_seq.append(placer) # We expect the first placer in the sequence to fail without # `ignore_contacts_with_entities` because the second sphere is already at # the same location. prop_placer_init(physics, random_state=np.random.RandomState(0)) expected_message = prop_initializer._REJECTION_SAMPLING_FAILED.format( model_name=spheres[0].mjcf_model.model, max_attempts=1) with self.assertRaisesWithLiteralMatch(RuntimeError, expected_message): prop_placer_seq[0](physics, random_state=np.random.RandomState(0)) # Placing the first sphere should succeed if we ignore contacts involving # the second sphere. prop_placer_init(physics, random_state=np.random.RandomState(0)) prop_placer_seq[0](physics, random_state=np.random.RandomState(0), ignore_contacts_with_entities=[spheres[1]]) # Now place the second sphere with all collisions active. prop_placer_seq[1](physics, random_state=np.random.RandomState(0), ignore_contacts_with_entities=None) self.assertNoContactsInvolvingEntities(physics, spheres)
def test_sample_non_colliding_positions(self): halfwidth = 0.05 radius = halfwidth / 4. offset = np.array([0, 0, halfwidth + radius * 1.1]) lower = -np.full(3, halfwidth) + offset upper = np.full(3, halfwidth) + offset position_variation = distributions.Uniform(lower, upper) physics, spheres = _make_spheres(num_spheres=8, radius=radius, nconmax=1000) prop_placer = prop_initializer.PropPlacer(props=spheres, position=position_variation, ignore_collisions=False, settle_physics=False) prop_placer(physics, random_state=np.random.RandomState(0)) self.assertNoContactsInvolvingEntities(physics, spheres) self.assertPositionsWithinBounds(physics, spheres, lower, upper)
def test_rejection_sampling_failure(self): max_attempts_per_prop = 2 fixed_position = (0, 0, 0.1) # Guaranteed to always have collisions. physics, spheres = _make_spheres(num_spheres=2, radius=0.01, nconmax=1000) prop_placer = prop_initializer.PropPlacer( props=spheres, position=fixed_position, ignore_collisions=False, max_attempts_per_prop=max_attempts_per_prop) expected_message = prop_initializer._REJECTION_SAMPLING_FAILED.format( model_name=spheres[1].mjcf_model. model, # Props are placed in order. max_attempts=max_attempts_per_prop) with self.assertRaisesWithLiteralMatch(RuntimeError, expected_message): prop_placer(physics, random_state=np.random.RandomState(0))
def test_no_exception_if_contact_buffer_transiently_full(self): max_attempts_per_prop = 2 radius = 0.1 num_spheres = 3 physics, spheres = _make_spheres(num_spheres=num_spheres, radius=radius, nconmax=1) fixed_positions = [[-radius * 1.01, 0., 0], [radius * 1.01, 0., 0.]] for sphere, position in zip(spheres[:2], fixed_positions): sphere.set_pose(physics, position=position) candidate_positions = [ [0., 0., 0.], # Collides with both fixed spheres. [5 * radius, 0., 0.] ] # Does not collide with either sphere. # The first candidate position transiently fills the contact buffer. prop_placer = prop_initializer.PropPlacer( props=spheres[2:], position=deterministic.Sequence(candidate_positions), ignore_collisions=False, max_attempts_per_prop=max_attempts_per_prop) prop_placer(physics, random_state=np.random.RandomState(0))