Python Quaternion.rotate方法代码示例

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import rotate [as 别名]
class RigidTransform(object):
    def __init__(self, rotation_quat, translation_vec):
        self.quat = Quaternion(rotation_quat)
        self.tvec = numpy.array(translation_vec)

    def inverse(self):
        """ returns a new RigidTransform that corresponds to the inverse of this one """
        qinv = self.quat.inverse()
        return RigidTransform(qinv, qinv.rotate(- self.tvec))

    def interpolate(self, other_transform, this_weight):
        assert this_weight >= 0 and this_weight <= 1
        t = self.tvec * this_weight + other_transform.tvec * (1 - this_weight)
        r = self.quat.interpolate(other_transform.quat, this_weight)
        return RigidTransform(r, t)

    def __mul__(self, other):
        if isinstance(other, RigidTransform):
            t = self.quat.rotate(other.tvec) + self.tvec
            r = self.quat * other.quat
            return RigidTransform(r, t)
        else:
            olen = len(other)
            if olen == 3:
                r = numpy.array(self.quat.rotate(other))
                return r + self.tvec
            elif olen == 4:
                return np.dot(self.to_homogeneous_matrix(), other)
            else:
                raise ValueError()

    def to_homogeneous_matrix(self):
        result = self.quat.to_matrix_homogeneous()
	result.A[:3, 3] = self.tvec
        return result

    def to_roll_pitch_yaw_x_y_z(self):
        r, p, y = self.quat.to_roll_pitch_yaw()
        return numpy.array((r, p, y, self.tvec[0], self.tvec[1], self.tvec[2]))

    @staticmethod
    def from_roll_pitch_yaw_x_y_z(r, p, yaw, x, y, z):
        q = Quaternion.from_roll_pitch_yaw(r, p, yaw)
        return RigidTransform(q, (x, y, z))

    def quaternion(self):
        return self.quat

    def translation(self):
        return self.tvec

    @staticmethod
    def identity():
        return RigidTransform((1, 0, 0, 0), (0, 0, 0))

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import rotate [as 别名]
    def test_matrix_io(self):
        v = np.random.uniform(-100, 100, 3)

        for i in range(10):
            q0 = Quaternion.random()
            R  = q0.rotation_matrix()
            q1 = Quaternion(matrix=R)
            np.testing.assert_almost_equal(q0.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE)
            np.testing.assert_almost_equal(q0.rotate(v), q1.rotate(v), decimal=ALMOST_EQUAL_TOLERANCE)
            np.testing.assert_almost_equal(q1.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE)

            self.assertTrue((q0 == q1) or (q0 == -q1)) # q1 and -q1 are equivalent rotations

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import rotate [as 别名]
 def test_integration(self):
     rotation_rate = [0, 0, 2*pi] # one rev per sec around z
     v = [1, 0, 0] # test vector
     for dt in [0, 0.25, 0.5, 0.75, 1, 2, 10, 1e-10, random()*10]: # time step in seconds
         qt = Quaternion() # no rotation
         qt.integrate(rotation_rate, dt)
         q_truth = Quaternion(axis=[0,0,1], angle=dt*2*pi)   
         a = qt.rotate(v)
         b = q_truth.rotate(v)
         np.testing.assert_almost_equal(a, b, decimal=ALMOST_EQUAL_TOLERANCE)
         self.assertTrue(qt.is_unit())
     # Check integrate() is norm-preserving over many calls
     q = Quaternion()
     for i in range(1000):
         q.integrate([pi, 0, 0], 0.001)
     self.assertTrue(q.is_unit())

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import rotate [as 别名]
    def test_rotate(self):
        q  = Quaternion(axis=[1,1,1], angle=2*pi/3)
        q2 = Quaternion(axis=[1, 0, 0], angle=-pi)
        q3 = Quaternion(axis=[1, 0, 0], angle=pi)
        precision = ALMOST_EQUAL_TOLERANCE
        for r in [1, 3.8976, -69.7, -0.000001]:
            # use np.testing.assert_almost_equal() to compare float sequences
            np.testing.assert_almost_equal(q.rotate((r, 0, 0)), (0, r, 0), decimal=ALMOST_EQUAL_TOLERANCE)
            np.testing.assert_almost_equal(q.rotate([0, r, 0]), [0, 0, r], decimal=ALMOST_EQUAL_TOLERANCE)
            np.testing.assert_almost_equal(q.rotate(np.array([0, 0, r])), np.array([r, 0, 0]), decimal=ALMOST_EQUAL_TOLERANCE)
            self.assertEqual(q.rotate(Quaternion(vector=[-r, 0, 0])), Quaternion(vector=[0, -r, 0]))
            np.testing.assert_almost_equal(q.rotate([0, -r, 0]), [0, 0, -r], decimal=ALMOST_EQUAL_TOLERANCE)
            self.assertEqual(q.rotate(Quaternion(vector=[0, 0, -r])), Quaternion(vector=[-r, 0, 0]))

            np.testing.assert_almost_equal(q2.rotate((r, 0, 0)), q3.rotate((r, 0, 0)), decimal=ALMOST_EQUAL_TOLERANCE)
            np.testing.assert_almost_equal(q2.rotate((0, r, 0)), q3.rotate((0, r, 0)), decimal=ALMOST_EQUAL_TOLERANCE)
            np.testing.assert_almost_equal(q2.rotate((0, 0, r)), q3.rotate((0, 0, r)), decimal=ALMOST_EQUAL_TOLERANCE)

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import rotate [as 别名]
    def test_init_from_explicit_matrix(self):
        
        def R_z(theta):
            """
            Generate a rotation matrix describing a rotation of theta degrees about the z-axis
            """
            c = cos(theta)
            s = sin(theta)
            return np.array([
                [c,-s, 0],
                [s, c, 0],
                [0, 0, 1]])

        v = np.array([1, 0, 0])
        for angle in [0, pi/6, pi/4, pi/2, pi, 4*pi/3, 3*pi/2, 2*pi]:
            R = R_z(angle) # rotation matrrix describing rotation of 90 about +z
            v_prime_r = np.dot(R, v)

            q1 = Quaternion(axis=[0,0,1], angle=angle)
            v_prime_q1 = q1.rotate(v)

            np.testing.assert_almost_equal(v_prime_r, v_prime_q1, decimal=ALMOST_EQUAL_TOLERANCE)

            q2 = Quaternion(matrix=R)
            v_prime_q2 = q2.rotate(v)

            np.testing.assert_almost_equal(v_prime_q2, v_prime_r, decimal=ALMOST_EQUAL_TOLERANCE)

        R = np.matrix(np.eye(3))
        q3 = Quaternion(matrix=R)
        v_prime_q3 = q3.rotate(v)
        np.testing.assert_almost_equal(v, v_prime_q3, decimal=ALMOST_EQUAL_TOLERANCE)
        self.assertEqual(q3, Quaternion())

        R[0,1] += 3 # introduce error to make matrix non-orthogonal
        with self.assertRaises(ValueError):
            q4 = Quaternion(matrix=R)