def test_inv_6dof(self):
a = np.array([2, 2, 2, np.pi, np.pi, np.pi])
Pa = np.diag([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
r, Pr = mops6dof.inv(a, Pa)
npt.assert_almost_equal(mops6dof.compose(r, a)[0], np.zeros(6))
self.assertEqual(Pr.shape, (6, 6))
def test_compose_6dof(self):
a = np.array([2, 2, 2, np.pi, np.pi, np.pi])
b = np.array([2, 2, 2])
c = np.array([2, 2, 2, np.pi, np.pi, np.pi])
Pa = np.diag([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
Pc = np.diag([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
r_expected = np.array([4., 4., 4., 0., 0., 0.])
r, Pr = mops6dof.compose(a, b)
npt.assert_almost_equal(r, r_expected[0:3])
r, Pr = mops6dof.compose(a, c, Pa, Pc)
npt.assert_almost_equal(r, r_expected)
self.assertEqual(Pr.shape, (6, 6))
