def test_update_orientation_half_rev_x_axis(): """ Test update_orientation() rotating 1/2 of a revolution about the x-axis. """ test_rocket = rm.Rocket() test_rocket.gyro = sensors.Gyro() delta_time = 0.5 # 1/2 second angular_rates = np.array([2 * np.pi, 0, 0]) # 1 rev/s in x orientation_after_update = Quaternion(axis=[1, 0, 0], angle=np.pi).elements new_orientation = test_rocket.gyro.update(test_rocket, angular_rates, delta_time) np.testing.assert_allclose(new_orientation, orientation_after_update, atol=rm.TOLERANCE)
def test_update_orientation_eighth_rev_y_axis(): """ Test update_orientation() rotating 1/8 of a revolution about the y-axis. """ test_rocket = rm.Rocket() test_rocket.gyro = sensors.Gyro() delta_time = 0.125 # 1/8 second angular_rates = np.array([0, 2 * np.pi, 0]) # 1 rev/s in y orientation_after_update = Quaternion(axis=[0, 1, 0], angle=(np.pi / 4)).elements new_orientation = test_rocket.gyro.update(test_rocket, angular_rates, delta_time) np.testing.assert_allclose(new_orientation, orientation_after_update, atol=rm.TOLERANCE)
def test_update_orientation_three_quarter_rev_z_axis(): """ Test update_orientation() rotating 3/4 of a revolution about the z-axis. """ test_rocket = rm.Rocket() test_rocket.gyro = sensors.Gyro() delta_time = 0.75 # 3/4 second angular_rates = np.array([0, 0, 2 * np.pi]) # 1 rev/s in z orientation_after_update = Quaternion(axis=[0, 0, 1], angle=(3 * np.pi / 2)).elements new_orientation = test_rocket.gyro.update(test_rocket, angular_rates, delta_time) np.testing.assert_allclose(new_orientation, orientation_after_update, atol=rm.TOLERANCE)
def test_update_orientation_quarter_rev_x_z_axes(): """ Test update_orientation() rotating 1/4 of a revolution about the x and z axes. """ test_rocket = rm.Rocket() test_rocket.gyro = sensors.Gyro() delta_time = 0.25 # 1/4 second angular_rates = np.array([5.61985144, 0, 2.80992617]) orientation_after_update = Quaternion(axis=[2, 0, 1], angle=(np.pi / 2)).elements new_orientation = test_rocket.gyro.update(test_rocket, angular_rates, delta_time) np.testing.assert_allclose(new_orientation, orientation_after_update, atol=rm.TOLERANCE)
def test_update_orientation_from_non_identity_quat(): """ Test update_orientation() rotating from non-identity quaternion. Note: rotation is 1/4 revolution about x axis. """ test_rocket = rm.Rocket() test_rocket.gyro = sensors.Gyro() orientation = Quaternion(axis=[-1, 0, 0], angle=(np.pi / 2)) test_rocket.orientation = orientation.elements delta_time = 0.25 # 1/4 second angular_rates = np.array([2 * np.pi, 0, 0]) orientation_after_update = Quaternion().elements new_orientation = test_rocket.gyro.update(test_rocket, angular_rates, delta_time) np.testing.assert_allclose(new_orientation, orientation_after_update, atol=rm.TOLERANCE)
def test_update_orientation_half_rev_all_axes(): """ Test update_orientation() rotating 1/2 of a revolution about all of the axes. """ test_rocket = rm.Rocket() test_rocket.gyro = sensors.Gyro() delta_time = 0.5 # 1/2 second angular_rates = np.array( [np.pi / np.sqrt(3), np.pi / np.sqrt(3), np.pi / np.sqrt(3)]) # 1/6 rev/s for all orientation_after_update = Quaternion(axis=[1, 1, 1], angle=(np.pi / 2)).elements new_orientation = test_rocket.gyro.update(test_rocket, angular_rates, delta_time) np.testing.assert_allclose(new_orientation, orientation_after_update, atol=rm.TOLERANCE)