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