add test for trinv

petercorke · petercorke · commit 573ef2f725d1 · 2020-05-18T18:02:33.000+10:00
diff --git a/spatialmath/base/test_transforms.py b/spatialmath/base/test_transforms.py
@@ -446,6 +446,16 @@ def test_plot(self):
 
 class Test3D(unittest.TestCase):
 
+    def test_trinv(self):
+        T = np.eye(4)
+        nt.assert_array_almost_equal(trinv(T), T)
+        
+        T = trotx(0.3)
+        nt.assert_array_almost_equal(trinv(T)@T, np.eye(4))
+        
+        T = transl(1,2,3)
+        nt.assert_array_almost_equal(trinv(T)@T, np.eye(4))
+        
     def test_rotx(self):
         R = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
         nt.assert_array_almost_equal(rotx(0), R)
