def test_unique_representation_quat(self):
a = 1.2345
# These quaternions represent equivalent roatations
l = 1/numpy.sqrt(3)
q_same = [rotation.unique_representation_quat(rotation.quat(a, l, l, l)),
rotation.unique_representation_quat(rotation.quat(-a, -l, -l, -l)),
rotation.unique_representation_quat(rotation.quat(a + 2*numpy.pi, l, l, l)),
rotation.unique_representation_quat(rotation.quat(a - 2*numpy.pi, l, l, l))]
q_expected = q_same[0]
for q in q_same[1:]:
for q_i, q_expected_i in zip(q, q_expected):
self.assertAlmostEqual(q_i, q_expected_i)
def test_unique_representation_quat(self):
a = 1.2345
# These quaternions represent equivalent roatations
l = 1 / numpy.sqrt(3)
q_same = [
rotation.unique_representation_quat(rotation.quat(a, l, l, l)),
rotation.unique_representation_quat(rotation.quat(-a, -l, -l, -l)),
rotation.unique_representation_quat(
rotation.quat(a + 2 * numpy.pi, l, l, l)),
rotation.unique_representation_quat(
rotation.quat(a - 2 * numpy.pi, l, l, l))
]
q_expected = q_same[0]
for q in q_same[1:]:
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)
