def test_quat_from_rotmx(self):
a = 1.2345
R = rotation.R_x(a)
q = rotation.quat_from_rotmx(R)
# q = [w, x, y, z] = [cos(a/2), ux sin(a/2), uy sin(a/2), uz sin(a/2)]
q_expected = numpy.array([numpy.cos(a/2.), numpy.sin(a/2.), 0., 0.])
for q_i, q_expected_i in zip(q, q_expected):
self.assertAlmostEqual(q_i, q_expected_i)
def test_rotation_conversions_betw_formalisms(self, N=1000):
"""
Create random euler angles with different axes formalisms and test conversion in a circular fashion.
Euler angles => rotation matrix => quaternion => unique representation of quaternion (q_a) => Euler angles => rotation materix => quaterinion => unique representation of quaternion (q_b).
Finally compare the two quaternions q_a and q_b.
Repeat N times.
"""
rotation_matrices = {
'xyz': (rotation.R_x, rotation.R_y, rotation.R_z),
'xzy': (rotation.R_x, rotation.R_z, rotation.R_y),
'yxz': (rotation.R_y, rotation.R_x, rotation.R_z),
'yzx': (rotation.R_y, rotation.R_z, rotation.R_x),
'zxy': (rotation.R_z, rotation.R_x, rotation.R_y),
'zyx': (rotation.R_z, rotation.R_y, rotation.R_x),
'yxy': (rotation.R_y, rotation.R_x, rotation.R_y),
'zxz': (rotation.R_z, rotation.R_x, rotation.R_z),
'xyx': (rotation.R_x, rotation.R_y, rotation.R_x),
'zyz': (rotation.R_z, rotation.R_y, rotation.R_z),
'xzx': (rotation.R_x, rotation.R_z, rotation.R_x),
'yzy': (rotation.R_y, rotation.R_z, rotation.R_y),
}
succ = 0
NN = len(rotation_matrices) * N
err = numpy.ones(NN)
k = 0
for ax, (R1, R2, R3) in rotation_matrices.items():
for i in range(N):
euler_a = numpy.random.random(3) * numpy.pi * 2
euler_a[1] /= 2.
# Euler angles => rotaion matrix
R_a = R1(euler_a[0]).dot(R2(euler_a[1]).dot(R3(euler_a[2])))
# Rotation matrix => unique quaternion
q_a = rotation.quat_from_rotmx(R_a)
q_a = rotation.unique_representation_quat(q_a)
# Unique quaternion => Euler angles
euler_b = rotation.euler_from_quat(q_a, ax)
# Euler angles => quaternion
R_b = R1(euler_b[0]).dot(R2(euler_b[1]).dot(R3(euler_b[2])))
# Rotation matrix => unique quaternion
q_b = rotation.quat_from_rotmx(R_b)
q_b = rotation.unique_representation_quat(q_b)
# Calculate discrepancy
for q_a_i, q_b_i in zip(q_a, q_b):
self.assertAlmostEqual(q_a_i, q_b_i)
def test_rotation_conversions_betw_formalisms(self, N=1000):
"""
Create random euler angles with different axes formalisms and test conversion in a circular fashion.
Euler angles => rotation matrix => quaternion => unique representation of quaternion (q_a) => Euler angles => rotation materix => quaterinion => unique representation of quaternion (q_b).
Finally compare the two quaternions q_a and q_b.
Repeat N times.
"""
rotation_matrices = {
'xyz': (rotation.R_x, rotation.R_y, rotation.R_z),
'xzy': (rotation.R_x, rotation.R_z, rotation.R_y),
'yxz': (rotation.R_y, rotation.R_x, rotation.R_z),
'yzx': (rotation.R_y, rotation.R_z, rotation.R_x),
'zxy': (rotation.R_z, rotation.R_x, rotation.R_y),
'zyx': (rotation.R_z, rotation.R_y, rotation.R_x),
'yxy': (rotation.R_y, rotation.R_x, rotation.R_y),
'zxz': (rotation.R_z, rotation.R_x, rotation.R_z),
'xyx': (rotation.R_x, rotation.R_y, rotation.R_x),
'zyz': (rotation.R_z, rotation.R_y, rotation.R_z),
'xzx': (rotation.R_x, rotation.R_z, rotation.R_x),
'yzy': (rotation.R_y, rotation.R_z, rotation.R_y),
}
succ = 0
NN = len(rotation_matrices)*N
err = numpy.ones(NN)
k = 0
for ax,(R1,R2,R3) in rotation_matrices.items():
for i in range(N):
euler_a = numpy.random.random(3) * numpy.pi * 2
euler_a[1] /= 2.
# Euler angles => rotaion matrix
R_a = R1(euler_a[0]).dot(R2(euler_a[1]).dot(R3(euler_a[2])))
# Rotation matrix => unique quaternion
q_a = rotation.quat_from_rotmx(R_a)
q_a = rotation.unique_representation_quat(q_a)
# Unique quaternion => Euler angles
euler_b = rotation.euler_from_quat(q_a, ax)
# Euler angles => quaternion
R_b = R1(euler_b[0]).dot(R2(euler_b[1]).dot(R3(euler_b[2])))
# Rotation matrix => unique quaternion
q_b = rotation.quat_from_rotmx(R_b)
q_b = rotation.unique_representation_quat(q_b)
# Calculate discrepancy
for q_a_i, q_b_i in zip(q_a, q_b):
self.assertAlmostEqual(q_a_i, q_b_i)
def test_quat_from_rotmx(self):
a = 1.2345
R = rotation.R_x(a)
q = rotation.quat_from_rotmx(R)
# q = [w, x, y, z] = [cos(a/2), ux sin(a/2), uy sin(a/2), uz sin(a/2)]
q_expected = numpy.array(
[numpy.cos(a / 2.), numpy.sin(a / 2.), 0., 0.])
for q_i, q_expected_i in zip(q, q_expected):
self.assertAlmostEqual(q_i, q_expected_i)
