Merge pull request #281 from orocos/fix/tests

First step into fixing inconsistent test results

Author: MatthijsBurgh

python_orocos_kdl/tests/PyKDLtest.py

@@ -39,9 +39,10 @@
     import jointtypetest
     suite.addTest(jointtypetest.suite())
 
-result = unittest.TextTestRunner(verbosity=3).run(suite)
+if __name__ == "__main__":
+    result = unittest.TextTestRunner(verbosity=3).run(suite)
 
-if result.wasSuccessful():
-    sys.exit(0)
-else:
-    sys.exit(1)
+    if result.wasSuccessful():
+        sys.exit(0)
+    else:
+        sys.exit(1)

python_orocos_kdl/tests/kinfamtest.py

@@ -75,7 +75,9 @@ def setUp(self):
         self.fksolverpos = ChainFkSolverPos_recursive(self.chain)
         self.fksolvervel = ChainFkSolverVel_recursive(self.chain)
         self.iksolvervel = ChainIkSolverVel_pinv(self.chain)
+        self.iksolvervel_givens = ChainIkSolverVel_pinv_givens(self.chain)
         self.iksolverpos = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel)
+        self.iksolverpos_givens = ChainIkSolverPos_NR(self.chain, self.fksolverpos, self.iksolvervel_givens)
 
     def testRotationalInertia(self):
         ri = RotationalInertia(1, 2, 3, 4, 7, 5)
@@ -201,9 +203,7 @@ def testFkVelAndJac(self):
         MultiplyJacobian(jac, qvel.qdot, t)
         self.assertEqual(cart.deriv(), t)
 
-    def testFkVelAndIkVel(self):
-        epsJ = 1E-7
-
+    def testFkVelAndIkVelImpl(self, fksolvervel, iksolvervel, epsJ):
         q = JntArray(self.chain.getNrOfJoints())
         qdot = JntArray(self.chain.getNrOfJoints())
         for i in range(q.rows()):
@@ -215,12 +215,20 @@ def testFkVelAndIkVel(self):
 
         cart = FrameVel()
 
-        self.assertTrue(0 == self.fksolvervel.JntToCart(qvel, cart))
-        self.assertTrue(0 == self.iksolvervel.CartToJnt(qvel.q, cart.deriv(), qdot_solved))
+        self.assertTrue(0 == fksolvervel.JntToCart(qvel, cart))
+        self.assertTrue(0 == iksolvervel.CartToJnt(qvel.q, cart.deriv(), qdot_solved))
 
         self.assertEqual(qvel.qdot, qdot_solved)
 
-    def testFkPosAndIkPos(self):
+    def testFkVelAndIkVel(self):
+        epsJ = 1e-7
+        self.testFkVelAndIkVelImpl(self.fksolvervel, self.iksolvervel, epsJ)
+
+    def testFkVelAndIkVelGivens(self):
+        epsJ = 1e-7
+        self.testFkVelAndIkVelImpl(self.fksolvervel, self.iksolvervel_givens, epsJ)
+
+    def testFkPosAndIkPosImpl(self, fksolverpos, iksolverpos, epsJ):
         q = JntArray(self.chain.getNrOfJoints())
         for i in range(q.rows()):
             q[i] = random.uniform(-3.14, 3.14)
@@ -234,12 +242,20 @@ def testFkPosAndIkPos(self):
         F1 = Frame.Identity()
         F2 = Frame.Identity()
 
-        self.assertTrue(0 == self.fksolverpos.JntToCart(q, F1))
-        self.assertTrue(0 == self.iksolverpos.CartToJnt(q_init, F1, q_solved))
-        self.assertTrue(0 == self.fksolverpos.JntToCart(q_solved, F2))
+        self.assertTrue(0 == fksolverpos.JntToCart(q, F1))
+        self.assertTrue(0 == iksolverpos.CartToJnt(q_init, F1, q_solved))
+        self.assertTrue(0 == fksolverpos.JntToCart(q_solved, F2))
 
         self.assertEqual(F1, F2)
-        self.assertEqual(q, q_solved)
+        self.assertTrue(Equal(q, q_solved, epsJ), "{} != {}".format(q, q_solved))
+
+    def testFkPosAndIkPos(self):
+        epsJ = 1e-3
+        self.testFkPosAndIkPosImpl(self.fksolverpos, self.iksolverpos, epsJ)
+
+    def testFkPosAndIkPosGivens(self):
+        epsJ = 1e-3
+        self.testFkPosAndIkPosImpl(self.fksolverpos, self.iksolverpos_givens, epsJ)
 
     def compare_Jdot_Diff_vs_Solver(self, dt, representation):
         NrOfJoints = self.chain.getNrOfJoints()
@@ -338,7 +354,9 @@ def suite():
     suite.addTest(KinfamTestFunctions('testFkPosAndJac'))
     suite.addTest(KinfamTestFunctions('testFkVelAndJac'))
     suite.addTest(KinfamTestFunctions('testFkVelAndIkVel'))
+    suite.addTest(KinfamTestFunctions('testFkVelAndIkVelGivens'))
     suite.addTest(KinfamTestFunctions('testFkPosAndIkPos'))
+    suite.addTest(KinfamTestFunctions('testFkPosAndIkPosGivens'))
     suite.addTest(KinfamTestFunctions('testJacDot'))
     suite.addTest(KinfamTestTree('testTreeGetChainMemLeak'))
     return suite