def test_sclerp_position(self):
"""test Screw Linear Interpolation for diff position, same orientation"""
dq1 = DualQuaternion.from_translation_vector([2, 2, 2])
dq2 = DualQuaternion.from_translation_vector([3, 4, -2])
interpolated1 = DualQuaternion.sclerp(dq1, dq2, 0.5)
expected1 = DualQuaternion.from_translation_vector([2.5, 3, 0])
self.assertEqual(interpolated1, expected1)
interpolated2 = DualQuaternion.sclerp(dq1, dq2, 0.1)
expected2 = DualQuaternion.from_translation_vector([2.1, 2.2, 1.6])
self.assertEqual(interpolated2, expected2)
def test_sclerp_orientation(self):
"""test Screw Linear Interpolation for diff orientation, same position"""
T_id = DualQuaternion.identity().homogeneous_matrix()
T_id[0:2, 0:2] = np.array([[0, -1], [1, 0]]) # rotate 90 around z
dq2 = DualQuaternion.from_homogeneous_matrix(T_id)
interpolated1 = DualQuaternion.sclerp(self.identity_dq, dq2, 0.5)
T_exp = DualQuaternion.identity().homogeneous_matrix()
sq22 = np.sqrt(2) / 2
T_exp[0:2, 0:2] = np.array([[sq22, -sq22],
[sq22, sq22]]) # rotate 45 around z
expected1 = DualQuaternion.from_homogeneous_matrix(T_exp)
self.assertEqual(interpolated1, expected1)
interpolated2 = DualQuaternion.sclerp(self.identity_dq, dq2, 0)
interpolated3 = DualQuaternion.sclerp(self.identity_dq, dq2, 1)
self.assertEqual(interpolated2, self.identity_dq)
self.assertEqual(interpolated3, dq2)
def test_sclerp_screw(self):
"""Interpolating with ScLERP should yield same result as interpolating with screw parameters
ScLERP is a screw motion interpolation with constant rotation and translation speeds. We can
simply interpolate screw parameters theta and d and we should get the same result.
"""
taus = [0., 0.23, 0.6, 1.0]
l, m, theta, d = self.normalized_dq.screw()
for tau in taus:
# interpolate using sclerp
interpolated_dq = DualQuaternion.sclerp(self.identity_dq,
self.normalized_dq, tau)
# interpolate using screw: l and m stay the same, theta and d vary with tau
interpolated_dq_screw = DualQuaternion.from_screw(
l, m, tau * theta, tau * d)
self.assertEqual(interpolated_dq, interpolated_dq_screw)
pour0, 20 500, 5
euler / quat trans 0.333 < 1.154 Rot 0.242 < 0.789
euler / exp trans 0.333 < 1.164 Rot 0.242 < 0.788
quat / exp trans 0.015 < 0.068 Rot 0.010 < 0.040
#difference / mean
#from test_mean_rot import get_euler_from_dq, get_euler_from_qr, get_info_from_dq, get_affine_rot_from_euler,paw_quaternion
rot_euler = np.random.uniform(-20,20,size=(2, 3)) #in degree
aff1, aff2 = get_affine_rot_from_euler(rot_euler)
aff1=spm_matrix([0,0,0,0,10,0,1,1,1,0,0,0],order=1)
aff2=spm_matrix([0,0,0,0,20,0,1,1,1,0,0,0],order=1)
dq1=DualQuaternion.from_homogeneous_matrix(aff1);dq2=DualQuaternion.from_homogeneous_matrix(aff2)
dqmean = DualQuaternion.sclerp(dq1,dq2,0.5); print(get_info_from_dq(dqmean)); print(get_euler_from_dq(dqmean))
np.mean(rot_euler,axis=0)
#domage, ca marche pas !
dqmean2 = dq1.pow(0.5)*dq2.pow(0.5); print(get_info_from_dq(dqmean2)); print(get_euler_from_dq(dqmean2))
dqdiff = dq1.inverse()*dq2 #dq1*dq2.quaternion_conjugate()
[print(f'{k}: {val}') for k,val in get_info_from_dq(dqdiff).items()]; print(get_euler_from_dq(dqdiff))
#dqmean3 = dqdiff.pow(0.5)*dq1.quaternion_conjugate(); get_info_from_dq(dqmean3); print(get_euler_from_dq(dqmean3))
dqmean3 = dq1 * dqdiff.pow(0.5); print(get_info_from_dq(dqmean3)); print(get_euler_from_dq(dqmean3))
qr1 = nq.from_rotation_matrix(aff1); qr2 = nq.from_rotation_matrix(aff2); get_euler_from_qr(qr1)
qrmean = paw_quaternion(qr1, 0.5) * paw_quaternion(qr2, 0.5) ; print(get_info_from_quat(qrmean))
qrmc = nq.mean_rotor_in_chordal_metric([qr1, qr2]); print(get_info_from_quat(qrmc)); get_euler_from_qr(qrmc)