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)
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