def test_joints_pos_observables(self, joint_pos, expected_obs): joint_index = 0 arm = kinova.JacoArm(name) physics = mjcf.Physics.from_mjcf_model(arm.mjcf_model) physics.bind(arm.joints).qpos[joint_index] = joint_pos actual_obs = arm.observables.joints_pos(physics)[joint_index] np.testing.assert_array_almost_equal(expected_obs, actual_obs)
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 test_backdriving_torque(self, joint_index, min_expected_torque, max_expected_torque): arm = kinova.JacoArm(name) physics = mjcf.Physics.from_mjcf_model(arm.mjcf_model) bound_joint = physics.bind(arm.joints[joint_index]) torque = min_expected_torque * 0.8 velocity_threshold = 0.1 * 2 * np.pi / 60. # 0.1 RPM torque_increment = 0.01 seconds_per_torque_increment = 1. max_torque = max_expected_torque * 1.1 while torque < max_torque: # Ensure that no other forces are acting on the arm. with physics.model.disable('gravity', 'contact', 'actuation'): # Reset the simulation so that the initial velocity is zero. physics.reset() bound_joint.qfrc_applied = torque while physics.time() < seconds_per_torque_increment: physics.step() if bound_joint.qvel[0] >= velocity_threshold: self.assertBetween(torque, min_expected_torque, max_expected_torque) return # If we failed to accelerate the joint to the target velocity within the # time limit we'll reset the simulation and increase the torque. torque += torque_increment self.fail( 'Torque of {} Nm insufficient to backdrive joint.'.format(torque))
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_arm(obs_settings): """Constructs a robot arm with manipulation-specific defaults. Args: obs_settings: `observations.ObservationSettings` instance. Returns: An instance of `manipulators.base.RobotArm`. """ return kinova.JacoArm(observable_options=observations.make_options( obs_settings, observations.JACO_ARM_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_velocity_actuation( self, actuator_index, control_input, expected_velocity): arm = kinova.JacoArm() physics = mjcf.Physics.from_mjcf_model(arm.mjcf_model) actuator = arm.actuators[actuator_index] bound_actuator = physics.bind(actuator) bound_joint = physics.bind(actuator.joint) acceleration_threshold = 1e-6 with physics.model.disable('contact', 'gravity'): bound_actuator.ctrl = control_input # Step until the joint has stopped accelerating. while abs(bound_joint.qacc) > acceleration_threshold: physics.step() self.assertAlmostEqual(bound_joint.qvel[0], expected_velocity, delta=0.01)
def test_joints_torque_observables(self, joint_index, applied_torque): arm = kinova.JacoArm() joint = arm.joints[joint_index] physics = mjcf.Physics.from_mjcf_model(arm.mjcf_model) with physics.model.disable('gravity', 'limit', 'contact', 'actuation'): # Apply a cartesian torque to the body containing the joint. We use # `xfrc_applied` rather than `qfrc_applied` because forces in # `qfrc_applied` are not measured by the torque sensor). physics.bind(joint.parent).xfrc_applied[3:] = ( applied_torque * physics.bind(joint).xaxis) observed_torque = arm.observables.joints_torque(physics)[joint_index] # Note the change in sign, since the sensor measures torques in the # child->parent direction. self.assertAlmostEqual(observed_torque, -applied_torque, delta=0.1)
def test_mass(self): arm = kinova.JacoArm(name) physics = mjcf.Physics.from_mjcf_model(arm.mjcf_model) mass = physics.bind(arm.mjcf_model.worldbody).subtreemass expected_mass = 4.4 self.assertAlmostEqual(mass, expected_mass)
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_can_compile_and_step_model(self): arm = kinova.JacoArm(name) physics = mjcf.Physics.from_mjcf_model(arm.mjcf_model) physics.step()