def test_RollPitchYaw(self):
roll, pitch, yaw = 0.1, 0.2, 0.3
np.testing.assert_array_equal(
Rot3.Rx(roll).matrix(), Rot3.AxisAngle(UNIT_X, roll).matrix()
)
np.testing.assert_array_equal(
Rot3.Ry(pitch).matrix(), Rot3.AxisAngle(UNIT_Y, pitch).matrix()
)
np.testing.assert_array_equal(
Rot3.Rz(yaw).matrix(), Rot3.AxisAngle(UNIT_Z, yaw).matrix()
)
def test_to_rotation_matrix(self):
rot1 = self.q1.to_rotation_matrix()
np.testing.assert_array_almost_equal(rot1, np.identity(3))
M2 = Rot3.Rx(self.theta1).matrix()
rot2 = self.q2.to_rotation_matrix()
self.assertTrue(np.allclose(rot2, M2))
M3 = Rot3.Rz(self.theta2).matrix()
rot3 = self.q3.to_rotation_matrix()
np.testing.assert_array_almost_equal(rot3, M3)
def euler_to_matrix(e):
"""
Convert ZYX Euler angles to 3x3 rotation matrix.
Inputs:
e (numpy 3-vector of float) - ZYX Euler angles (radians)
Return:
matrix (3x3 numpy array2 of float) - rotation matrix
TODO: This could be optimized somewhat by using the direct
equations for the final matrix rather than multiplying out the
matrices.
"""
return (Rot3.Rz(e[0]) * Rot3.Ry(e[1]) * Rot3.Rx(e[2])).R
def test_quat_matrix(self):
rx = 10 # degrees
ry = 20
rz = 30
Rx = Rot3.Rx(math.radians(rx))
Ry = Rot3.Ry(math.radians(ry))
Rz = Rot3.Rz(math.radians(rz))
# matrix -> quat
R = Rz * Ry * Rx
q = utils.matrix_to_quat(R.R)
expected_q = np.array([0.9515485, 0.0381346, 0.1893079,
0.2392983]) # computed manually
np.testing.assert_array_almost_equal(q, expected_q, 4)
# quat -> matrix
R2 = utils.quat_to_matrix(expected_q)
np.testing.assert_array_almost_equal(R2, R.R, 4)
# round trip
R2 = utils.quat_to_matrix(q)
np.testing.assert_array_almost_equal(R.R, R2, 4)
