def test_finger_travel_time(self, opening): hand = kinova.JacoHand() physics = mjcf.Physics.from_mjcf_model(hand.mjcf_model) bound_actuators = physics.bind(hand.actuators) bound_joints = physics.bind(hand.joints) min_ctrl, max_ctrl = bound_actuators.ctrlrange.T min_qpos, max_qpos = bound_joints.range.T # Measure the time taken for the finger joints to traverse 99.9% of their # total range. qpos_tol = 1e-3 * (max_qpos - min_qpos) if opening: hand.set_grasp(physics=physics, close_factors=1.) # Fully closed. np.testing.assert_array_almost_equal(bound_joints.qpos, max_qpos) target_pos = min_qpos # Fully open. ctrl = min_ctrl # Open the fingers as fast as the actuators will allow. else: hand.set_grasp(physics=physics, close_factors=0.) # Fully open. np.testing.assert_array_almost_equal(bound_joints.qpos, min_qpos) target_pos = max_qpos # Fully closed. ctrl = max_ctrl # Close the fingers as fast as the actuators will allow. # Run the simulation until all joints have reached their target positions. bound_actuators.ctrl = ctrl while np.any(abs(bound_joints.qpos - target_pos) > qpos_tol): with physics.model.disable('gravity'): physics.step() expected_travel_time = 1.2 # Seconds. self.assertAlmostEqual(physics.time(), expected_travel_time, delta=0.1)
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_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 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 make_hand(obs_settings): """Constructs a robot hand with manipulation-specific defaults. Args: obs_settings: `observations.ObservationSettings` instance. Returns: An instance of `manipulators.base.RobotHand`. """ return kinova.JacoHand(use_pinch_site_as_tcp=True, observable_options=observations.make_options( obs_settings, observations.JACO_HAND_OBSERVABLES))
def __init__(self, observation_settings, opponent, game_logic, board, markers): """Initializes the task. Args: observation_settings: An `observations.ObservationSettings` namedtuple specifying configuration options for each category of observation. opponent: Opponent used for generating opponent moves. game_logic: Logic for keeping track of the logical state of the board. board: Board to use. markers: Markers to use. """ self._game_logic = game_logic self._game_opponent = opponent arena = arenas.Standard(observable_options=observations.make_options( observation_settings, observations.ARENA_OBSERVABLES)) arena.attach(board) arm = kinova.JacoArm(observable_options=observations.make_options( observation_settings, observations.JACO_ARM_OBSERVABLES)) hand = kinova.JacoHand(observable_options=observations.make_options( observation_settings, observations.JACO_HAND_OBSERVABLES)) arm.attach(hand) arena.attach_offset(arm, offset=(0, _ARM_Y_OFFSET, 0)) arena.attach(markers) # Geoms belonging to the arm and hand are placed in a custom group in order # to disable their visibility to the top-down camera. NB: we assume that # there are no other geoms in ROBOT_GEOM_GROUP that don't belong to the # robot (this is usually the case since the default geom group is 0). If # there are then these will also be invisible to the top-down camera. for robot_geom in arm.mjcf_model.find_all('geom'): robot_geom.group = arenas.ROBOT_GEOM_GROUP self._arena = arena self._board = board self._arm = arm self._hand = hand self._markers = markers self._tcp_initializer = initializers.ToolCenterPointInitializer( hand=hand, arm=arm, position=distributions.Uniform(_TCP_LOWER_BOUNDS, _TCP_UPPER_BOUNDS), quaternion=_uniform_downward_rotation()) # Add an observable exposing the logical state of the board. board_state_observable = observable.Generic( lambda physics: self._game_logic.get_board_state()) board_state_observable.configure( **observation_settings.board_state._asdict()) self._task_observables = {'board_state': board_state_observable}
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 test_can_attach_hand(self): arm = kinova.JacoArm(name) hand = kinova.JacoHand() arm.attach(hand) physics = mjcf.Physics.from_mjcf_model(arm.mjcf_model) physics.step()
def test_hand_mass(self): hand = kinova.JacoHand() physics = mjcf.Physics.from_mjcf_model(hand.mjcf_model) mass = physics.bind(hand.mjcf_model.worldbody).subtreemass expected_mass = 0.727 self.assertAlmostEqual(mass, expected_mass)
def test_can_compile_and_step_model(self): hand = kinova.JacoHand() physics = mjcf.Physics.from_mjcf_model(hand.mjcf_model) physics.step()