fixed TypeError (AtsushiSakai#414)

* fixed TypeError

* updated global variables

* updated global variables

* fix comment

* Change try-except to if

Author: raghuramshankar

ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py

@@ -34,9 +34,14 @@
 def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
     """
     Computes the inverse kinematics for a planar 2DOF arm
+    When out of bounds, rewrite x and y with last correct values
     """
+    global x, y
     while True:
         try:
+            if x is not None and y is not None:
+                x_prev = x
+                y_prev = y
             if np.sqrt(x**2 + y**2) > (l1 + l2):
                 theta2_goal = 0
             else:
@@ -54,11 +59,15 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
             theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt
         except ValueError as e:
             print("Unreachable goal")
+        except TypeError:
+            x = x_prev
+            y = y_prev
 
         wrist = plot_arm(theta1, theta2, x, y)
 
         # check goal
-        d2goal = np.hypot(wrist[0] - x, wrist[1] - y)
+        if x is not None and y is not None:
+            d2goal = np.hypot(wrist[0] - x, wrist[1] - y)
 
         if abs(d2goal) < GOAL_TH and x is not None:
             return theta1, theta2
@@ -118,8 +127,8 @@ def main():  # pragma: no cover
     fig = plt.figure()
     fig.canvas.mpl_connect("button_press_event", click)
     # for stopping simulation with the esc key.
-    fig.canvas.mpl_connect('key_release_event',
-            lambda event: [exit(0) if event.key == 'escape' else None])
+    fig.canvas.mpl_connect('key_release_event', lambda event: [
+                           exit(0) if event.key == 'escape' else None])
     two_joint_arm()