def test_ignore_collision_not_involving_robot(self):
arena, arm, hand = self.make_model()
target_pos = np.array([0.1, 0.2, 0.3])
target_quat = np.array([0., 1., 1., 0.])
# Add two boxes that are always in contact with each other, but never with
# the arm or hand (since they are not within reach).
side_length = 0.1
x_offset = 10.
bottom_box = props.Primitive(geom_type='box',
size=[side_length] * 3,
pos=[x_offset, 0, 0])
top_box = props.Primitive(geom_type='box',
size=[side_length] * 3,
pos=[x_offset, 0, 2 * side_length])
arena.attach(bottom_box)
arena.add_free_entity(top_box)
initializer = tcp_initializer.ToolCenterPointInitializer(
hand=hand, arm=arm, position=target_pos, quaternion=target_quat)
physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model)
# Confirm that there are actually contacts between the two boxes.
self.assertEntitiesInContact(physics, bottom_box, top_box)
# Check that the initializer still succeeds.
initializer(physics=physics, random_state=np.random.RandomState(0))
self.assertTargetPoseAchieved(physics.bind(hand.tool_center_point),
target_pos, target_quat)
def test_ignore_robot_collision_with_free_body(self):
arena, arm, hand = self.make_model()
target_pos = np.array([0.1, 0.2, 0.3])
target_quat = np.array([0., 1., 1., 0.])
# The obstacle is still placed at the target location, but this time it has
# a freejoint and is held in place by a weld constraint.
obstacle = props.Primitive(geom_type='sphere',
size=[0.3],
pos=target_pos)
attachment_frame = arena.add_free_entity(obstacle)
attachment_frame.pos = target_pos
arena.mjcf_model.equality.add('weld',
body1=attachment_frame,
relpose=np.hstack(
[target_pos, [1., 0., 0., 0.]]))
physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model)
initializer = tcp_initializer.ToolCenterPointInitializer(
hand=hand, arm=arm, position=target_pos, quaternion=target_quat)
# Check that the initializer succeeds.
initializer(physics=physics, random_state=np.random.RandomState(0))
self.assertTargetPoseAchieved(physics.bind(hand.tool_center_point),
target_pos, target_quat)
# Confirm that the obstacle and the hand are in contact.
self.assertEntitiesInContact(physics, hand, obstacle)
def test_initialize_to_fixed_pose(self, target_pos, target_quat,
with_hand):
arena, arm, hand = self.make_model(with_hand=with_hand)
initializer = tcp_initializer.ToolCenterPointInitializer(
hand=hand, arm=arm, position=target_pos, quaternion=target_quat)
physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model)
initializer(physics=physics, random_state=np.random.RandomState(0))
site = hand.tool_center_point if with_hand else arm.wrist_site
self.assertTargetPoseAchieved(physics.bind(site), target_pos,
target_quat)