Exemplo n.º 1
0
 def test_distance(self):
     q = Quaternion(scalar=0, vector=[1,0,0])
     p = Quaternion(scalar=0, vector=[0,1,0])
     self.assertEqual(pi/2, Quaternion.distance(q,p))
     q = Quaternion(angle=pi/2, axis=[1,0,0])
     p = Quaternion(angle=pi/2, axis=[0,1,0])
     self.assertEqual(pi/3, Quaternion.distance(q,p))
     q = Quaternion(scalar=1, vector=[1,1,1])
     p = Quaternion(scalar=-1, vector=[-1,-1,-1])
     p._normalise()
     q._normalise()
     self.assertAlmostEqual(0, Quaternion.distance(q,p), places=8)
Exemplo n.º 2
0
def rpy_to_quaternion(rpy):
    # based on wikipedia: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
    roll = rpy[0]
    pitch = rpy[1]
    yaw = rpy[2]

    cy = math.cos(yaw * 0.5)
    sy = math.sin(yaw * 0.5)
    cr = math.cos(roll * 0.5)
    sr = math.sin(roll * 0.5)
    cp = math.cos(pitch * 0.5)
    sp = math.sin(pitch * 0.5)

    w = cy * cr * cp + sy * sr * sp
    x = cy * sr * cp - sy * cr * sp
    y = cy * cr * sp + sy * sr * cp
    z = sy * cr * cp - cy * sr * sp

    quat = Quaternion(w, x, y, z)
    quat._normalise()

    return quat