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_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_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_normalize(self):
self.assertTrue(self.identity_dq.is_normalized())
self.assertEqual(self.identity_dq.normalized(), self.identity_dq)
unnormalized_dq = DualQuaternion.from_quat_pose_array(
[1, 2, 3, 4, 5, 6, 7])
unnormalized_dq.normalize() # now normalized!
self.assertTrue(unnormalized_dq.is_normalized())
def from_ros_transform(transform_msg):
"""
Create a DualQuaternion instance from a ROS Transform msg
:param transform_msg: geometry_msgs.msg.Transform()
"""
tra = transform_msg.translation
rot = transform_msg.rotation
return DualQuaternion.from_quat_pose_array([rot.w, rot.x, rot.y, rot.z, tra.x, tra.y, tra.z])
def from_ros_pose(pose_msg):
"""
Create a DualQuaternion instance from a ROS Pose msg
:param pose_msg: geometry_msgs.msg.Pose()
"""
tra = pose_msg.position
rot = pose_msg.orientation
return DualQuaternion.from_quat_pose_array([rot.w, rot.x, rot.y, rot.z, tra.x, tra.y, tra.z])
def from_ros(msg):
"""
Create a DualQuaternion instance from a ROS Pose or Transform message
:param msg: geometry_msgs.msg.Pose or geometry_msgs.msg.Transform
"""
try:
tra = msg.position
rot = msg.orientation
except AttributeError:
tra = msg.translation
rot = msg.rotation
return DualQuaternion.from_quat_pose_array([rot.w, rot.x, rot.y, rot.z, tra.x, tra.y, tra.z])
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)