put angles into range [-pi,pi)

update tests

Author: petercorke

roboticstoolbox/models/DH/Puma560.py

@@ -19,6 +19,7 @@
 import numpy as np
 from roboticstoolbox import DHRobot, RevoluteDH
 from spatialmath import SE3
+from spatialmath import base
 
 
 class Puma560(DHRobot):
@@ -260,6 +261,7 @@ def ik3(robot, T, config='lun'):
             else:
                 raise ValueError('bad configuration string')
 
+
             # Solve for theta[1]
             # V114 is defined in equation 43, p.39.
             # r is defined in equation 47, p.39.
@@ -295,8 +297,9 @@ def ik3(robot, T, config='lun'):
                 den = np.cos(theta[1]) * Pz - np.sin(theta[1]) * V114
                 theta[2] = np.arctan2(a3, d4) - np.arctan2(num, den)
 
-            return theta
+            theta = base.angdiff(theta)
 
+            return theta
 
         return self.ikine_6s(T, config, ik3)
 
@@ -307,7 +310,9 @@ def ik3(robot, T, config='lun'):
     print(puma)
     print(puma.dyntable())
     T = puma.fkine(puma.qn)
+    print(puma.ikine_a(T, 'lu').q)
     print(puma.ikine_a(T, 'ru').q)
+    print(puma.ikine_a(T, 'ld').q)
     print(puma.ikine_a(T, 'rd').q)
-    qi = puma.ikine_a(T, 'rd').q
-    puma.plot(qi)
+
+    puma.plot(puma.qz)

tests/test_DHRobot.py

@@ -894,12 +894,12 @@ def test_ikine_a(self):
         sol = puma.ikine_a(T, "u")
         self.assertTrue(sol.success)
         self.assertAlmostEqual(np.linalg.norm(T-puma.fkine(sol.q)), 0, places=6)
-        self.assertTrue(sol.q[1] > -3)
+        self.assertTrue(sol.q[1] > 0)
 
         sol = puma.ikine_a(T, "d")
         self.assertTrue(sol.success)
         self.assertAlmostEqual(np.linalg.norm(T-puma.fkine(sol.q)), 0, places=6)
-        self.assertTrue(sol.q[1] < -3)
+        self.assertTrue(sol.q[1] < 0)
 
         sol = puma.ikine_a(T, "n")
         self.assertTrue(sol.success)