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)