achllle / dual_quaternions Goto Github PK

from scipy.spatial.transform import Rotation
import numpy as np
from dual_quaternions import DualQuaternion

T1 = np.array([[-1.20911060e-02, -9.99300112e-01,  3.53990259e-02,
        -4.36968382e+00],
       [ 9.93674343e-01, -1.59607498e-02, -1.11160043e-01,
         1.54711835e+01],
       [ 1.11647238e-01,  3.38310559e-02,  9.93171865e-01,
         9.34843861e-01],
       [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,
         1.00000000e+00]])

T2 = np.array([[-3.37726903e-02, -9.99344514e-01,  1.30364371e-02,
        -4.47583692e+00],
       [ 9.92807002e-01, -3.50451704e-02, -1.14481845e-01,
         1.54705164e+01],
       [ 1.14863668e-01,  9.07630609e-03,  9.93339800e-01,
         9.37977190e-01],
       [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,
         1.00000000e+00]])
# Rotation matrices
R1 = T1[:3, :3]
R2 = T2[:3, :3]

# euler angles
euler1 = Rotation.from_matrix(R1).as_euler('zyx')
euler2 = Rotation.from_matrix(R2).as_euler('zyx')

print("Euler angles for T1: ", euler1)
print("Euler angles for T2: ", euler2)

dq1 = DualQuaternion.from_homogeneous_matrix(T1)
dq2 = DualQuaternion.from_homogeneous_matrix(T2)
delta = 0.5
# sclerp
dq = DualQuaternion.sclerp(dq1, dq2, delta)
T = dq.homogeneous_matrix()

# Linear interpolation is not satisfied
np.dot(np.linalg.inv(T1), T) - np.dot(T, np.linalg.inv(T2))