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
