Fix bug in IK config handler

petercorke · petercorke · commit 499407edd49a · 2021-02-05T17:06:18.000+10:00
diff --git a/roboticstoolbox/models/DH/Puma560.py b/roboticstoolbox/models/DH/Puma560.py
@@ -228,7 +228,7 @@ def ikine_a(self, T, config="lun"):
             GTRI/ATRP/IIMB, Georgia Institute of Technology, 2/13/95
  
         """
-        def ik3(robot, T, config):
+        def ik3(robot, T, config='lun'):
 
             # solve for the first three joints
 
@@ -253,8 +253,10 @@ def ik3(robot, T, config):
             r = np.sqrt(Px**2 + Py**2)
             if 'r' in config:
                 theta[0] = np.arctan2(Py, Px) + np.arcsin(d3 / r)
-            else:
+            elif 'l' in config:
                 theta[0] = np.arctan2(Py, Px) + np.pi - np.arcsin(d3 / r)
+            else:
+                raise ValueError('bad configuration string')
 
             # Solve for theta[1]
             # V114 is defined in equation 43, p.39.
@@ -264,10 +266,12 @@ def ik3(robot, T, config):
             # configuration parameter n2
             if 'u' in config:
                 n2 = 1
-            else:
+            elif 'd' in config:
                 n2 = -1
+            else:
+                raise ValueError('bad configuration string')
 
-            if 'r' in config:
+            if 'l' in config:
                 n2 = -n2
 
             V114 = Px * np.cos(theta[0]) + Py * np.sin(theta[0])
@@ -300,3 +304,8 @@ def ik3(robot, T, config):
     puma = Puma560(symbolic=False)
     print(puma)
     print(puma.dyntable())
+    T = puma.fkine(puma.qn)
+    print(puma.ikine_a(T, 'ru').q)
+    print(puma.ikine_a(T, 'rd').q)
+    qi = puma.ikine_a(T, 'rd').q
+    puma.plot(qi)
