Example #1
0
    def test_m44_q_equivalence(self):
        """Test for equivalance of matrix and quaternion rotations.

        Create a matrix and quaternion, rotate each by the same values
        then convert matrix<->quaternion and check the results are the same.
        """
        m = matrix44.create_from_x_rotation(np.pi / 2.)
        mq = quaternion.create_from_matrix(m)

        q = quaternion.create_from_x_rotation(np.pi / 2.)
        qm = matrix44.create_from_quaternion(q)

        self.assertTrue(
            np.allclose(np.dot([1., 0., 0., 1.], m), [1., 0., 0., 1.]))
        self.assertTrue(
            np.allclose(np.dot([1., 0., 0., 1.], qm), [1., 0., 0., 1.]))

        self.assertTrue(
            np.allclose(quaternion.apply_to_vector(q, [1., 0., 0., 1.]),
                        [1., 0., 0., 1.]))
        self.assertTrue(
            np.allclose(quaternion.apply_to_vector(mq, [1., 0., 0., 1.]),
                        [1., 0., 0., 1.]))

        np.testing.assert_almost_equal(q, mq, decimal=5)
        np.testing.assert_almost_equal(m, qm, decimal=5)
Example #2
0
 def test_create_from_matrix_x(self):
     result = quaternion.create_from_matrix([
         [1., 0., 0.],
         [0., -1., 0.],
         [0., 0., -1.],
     ])
     np.testing.assert_almost_equal(result, [1., 0., 0., 0.], decimal=5)
     self.assertTrue(result.dtype == np.float)
Example #3
0
 def test_create_from_matrix_y(self):
     result = quaternion.create_from_matrix([
         [-1., 0., 0.],
         [0., 1., 0.],
         [0., 0., -1.],
     ])
     np.testing.assert_almost_equal(result, [0., 1., 0., 0.], decimal=5)
     self.assertTrue(result.dtype == np.float)
Example #4
0
    def test_quaternion_matrix_conversion(self):
        # https://au.mathworks.com/help/robotics/ref/quat2rotm.html?requestedDomain=www.mathworks.com
        q = quaternion.create(0.7071, 0., 0., 0.7071)
        m33 = matrix33.create_from_quaternion(q)
        expected = np.array([
            [1., 0., 0.],
            [0.,-0.,-1.],
            [0., 1.,-0.],
        ])
        self.assertTrue(np.allclose(m33, expected))

        # issue #42
        q = quaternion.create(*[0.80087974, 0.03166748, 0.59114721,-0.09018753])
        m33 = matrix33.create_from_quaternion(q)
        q2 = quaternion.create_from_matrix(m33)
        print(q, q2)
        self.assertTrue(np.allclose(q, q2))

        q3 = quaternion.create_from_matrix(m33.T)
        self.assertFalse(np.allclose(q, q3))
Example #5
0
    def test_quaternion_matrix_conversion(self):
        # https://au.mathworks.com/help/robotics/ref/quat2rotm.html?requestedDomain=www.mathworks.com
        q = quaternion.create(0.7071, 0., 0., 0.7071)
        m33 = matrix33.create_from_quaternion(q)
        expected = np.array([
            [1., 0., 0.],
            [0., -0., -1.],
            [0., 1., -0.],
        ])
        self.assertTrue(np.allclose(m33, expected))

        # issue #42
        q = quaternion.create(
            *[0.80087974, 0.03166748, 0.59114721, -0.09018753])
        m33 = matrix33.create_from_quaternion(q)
        q2 = quaternion.create_from_matrix(m33)
        print(q, q2)
        self.assertTrue(np.allclose(q, q2))

        q3 = quaternion.create_from_matrix(m33.T)
        self.assertFalse(np.allclose(q, q3))
Example #6
0
    def test_operators_matrix44(self):
        q = Quaternion()
        m = Matrix44.from_x_rotation(0.5)

        # add
        self.assertRaises(ValueError, lambda: q + m)

        # subtract
        self.assertRaises(ValueError, lambda: q - m)

        # multiply
        self.assertTrue(np.array_equal(q * m, quaternion.cross(quaternion.create(), quaternion.create_from_matrix(matrix44.create_from_x_rotation(0.5)))))

        # divide
        self.assertRaises(ValueError, lambda: q / m)
Example #7
0
    def test_m44_q_equivalence(self):
        """Test for equivalance of matrix and quaternion rotations.

        Create a matrix and quaternion, rotate each by the same values
        then convert matrix<->quaternion and check the results are the same.
        """
        m = matrix44.create_from_x_rotation(np.pi / 2.)
        mq = quaternion.create_from_matrix(m)

        q = quaternion.create_from_x_rotation(np.pi / 2.)
        qm = matrix44.create_from_quaternion(q)

        self.assertTrue(np.allclose(np.dot([1.,0.,0.,1.], m), [1.,0.,0.,1.]))
        self.assertTrue(np.allclose(np.dot([1.,0.,0.,1.], qm), [1.,0.,0.,1.]))

        self.assertTrue(np.allclose(quaternion.apply_to_vector(q, [1.,0.,0.,1.]), [1.,0.,0.,1.]))
        self.assertTrue(np.allclose(quaternion.apply_to_vector(mq, [1.,0.,0.,1.]), [1.,0.,0.,1.]))

        np.testing.assert_almost_equal(q, mq, decimal=5)
        np.testing.assert_almost_equal(m, qm, decimal=5)
Example #8
0
 def test_create_from_matrix_unit(self):
     result = quaternion.create_from_matrix(np.eye(3))
     np.testing.assert_almost_equal(result, [0., 0., 0., 1.], decimal=5)
     self.assertTrue(result.dtype == np.float)
Example #9
0
 def test_create_from_matrix_unit(self):
     result = quaternion.create_from_matrix(np.eye(3))
     np.testing.assert_almost_equal(result, [0., 0., 0., 1.], decimal=5)
     self.assertTrue(result.dtype == np.float)