IK work

jhavl · jhavl · commit ebc14891bb5f · 2020-12-01T15:23:38.000+10:00
diff --git a/roboticstoolbox/robot/IK.py b/roboticstoolbox/robot/IK.py
@@ -316,6 +316,7 @@ def ikine_LMS(
         :type tol: float
         :param ωN: damping coefficient
         :type ωN: float (default 1e-3)
+
         :return: inverse kinematic solution
         :rtype: named tuple
 
@@ -396,7 +397,7 @@ def ikine_LMS(
         def angle_axis(T, Td):
             d = base.transl(Td) - base.transl(T)
             R = base.t2r(Td) @ base.t2r(T).T
-            l = np.r_[R[2, 1]-R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]  # noqa
+            l = np.r_[R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]  # noqa
             if base.iszerovec(l):
                 # diagonal matrix case
                 if np.trace(R) > 0:
@@ -436,6 +437,8 @@ def angle_axis(T, Td):
         for Tk in T:
             iterations = 0
             failure = None
+            nm = None
+
             while True:
                 # Update the count and test against iteration limit
                 iterations += 1
