fix for test

Author: AtsushiSakai

ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py

@@ -48,8 +48,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
         except ValueError as e:
             print("Unreachable goal")
 
-        if show_animation:
-            wrist = plot_arm(theta1, theta2, x, y)
+        wrist = plot_arm(theta1, theta2, x, y)
 
         # check goal
         d2goal = np.math.sqrt((wrist[0] - x)**2 + (wrist[1] - y)**2)
@@ -64,23 +63,24 @@ def plot_arm(theta1, theta2, x, y):
     wrist = elbow + \
         np.array([l2 * np.cos(theta1 + theta2), l2 * np.sin(theta1 + theta2)])
 
-    plt.cla()
+    if show_animation:
+        plt.cla()
 
-    plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-')
-    plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-')
+        plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-')
+        plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-')
 
-    plt.plot(shoulder[0], shoulder[1], 'ro')
-    plt.plot(elbow[0], elbow[1], 'ro')
-    plt.plot(wrist[0], wrist[1], 'ro')
+        plt.plot(shoulder[0], shoulder[1], 'ro')
+        plt.plot(elbow[0], elbow[1], 'ro')
+        plt.plot(wrist[0], wrist[1], 'ro')
 
-    plt.plot([wrist[0], x], [wrist[1], y], 'g--')
-    plt.plot(x, y, 'g*')
+        plt.plot([wrist[0], x], [wrist[1], y], 'g--')
+        plt.plot(x, y, 'g*')
 
-    plt.xlim(-2, 2)
-    plt.ylim(-2, 2)
+        plt.xlim(-2, 2)
+        plt.ylim(-2, 2)
 
-    plt.show()
-    plt.pause(dt)
+        plt.show()
+        plt.pause(dt)
 
     return wrist