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)