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)