Beispiel #1
0
    def test_compose_3dof(self):
        a = np.array([5., 5., np.pi])
        c = np.array([1., 2., np.pi])

        Pa = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 0.5]])
        Pc = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])

        r, Pr = mops3dof.compose(a, c)
        npt.assert_almost_equal(r, np.array([4., 3., 0.]))

        r, Pr = mops3dof.compose(a, c, Pa, Pc)
        npt.assert_almost_equal(r, np.array([4., 3., 0.]))
        self.assertEqual(Pr.shape, (3, 3))
Beispiel #2
0
    def test_inv_3dof(self):
        a = np.array([1.0, 1.0, np.pi])
        Pa = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 0.5]])

        r, Pr = mops3dof.inv(a, Pa)
        npt.assert_almost_equal(mops3dof.compose(r, a)[0], np.zeros(3))
        self.assertEqual(Pr.shape, (3, 3))