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_screw(self):
# test unit
l, m, theta, d = self.identity_dq.screw()
self.assertEqual(d, 0)
self.assertEqual(theta, 0)
# test pure translation
trans = [10, 5, 0]
dq_trans = DualQuaternion.from_translation_vector(trans)
l, m, theta, d = dq_trans.screw()
self.assertAlmostEqual(d, np.linalg.norm(trans), 2)
self.assertAlmostEqual(theta, 0)
# test pure rotation
theta1 = np.pi / 2
dq_rot = DualQuaternion.from_dq_array(
[np.cos(theta1 / 2),
np.sin(theta1 / 2), 0, 0, 0, 0, 0, 0])
l, m, theta, d = dq_rot.screw()
self.assertAlmostEqual(theta, theta1)
# test simple rotation and translation: rotate in the plane of a coordinate system with the screw axis offset
# along +y. Rotate around z axis so that the coordinate system stays in the plane. Translate along z-axis
theta2 = np.pi / 2
dq_rot2 = DualQuaternion.from_dq_array(
[np.cos(theta2 / 2), 0, 0,
np.sin(theta2 / 2), 0, 0, 0, 0])
dist_axis = 5.
displacement_z = 3.
dq_trans = DualQuaternion.from_translation_vector([
dist_axis * np.sin(theta2), dist_axis * (1. - np.cos(theta2)),
displacement_z
])
dq_comb = dq_trans * dq_rot2
l, m, theta, d = dq_comb.screw()
try:
# the direction of the axis should align with the z axis of the origin
np.testing.assert_array_almost_equal(l,
np.array([0, 0, 1]),
decimal=3)
# m = p x l with p any point on the line
np.testing.assert_array_almost_equal(
np.cross(np.array([[0, dist_axis, 0]]), l).flatten(), m)
except AssertionError as e:
self.fail(e)
self.assertAlmostEqual(
d, displacement_z) # only displacement along z should exist here
self.assertAlmostEqual(theta, theta2) # the angles should be the same
def test_homogeneous_conversion(self):
# 1. starting from a homogeneous matrix
theta1 = np.pi / 2 # 90 deg
trans = [10., 5., 0.]
H1 = np.array([[1., 0., 0., trans[0]],
[0., np.cos(theta1), -np.sin(theta1), trans[1]],
[0., np.sin(theta1),
np.cos(theta1), trans[2]], [0., 0., 0., 1.]])
# check that if we convert to DQ and back to homogeneous matrix, we get the same result
double_conv1 = DualQuaternion.from_homogeneous_matrix(
H1).homogeneous_matrix()
try:
np.testing.assert_array_almost_equal(H1, double_conv1)
except AssertionError as e:
self.fail(e)
# check that dual quaternions are also equal
dq1 = DualQuaternion.from_homogeneous_matrix(H1)
dq_double1 = DualQuaternion.from_homogeneous_matrix(double_conv1)
self.assertEqual(dq1, dq_double1)
# 2. starting from a DQ
dq_trans = DualQuaternion.from_translation_vector([10, 5, 0])
dq_rot = DualQuaternion.from_dq_array(
[np.cos(theta1 / 2),
np.sin(theta1 / 2), 0, 0, 0, 0, 0, 0])
dq2 = dq_trans * dq_rot
# check that this is the same as the previous DQ
self.assertEqual(dq2, dq1)
# check that if we convert to homogeneous matrix and back, we get the same result
double_conv2 = DualQuaternion.from_homogeneous_matrix(
dq2.homogeneous_matrix())
self.assertEqual(dq2, double_conv2)
def test_creation(self):
# from dual quaternion array: careful, need to supply a normalized DQ
dql = np.array([
0.7071067811, 0.7071067811, 0, 0, -3.535533905, 3.535533905,
1.767766952, -1.767766952
])
dq1 = DualQuaternion.from_dq_array(dql)
dq2 = DualQuaternion.from_dq_array(dql)
self.assertEqual(dq1, dq2)
# from quaternion + translation array
dq3 = DualQuaternion.from_quat_pose_array(
np.array([1, 2, 3, 4, 5, 6, 7]))
dq4 = DualQuaternion.from_quat_pose_array([1, 2, 3, 4, 5, 6, 7])
self.assertEqual(dq3, dq4)
# from homogeneous transformation matrix
T = np.array([[1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 1, 1], [0, 0, 0, 1]])
dq7 = DualQuaternion.from_homogeneous_matrix(T)
self.assertEqual(dq7.q_r, quaternion.one)
self.assertEqual(dq7.translation(), [2, 3, 1])
try:
np.testing.assert_array_almost_equal(dq7.homogeneous_matrix(), T)
except AssertionError as e:
self.fail(e)
# from a point
dq8 = DualQuaternion.from_translation_vector([4, 6, 8])
self.assertEqual(dq8.translation(), [4, 6, 8])
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)
def test_add(self):
dq1 = DualQuaternion.from_translation_vector([4, 6, 8])
dq2 = DualQuaternion.from_translation_vector([1, 2, 3])
sum = dq1 + dq2
self.assertEqual(sum.q_d, np.quaternion(0., 2.5, 4., 5.5))
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)
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