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