Gripper tests

jhavl · jhavl · commit ad8505a157f5 · 2020-11-23T16:21:50.000+10:00
diff --git a/roboticstoolbox/robot/Gripper.py b/roboticstoolbox/robot/Gripper.py
@@ -20,6 +20,8 @@ def __init__(
             if link.isjoint:
                 self._n += 1
 
+        print(self.n)
+
         self.q = np.zeros(self.n)
         self._links = elinks
 
@@ -46,8 +48,8 @@ def visit_link(link, jindex):
                     raise ValueError(
                         'gripper joint index {link.jindex} was '
                         'repeated or out of range')
-                jset -= set(link.jindex)
-            if len(jset) > 0:
+                jset -= set([link.jindex])
+            if len(jset) > 0:   # pragma nocover # is impossible
                 raise ValueError('gripper joints {jset} were not assigned')
         else:
             # must be a mixture of ELinks with/without jindex
diff --git a/tests/test_Gripper.py b/tests/test_Gripper.py
@@ -0,0 +1,48 @@
+"""
+@author: Jesse Haviland
+"""
+
+import roboticstoolbox as rtb
+import unittest
+
+
+class TestGripper(unittest.TestCase):
+
+    def test_jindex_1(self):
+        e1 = rtb.ELink(rtb.ETS.rz(), jindex=0)
+        e2 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e1)
+        e3 = rtb.ELink(rtb.ETS.rz(), jindex=2, parent=e2)
+        e5 = rtb.ELink(rtb.ETS.rz(), jindex=0, parent=e3)
+        e7 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e5)
+
+        rtb.ERobot([e1, e2, e3, e5, e7], gripper_links=e5)
+
+    def test_jindex_2(self):
+        e1 = rtb.ELink(rtb.ETS.rz(), jindex=0)
+        e2 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e1)
+        e3 = rtb.ELink(rtb.ETS.rz(), jindex=2, parent=e2)
+        e4 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e3)
+
+        with self.assertRaises(ValueError):
+            rtb.ERobot([e1, e2, e3, e4], gripper_links=e4)
+
+    def test_jindex_3(self):
+        e1 = rtb.ELink(rtb.ETS.rz(), jindex=0)
+        e2 = rtb.ELink(rtb.ETS.rz(), jindex=1, parent=e1)
+        e3 = rtb.ELink(rtb.ETS.rz(), jindex=2, parent=e2)
+        e4 = rtb.ELink(rtb.ETS.rz(), jindex=0, parent=e3)
+        e5 = rtb.ELink(rtb.ETS.rz(), parent=e4)
+
+        with self.assertRaises(ValueError):
+            rtb.ERobot([e1, e2, e3, e4, e5], gripper_links=e4)
+
+    def test_jindex_4(self):
+        e1 = rtb.ELink(rtb.ETS.rz())
+        e2 = rtb.ELink(rtb.ETS.rz(), parent=e1)
+        e3 = rtb.ELink(rtb.ETS.rz(), parent=e2)
+        e4 = rtb.ELink(rtb.ETS.rz(), parent=e3)
+        e5 = rtb.ELink(rtb.ETS.rz(), parent=e4)
+        e6 = rtb.ELink(rtb.ETS.rz(), parent=e5)
+        e7 = rtb.ELink(rtb.ETS.rz(), parent=e6)
+
+        rtb.ERobot([e1, e2, e3, e4, e5, e6, e7], gripper_links=e3)
