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()
