fixed neo

jhavl · jhavl · commit c53bba9b9400 · 2020-11-09T14:06:49.000+10:00
diff --git a/examples/neo.py b/examples/neo.py
@@ -133,4 +133,4 @@
     panda.qd[:n] = qd[:n]
 
     # Step the simulator by 50 ms
-    env.step(50)
+    env.step(0.05)
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -648,19 +648,31 @@ def fkine(self, q=None, from_link=None, to_link=None):
         if q is None:
             q = self.q
 
+        path, n = self.get_path(from_link, to_link)
+
+        use_jindex = True
+
         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)
+        except ValueError:
+            try:
+                q = getvector(q, n, 'col')
+                use_jindex = False
+                j = 0
+            except ValueError:
+                trajn = q.shape[1]
+                verifymatrix(q, (self.n, trajn))
 
         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)
+                    if use_jindex:
+                        T = link.A(q[link.jindex, i], fast=True)
+                    else:
+                        T = link.A(q[j, i], fast=True)
+                        j += 1
                 else:
                     T = link.A(fast=True)
 
@@ -1317,7 +1329,7 @@ def indiv_calculation(link, link_col, q):
             return l_Ain, l_bin, d, wTcp
 
         for link in links:
-            if link.jtype == link.VARIABLE:
+            if link.isjoint:
                 j += 1
 
             for link_col in link.collision:
