def test_pinch_site_observables(self, pos, quat): arm = kinova.JacoArm(name) hand = kinova.JacoHand() arena = composer.Arena() arm.attach(hand) arena.attach(arm) physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model) # Normalize the quaternion. quat /= np.linalg.norm(quat) # Drive the arm so that the pinch site is at the desired position and # orientation. success = arm.set_site_to_xpos(physics=physics, random_state=np.random.RandomState(0), site=hand.pinch_site, target_pos=pos, target_quat=quat) self.assertTrue(success) # Check that the observations are as expected. observed_pos = hand.observables.pinch_site_pos(physics) np.testing.assert_allclose(observed_pos, pos, atol=1e-3) observed_rmat = hand.observables.pinch_site_rmat(physics).reshape(3, 3) expected_rmat = np.empty((3, 3), np.double) mjlib.mju_quat2Mat(expected_rmat.ravel(), quat) difference_rmat = observed_rmat.dot(expected_rmat.T) # 1.000000004 fails as np.arccos should be < 1.0 angular_difference = np.arccos( np.round((np.trace(difference_rmat) - 1) / 2)) self.assertLess(angular_difference, 1e-3)
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_pinch_site_observables(self, pos, quat): arm = kinova.JacoArm() hand = kinova.JacoHand() arena = composer.Arena() arm.attach(hand) arena.attach(arm) physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model) # Normalize the quaternion. quat /= np.linalg.norm(quat) # Drive the arm so that the pinch site is at the desired position and # orientation. success = arm.set_site_to_xpos(physics=physics, random_state=np.random.RandomState(0), site=hand.pinch_site, target_pos=pos, target_quat=quat) self.assertTrue(success) # Check that the observations are as expected. observed_pos = hand.observables.pinch_site_pos(physics) np.testing.assert_allclose(observed_pos, pos, atol=1e-3) observed_rmat = hand.observables.pinch_site_rmat(physics).reshape(3, 3) expected_rmat = np.empty((3, 3), np.double) mjlib.mju_quat2Mat(expected_rmat.ravel(), quat) difference_rmat = observed_rmat.dot(expected_rmat.T) # `difference_rmat` might not be perfectly orthonormal, which could lead to # an invalid value being passed to arccos. u, _, vt = np.linalg.svd(difference_rmat, full_matrices=False) ortho_difference_rmat = u.dot(vt) angular_difference = np.arccos( (np.trace(ortho_difference_rmat) - 1) / 2) self.assertLess(angular_difference, 1e-3)
def stack_bricks(top_brick, bottom_brick): """Stacks two Duplo bricks, returns the attachment frame of the top brick.""" arena = composer.Arena() # Bottom brick is fixed in place, top brick has a freejoint. arena.attach(bottom_brick) attachment_frame = arena.add_free_entity(top_brick) # Attachment frame is positioned such that the top brick is on top of the # bottom brick. attachment_frame.pos = (0, 0, 0.0192) return arena, attachment_frame
def setUp(self): super(PositionDetectorTest, self).setUp() self.arena = composer.Arena() self.props = [ primitive.Primitive(geom_type='sphere', size=(0.1, )), primitive.Primitive(geom_type='sphere', size=(0.1, )) ] for prop in self.props: self.arena.add_free_entity(prop) self.task = composer.NullTask(self.arena)
def make_model(self, with_hand=True): arm = kinova.JacoArm() arena = composer.Arena() arena.attach(arm) if with_hand: hand = kinova.JacoHand() arm.attach(hand) else: hand = None return arena, arm, hand
def test_grip_force(self): arena = composer.Arena() hand = kinova.JacoHand() arena.attach(hand) # A sphere with a touch sensor for measuring grip force. prop_model = mjcf.RootElement(model='grip_target') prop_model.worldbody.add('geom', type='sphere', size=[0.02]) touch_site = prop_model.worldbody.add('site', type='sphere', size=[0.025]) touch_sensor = prop_model.sensor.add('touch', site=touch_site) prop = composer.ModelWrapperEntity(prop_model) # Add some slide joints to allow movement of the target in the XY plane. # This helps the contact solver to converge more reliably. prop_frame = arena.attach(prop) prop_frame.add('joint', name='slide_x', type='slide', axis=(1, 0, 0)) prop_frame.add('joint', name='slide_y', type='slide', axis=(0, 1, 0)) physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model) bound_pinch_site = physics.bind(hand.pinch_site) bound_actuators = physics.bind(hand.actuators) bound_joints = physics.bind(hand.joints) bound_touch = physics.bind(touch_sensor) # Position the grip target at the pinch site. prop.set_pose(physics, position=bound_pinch_site.xpos) # Close the fingers with as much force as the actuators will allow. bound_actuators.ctrl = bound_actuators.ctrlrange[:, 1] # Run the simulation forward until the joints stop moving. physics.step() qvel_thresh = 1e-3 # radians / s while max(abs(bound_joints.qvel)) > qvel_thresh: physics.step() expected_min_grip_force = 20. expected_max_grip_force = 30. grip_force = bound_touch.sensordata self.assertBetween( grip_force, expected_min_grip_force, expected_max_grip_force, msg='Expected grip force to be between {} and {} N, got {} N.'. format(expected_min_grip_force, expected_max_grip_force, grip_force))
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 _make_free_prop(self, geom_type='sphere', size=(0.1, ), **kwargs): prop = primitive.Primitive(geom_type=geom_type, size=size, **kwargs) arena = composer.Arena() arena.add_free_entity(prop) physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model) return prop, physics