def test_ignore_collision_not_involving_robot(self):
arena, arm, hand = self.make_model()
target_pos = np.array([0.1, 0.2, 0.3])
target_quat = np.array([0., 1., 1., 0.])
# Add two boxes that are always in contact with each other, but never with
# the arm or hand (since they are not within reach).
side_length = 0.1
x_offset = 10.
bottom_box = props.Primitive(geom_type='box',
size=[side_length] * 3,
pos=[x_offset, 0, 0])
top_box = props.Primitive(geom_type='box',
size=[side_length] * 3,
pos=[x_offset, 0, 2 * side_length])
arena.attach(bottom_box)
arena.add_free_entity(top_box)
initializer = tcp_initializer.ToolCenterPointInitializer(
hand=hand, arm=arm, position=target_pos, quaternion=target_quat)
physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model)
# Confirm that there are actually contacts between the two boxes.
self.assertEntitiesInContact(physics, bottom_box, top_box)
# Check that the initializer still succeeds.
initializer(physics=physics, random_state=np.random.RandomState(0))
self.assertTargetPoseAchieved(physics.bind(hand.tool_center_point),
target_pos, target_quat)
def test_detect_hit(self):
arena = composer.Arena()
ball = soccer_ball.SoccerBall(radius=0.35, mass=0.045, name='test_ball')
player = team.Player(
team=team.Team.HOME,
walker=props.Primitive(geom_type='sphere', size=(0.1,), name='home'))
arena.add_free_entity(player.walker)
ball.register_player(player)
arena.add_free_entity(ball)
random_state = np.random.RandomState(42)
physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model)
physics.step()
ball.initialize_episode(physics, random_state)
ball.before_step(physics, random_state)
self.assertEqual(ball.hit, False)
self.assertEqual(ball.repossessed, False)
self.assertEqual(ball.intercepted, False)
self.assertIsNone(ball.last_hit)
self.assertIsNone(ball.dist_between_last_hits)
ball.after_substep(physics, random_state)
ball.after_step(physics, random_state)
self.assertEqual(ball.hit, True)
self.assertEqual(ball.repossessed, True)
self.assertEqual(ball.intercepted, True)
self.assertEqual(ball.last_hit, player)
# Only one hit registered.
self.assertIsNone(ball.dist_between_last_hits)
def test_ignore_robot_collision_with_free_body(self):
arena, arm, hand = self.make_model()
target_pos = np.array([0.1, 0.2, 0.3])
target_quat = np.array([0., 1., 1., 0.])
# The obstacle is still placed at the target location, but this time it has
# a freejoint and is held in place by a weld constraint.
obstacle = props.Primitive(geom_type='sphere',
size=[0.3],
pos=target_pos)
attachment_frame = arena.add_free_entity(obstacle)
attachment_frame.pos = target_pos
arena.mjcf_model.equality.add('weld',
body1=attachment_frame,
relpose=np.hstack(
[target_pos, [1., 0., 0., 0.]]))
physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model)
initializer = tcp_initializer.ToolCenterPointInitializer(
hand=hand, arm=arm, position=target_pos, quaternion=target_quat)
# Check that the initializer succeeds.
initializer(physics=physics, random_state=np.random.RandomState(0))
self.assertTargetPoseAchieved(physics.bind(hand.tool_center_point),
target_pos, target_quat)
# Confirm that the obstacle and the hand are in contact.
self.assertEntitiesInContact(physics, hand, obstacle)
def _pitch_with_ball(self, pitch_size, ball_pos):
pitch = pitch_lib.Pitch(size=pitch_size)
self.assertEqual(pitch.size, pitch_size)
sphere = props.Primitive(geom_type='sphere', size=(0.1,), pos=ball_pos)
pitch.register_ball(sphere)
pitch.attach(sphere)
env = composer.Environment(
composer.NullTask(pitch), random_state=np.random.RandomState(42))
env.reset()
return pitch
def _make_spheres(num_spheres, radius, nconmax):
spheres = []
arena = composer.Arena()
arena.mjcf_model.worldbody.add('geom',
type='plane',
size=[1, 1, 0.1],
pos=[0., 0., -2 * radius],
name='ground')
for i in range(num_spheres):
sphere = props.Primitive(geom_type='sphere',
size=[radius],
name='sphere_{}'.format(i))
arena.add_free_entity(sphere)
spheres.append(sphere)
arena.mjcf_model.size.nconmax = nconmax
physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model)
return physics, spheres
def test_exception_if_hand_colliding_with_fixed_body(self):
arena, arm, hand = self.make_model()
target_pos = np.array([0.1, 0.2, 0.3])
target_quat = np.array([0., 1., 1., 0.])
max_rejection_samples = 10
max_ik_attempts = 5
# Place a fixed obstacle at the target location so that the TCP can't reach
# the target without colliding with it.
obstacle = props.Primitive(geom_type='sphere', size=[0.3])
attachment_frame = arena.attach(obstacle)
attachment_frame.pos = target_pos
physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model)
make_initializer = functools.partial(
tcp_initializer.ToolCenterPointInitializer,
hand=hand,
arm=arm,
position=target_pos,
quaternion=target_quat,
max_ik_attempts=max_ik_attempts,
max_rejection_samples=max_rejection_samples)
initializer = make_initializer()
with self.assertRaisesWithLiteralMatch(
RuntimeError,
tcp_initializer._REJECTION_SAMPLING_FAILED.format(
max_rejection_samples=max_rejection_samples,
max_ik_attempts=max_ik_attempts)):
initializer(physics=physics, random_state=np.random.RandomState(0))
# The initializer should succeed if we ignore collisions.
initializer_ignore_collisions = make_initializer(
ignore_collisions=True)
initializer_ignore_collisions(physics=physics,
random_state=np.random.RandomState(0))
self.assertTargetPoseAchieved(physics.bind(hand.tool_center_point),
target_pos, target_quat)
# Confirm that the obstacle and the hand are in contact.
self.assertEntitiesInContact(physics, hand, obstacle)
