Untitled

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)