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