import numpy as np
from scipy.spatial.transform import Rotation
def quaternion_slerp(q1, q2, t, eps=1e-6):
# Normalize input quaternions
q1 = q1 / np.linalg.norm(q1)
q2 = q2 / np.linalg.norm(q2)
# Compute the cosine of the angle between the two quaternions
cos_theta = np.dot(q1, q2)
# If the dot product is negative, negate one of the quaternions
if cos_theta < 0.0:
q1 = -q1
cos_theta = -cos_theta
# If the quaternions are very close, use linear interpolation to avoid numerical instability
if cos_theta > 1.0 - eps:
return q1 * (1 - t) + q2 * t
# Compute the angle between the quaternions
theta = np.arccos(cos_theta)
# Compute the scale factors for quaternion interpolation
sin_theta = np.sin(theta)
s1 = np.sin((1 - t) * theta) / sin_theta
s2 = np.sin(t * theta) / sin_theta
# Compute the interpolated quaternion
q_slerp = q1 * s1 + q2 * s2
return q_slerp
# Example usage:
q1 = np.array([1.0, 0.0, 0.0, 0.0]) # Identity quaternion
q2 = Rotation.from_euler('xyz', [90, 0, 0], degrees=True).as_quat() # Quaternion representing a 90-degree rotation around the x-axis
t = 0.5 # Interpolation parameter (0 <= t <= 1)
q_slerp = quaternion_slerp(q1, q2, t)
# Convert the resulting quaternion to an equivalent Euler angle representation
euler_angles = Rotation.from_quat(q_slerp).as_euler('xyz', degrees=True)
print("Interpolated quaternion:", q_slerp)
print("Equivalent Euler angles:", euler_angles)