3
import mrol_mapping.poseutil as poseutil
6
class Testposeutil(unittest.TestCase):
9
self.p.append(poseutil.Pose3D())
10
self.p.append(poseutil.Pose3D())
12
def testtuple_matrix(self):
13
X = (0, 0, 0, 0, 0, 0)
14
Y = poseutil.posetuple(poseutil.mat(X))
15
self.assertEquals(X, Y)
16
X = (0.1, -0.2, 0, 0.45, 0.9, -1.1)
17
Y = poseutil.posetuple(poseutil.mat(X))
18
self.assertTrue(np.allclose(X, Y))
20
def testeqandhash(self):
23
P.set([1.2, 2.3, 0.2, 0.12, 0.63, 0.9])
25
Q.setMatrix(P.getMatrix())
27
self.assertEquals(P, Q)
34
self.assertEquals(len(S), 1)
39
self.assertEquals(len(S), 2)
40
#self.assertEquals(self.p[0][0], 0)
41
#self.assertEquals(self.p[1][5], 1)
43
def testcombined(self):
45
combo = p.getCombined(p)
46
self.assertTrue(np.allclose(combo.get(), p.get()))
47
p.set([1, 1, 0, 0, 0, np.radians(45)])
48
combo = p.getCombined(p)
49
expected = poseutil.Pose3D()
50
expected.set([1, 1+2**0.5, 0, 0, 0, np.radians(90)])
51
self.assertTrue(np.allclose(expected.get(), combo.get()))
52
# TODO 3D tests still required
54
def testtransform(self):
56
p[0].rotz = np.radians(90)
57
X = np.matrix('0, 0, 0; 1, 0, 0')
58
Y = p[0].transformPoints(X)
59
expected = np.matrix('0, 0, 0; 0, 1, 0')
60
self.assertTrue(np.allclose(Y, expected)) # pure rotation
64
Y = p[1].transformPoints(X)
65
expected = np.matrix('1, 2, 3; 2, 2, 3')
66
self.assertTrue(np.allclose(Y, expected)) # pure translation
68
Y = p[0].transformPoints(X)
69
expected = np.matrix('1, 0, 0; 1, 1, 0')
70
# it rotates first then displaces
71
self.assertTrue(np.allclose(Y, expected)) # combined
73
def testinverse(self):
75
q = p[0].inverse().inverse()
76
self.assertEquals(p[0], q)
83
self.assertFalse(c is p[0])
84
self.assertEqual(c, p[0])
86
if __name__ == '__main__':