def setUp(self):
self.identity_dq = DualQuaternion.identity()
self.random_dq = DualQuaternion.from_quat_pose_array(
np.array([1, 2, 3, 4, 5, 6, 7]))
self.other_random_dq = DualQuaternion.from_quat_pose_array(
np.array([0.2, 0.1, 0.3, 0.07, 1.2, 0.9, 0.2]))
self.normalized_dq = self.random_dq.normalized()
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_equal(self):
self.assertEqual(self.identity_dq, DualQuaternion.identity())
self.assertEqual(
self.identity_dq,
DualQuaternion(-np.quaternion(1, 0, 0, 0),
-np.quaternion(0, 0, 0, 0)))
self.assertFalse(self.identity_dq == DualQuaternion(
np.quaternion(1, 0, 0, 1), -np.quaternion(0, 0, 0, 0)))
theta1 = np.pi / 180 * 20 # 20 deg
T_pure_rot = np.array([[1., 0., 0., 0.],
[0., np.cos(theta1), -np.sin(theta1), 0.],
[0., np.sin(theta1),
np.cos(theta1), 0.], [0., 0., 0., 1.]])
dq_pure_rot = DualQuaternion.from_homogeneous_matrix(T_pure_rot)
# manually flip sign on terms
dq_pure_rot.q_r = -dq_pure_rot.q_r
dq_pure_rot.q_d = -dq_pure_rot.q_d
try:
np.testing.assert_array_almost_equal(
dq_pure_rot.homogeneous_matrix(), T_pure_rot)
except AssertionError as e:
self.fail(e)
dq_pure_rot.q_d = -dq_pure_rot.q_d
try:
np.testing.assert_array_almost_equal(
dq_pure_rot.homogeneous_matrix(), T_pure_rot)
except AssertionError as e:
self.fail(e)
dq_pure_rot.q_r = -dq_pure_rot.q_r
try:
np.testing.assert_array_almost_equal(
dq_pure_rot.homogeneous_matrix(), T_pure_rot)
except AssertionError as e:
self.fail(e)
def test_quaternion_conjugate(self):
dq = self.normalized_dq * self.normalized_dq.quaternion_conjugate()
# a normalized quaternion multiplied with its quaternion conjugate should yield unit dual quaternion
self.assertEqual(dq, DualQuaternion.identity())
# test that the conjugate corresponds to the inverse of it's matrix representation
matr = self.normalized_dq.homogeneous_matrix()
inv = np.linalg.inv(matr)
self.assertEqual(DualQuaternion.from_homogeneous_matrix(inv),
self.normalized_dq.quaternion_conjugate())
# (dq1 @ dq2)* ?= dq2* @ dq1*
res1 = (self.random_dq * self.other_random_dq).quaternion_conjugate()
res2 = self.other_random_dq.quaternion_conjugate(
) * self.random_dq.quaternion_conjugate()
self.assertEqual(res1, res2)
def setUp(self):
self.identity_dq = DualQuaternion.identity()
self.identity_pose = Pose(Point(0., 0., 0.),
Quaternion(0., 0., 0., 1.))
self.identity_transform = Transform(Vector3(0., 0., 0.),
Quaternion(0., 0., 0., 1.))