def test_sample_goal_difficulty_4_no_initial_pose(self):
for i in range(1000):
goal = move_cube.sample_goal(difficulty=4)
# verify the goal is valid (i.e. within the allowed ranges)
try:
move_cube.validate_goal(goal)
except move_cube.InvalidGoalError as e:
self.fail(msg="Invalid goal: {} pose is {}, {}".format(
e, e.position, e.orientation), )
# verify the goal satisfies conditions of difficulty 2
self.assertLessEqual(goal.position[2], move_cube._max_height)
self.assertGreaterEqual(goal.position[2], move_cube._min_height)
def test_sample_goal_difficulty_1_no_initial_pose(self):
for i in range(1000):
goal = move_cube.sample_goal(difficulty=1)
# verify the goal is valid (i.e. within the allowed ranges)
try:
move_cube.validate_goal(goal)
except move_cube.InvalidGoalError as e:
self.fail(msg="Invalid goal: {} pose is {}, {}".format(
e, e.position, e.orientation), )
# verify the goal satisfies conditions of difficulty 1
# always on ground
self.assertEqual(goal.position[2], move_cube._CUBE_WIDTH / 2)
# no orientation
np.testing.assert_array_equal(goal.orientation, [0, 0, 0, 1])
def __init__(self, difficulty, initial_state, goal):
"""Initialize.
Args:
difficulty (int): Difficulty level of the goal. This is still
needed even for a fixed goal, as it is also used for computing
the reward (the cost function is different for the different
levels).
initial_state (move_cube.Pose): Initial pose of the object.
goal (move_cube.Pose): Goal pose of the object.
Raises:
Exception: If initial_state or goal are not valid. See
:meth:`move_cube.validate_goal` for more information.
"""
move_cube.validate_goal(initial_state)
move_cube.validate_goal(goal)
self.difficulty = difficulty
self.initial_state = initial_state
self.goal = goal
def test_validate_goal(self):
half_width = move_cube._CUBE_WIDTH / 2
yaw_rotation = Rotation.from_euler("z", 0.42).as_quat()
full_rotation = Rotation.from_euler("zxz", [0.42, 0.1, -2.3]).as_quat()
# test some valid goals
try:
move_cube.validate_goal(
move_cube.Pose([0, 0, half_width], [0, 0, 0, 1])
)
except Exception as e:
self.fail("Valid goal was considered invalid because %s" % e)
try:
move_cube.validate_goal(
move_cube.Pose([0.05, -0.1, half_width], yaw_rotation)
)
except Exception as e:
self.fail("Valid goal was considered invalid because %s" % e)
try:
move_cube.validate_goal(
move_cube.Pose([-0.12, 0.0, 0.06], full_rotation)
)
except Exception as e:
self.fail("Valid goal was considered invalid because %s" % e)
# test some invalid goals
# invalid values
with self.assertRaises(ValueError):
move_cube.validate_goal(move_cube.Pose([0, 0], [0, 0, 0, 1]))
with self.assertRaises(ValueError):
move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 1]))
# invalid positions
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0.3, 0, half_width], [0, 0, 0, 1])
)
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0, -0.3, half_width], [0, 0, 0, 1])
)
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(move_cube.Pose([0, 0, 0.3], [0, 0, 0, 1]))
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(move_cube.Pose([0, 0, 0], [0, 0, 0, 1]))
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0, 0, -0.01], [0, 0, 0, 1])
)
# valid CoM position but rotation makes it reach out of valid range
with self.assertRaises(move_cube.InvalidGoalError):
move_cube.validate_goal(
move_cube.Pose([0, 0, half_width], full_rotation)
)
