def test_restrict_angle_to_range_no_changes(self): """ Tests that the 'restrict_angle_to_range' function returns the same angle, since there is no offset and rotation_range is set from 0-360 degrees. """ offset = math.radians(0.0) rotation_range = numpy.radians([0.0, 360.0]) test_angles = numpy.radians([0, 30, 45, 90, 100, 150, 180, 270, 300, 360]) desired = numpy.radians([0, 30, 45, 90, 100, 150, 180, 270, 300, 0]) actual = [ pregrasp_planner_utils.restrict_angle_to_range(angle, offset, rotation_range) for angle in test_angles ] numpy.testing.assert_almost_equal(actual, desired)
def test_restrict_angle_to_range_rotation_inverted_range(self): """ Tests that the 'restrict_angle_to_range' function returns the only angles between an 'inverted' range (i.e. the first value of the range is greater than the second). It also restricts the value to be within 360 degrees. """ offset = math.radians(0.0) rotation_range = numpy.radians([270.0, 90.0]) test_angles = numpy.radians([0, 30, 45, 90, 100, 150, 180, 270, 300, 360]) desired = numpy.radians([0, 30, 45, 90, 280, 330, 0, 270, 300, 0]) actual = [ pregrasp_planner_utils.restrict_angle_to_range(angle, offset, rotation_range) for angle in test_angles ] numpy.testing.assert_almost_equal(actual, desired)
def test_restrict_angle_to_range_rotation_easy_range(self): """ Tests that the 'restrict_angle_to_range' function returns the only angles between the specified range. It also restricts the value to be within 360 degrees. """ offset = math.radians(0.0) rotation_range = numpy.radians([0.0, 180.0]) test_angles = numpy.radians([0, 30, 45, 90, 100, 150, 180, 270, 300, 360]) desired = numpy.radians([0, 30, 45, 90, 100, 150, 180, 90, 120, 180]) actual = [ pregrasp_planner_utils.restrict_angle_to_range(angle, offset, rotation_range) for angle in test_angles ] numpy.testing.assert_almost_equal(actual, desired)