erobot tests passing

Author: jhavl

examples/swifty.py

@@ -14,7 +14,7 @@
 # env.launch()
 
 # Create a Panda robot object
-panda = rtb.models.Panda()
+panda = rtb.models.ETS.Panda()
 
 # print(panda)
 # print(panda.base_link)

roboticstoolbox/robot/ERobot.py

@@ -139,7 +139,7 @@ def __init__(
 
         # Set the ee links
         self.ee_links = []
-        if gripper_links is None:
+        if len(gripper_links) == 0:
             for link in elinks:
                 # is this a leaf node? and do we not have any grippers
                 if len(link.child) == 0:
@@ -525,8 +525,6 @@ def ee_links(self, link):
 
 # --------------------------------------------------------------------- #
 
-    # TODO would prefer this was called ets, but that name taken for
-    # a property earlier
     def ets(self, ee=None):
         if ee is None:
             if len(self.ee_links) == 1:
@@ -625,22 +623,35 @@ def fkine(self, q=None, from_link=None, to_link=None):
         if to_link is None:
             to_link = self.ee_links[0]
 
-        q = self._getq(q)
+        trajn = 1
+
+        if q is None:
+            q = self.q
+
+        try:
+            q = getvector(q, self.n, 'col')
+        except ValueError:
+            trajn = q.shape[1]
+            verifymatrix(q, (self.n, trajn))
 
         path, _ = self.get_path(from_link, to_link)
-        j = 0
-        tr = self.base.A
 
-        for link in path:
-            if link.isjoint:
-                T = link.A(q[j], fast=True)
-                j += 1
-            else:
-                T = link.A(fast=True)
+        for i in range(trajn):
+            tr = self.base.A
+            for link in path:
+                if link.isjoint:
+                    T = link.A(q[link.jindex, i], fast=True)
+                else:
+                    T = link.A(fast=True)
+
+                tr = tr @ T
 
-            tr = tr @ T
+            if i == 0:
+                t = SE3(tr)
+            else:
+                t.append(SE3(tr))
 
-        return SE3(tr)
+        return t
 
     def fkine_all(self, q=None):
         '''

tests/test_ERobot.py

@@ -107,7 +107,7 @@ def test_fkine(self):
     def test_fkine_traj(self):
         panda = rp.models.ETS.Panda()
         q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
-        qq = np.r_[q, q, q, q]
+        qq = np.c_[q, q, q, q]
 
         ans = np.array([
             [-0.50827907, -0.57904589,  0.63746234,  0.44682295],
@@ -418,7 +418,7 @@ def test_jacobe(self):
     def test_init(self):
         l0 = rp.ELink()
         l1 = rp.ELink(parent=l0)
-        r = rp.ERobot([l0, l1], base=sm.SE3.Rx(1.3), base_link=l1, ee_links=l0)
+        r = rp.ERobot([l0, l1], base=sm.SE3.Rx(1.3), base_link=l1)
         r.base_link = l1
 
         with self.assertRaises(TypeError):