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_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 blend_poses(poses: Sequence[np.ndarray]) -> np.ndarray:
    """Blend a list of 4x4 transformation matrices together using dual
    quaternion blending.

    See: https://www.cs.utah.edu/~ladislav/kavan06dual/kavan06dual.pdf

    :param poses: A list of poses to blend.
    :return: The result of DQB applied on the input poses.
    """
    dq = DualQuaternion.from_dq_array([0, 0, 0, 0, 0, 0, 0])

    # We use a constant weight for all poses.
    weight = 1.0 / len(poses)

    for p in poses:
        dq += weight * DualQuaternion.from_homogeneous_matrix(p)
    dq.normalize()

    return dq.homogeneous_matrix()