def test_from_screw_and_back(self):
# start with a random valid dual quaternion
dq = DualQuaternion.from_quat_pose_array(
[0.5, 0.3, 0.1, 0.4, 2, 5, -2])
lr, mr, thetar, dr = dq.screw()
dq_reconstructed = DualQuaternion.from_screw(lr, mr, thetar, dr)
self.assertEqual(dq, dq_reconstructed)
# start with some screw parameters
l1 = np.array([0.4, 0.2, 0.5])
l1 /= np.linalg.norm(l1) # make sure l1 is normalized
# pick some point away from the origin
p1 = np.array([2.3, 0.9, 1.1])
m1 = np.cross(p1, l1)
d1 = 4.32
theta1 = 1.94
dq1 = DualQuaternion.from_screw(l1, m1, theta1, d1)
l2, m2, theta2, d2 = dq1.screw()
try:
np.testing.assert_array_almost_equal(l1, l2, decimal=3)
np.testing.assert_array_almost_equal(l1, l2, decimal=3)
except AssertionError as e:
self.fail(e)
self.assertAlmostEqual(theta1, theta2)
self.assertAlmostEqual(d1, d2)
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)
def test_from_screw(self):
# construct an axis along the positive z-axis
l = np.array([0, 0, 1])
# pick a point on the axis that defines it's location
p = np.array([-1, 0, 0])
# moment vector
m = np.cross(p, l)
theta = np.pi / 2
d = 3.
# this corresponds to a rotation around the axis parallel with the origin's z-axis through the point p
# the resulting transformation should move the origin to a DQ with elements:
desired_dq_rot = DualQuaternion.from_quat_pose_array(
[np.cos(theta / 2), 0, 0,
np.sin(theta / 2), 0, 0, 0])
desired_dq_trans = DualQuaternion.from_translation_vector([-1, 1, d])
desired_dq = desired_dq_trans * desired_dq_rot
dq = DualQuaternion.from_screw(l, m, theta, d)
self.assertEqual(dq, desired_dq)
[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)
dq1 = DualQuaternion.from_translation_vector([0,0,1])
dq2 = DualQuaternion.from_translation_vector([0,1,0])
l1=[0,0,1]; o1 = [10,10,0]; m1 = np.cross(l1,o1); theta1 = np.deg2rad(0); disp1=5;
l2=[0,1,0]; o2 = [10,10,0]; m2 = np.cross(l2,o2); theta2 = np.deg2rad(0); disp2=5;
dq1 = DualQuaternion.from_screw(l1, m1, theta1, disp1) #resultant aff trans is displacement of the origine from screw axis rot
dq2 = DualQuaternion.from_screw(l2, m2, theta2, disp2) #resultant aff trans is displacement of the origine from screw axis rot
dqmean2 = dq1.pow(0.5)*dq2.pow(0.5); get_info_from_dq(dqmean2)
(np.array(l1)*disp1 + np.array(l2)*disp2)/2
#test random rotation matrix
Nmat=1500
rot_euler = np.random.uniform(-20,20,size=(Nmat, 3)) #in degree
aff_eul = [spm_matrix( np.hstack([[0,0,0], r, [1,1,1,0,0,0]]), order=1) for r in rot_euler]
#does not work ... aff_eul = get_affine_rot_from_euler(rot_euler)
#aff_lis = [random_rotation_matrix(20/360) for i in range(Nmat)]
aff_lis = [random_rotation(20) for i in range(Nmat)]
#aff_lis = [random_rotation_matrix() for i in range(1000)]