fix #174 adjusted to work with erobots

Author: jhavl

roboticstoolbox/robot/ERobot.py

@@ -903,7 +903,7 @@ def fkine(self, q, end=None, start=None, tool=None):
         end, start, etool = self._get_limit_links(end, start)
 
         if etool is not None and tool is not None:
-            tool = (etool @ tool).A
+            tool = (etool * tool).A
         elif etool is not None:
             tool = etool.A
         elif tool is not None:

roboticstoolbox/robot/IK.py

@@ -495,7 +495,14 @@ def ikine_LMS(
         tcount = 0  # Total iteration count
 
         # bool vector indicating revolute joints
-        revolutes = np.array([link.isrevolute for link in self])
+        # joints = np.array([link.isjoint for link in self])
+        # revolutes = np.array([link.isrevolute for link in self])
+        revolutes = []
+
+        for link in self:
+            if link.isjoint:
+                revolutes.append(link.isrevolute)
+        revolutes = np.array(revolutes)
 
         q = q0
         for Tk in T:
@@ -531,6 +538,8 @@ def ikine_LMS(
                 # print(J)
 
                 # Wrap angles for revolute joints
+                print(revolutes)
+                print(q)
                 k = np.logical_and(q > np.pi, revolutes)
                 q[k] -= 2 * np.pi