use pytest for test runner (AtsushiSakai#452)

Author: AtsushiSakai

.github/pull_request_template.md

@@ -1,7 +1,7 @@
 <!-- 
 Thanks for contributing a pull request! 
 Please ensure that your PR satisfies the checklist before submitting:
-- [] This project only accept codes for python 3.8 or higher.
+- [] This project only accept codes for python 3.9 or higher.
 - [] If you add a new algorithm sample code, please add a unit test file under `test` dir.
      This sample test code might help you : https://github.com/AtsushiSakai/PythonRobotics/blob/master/tests/test_a_star.py
 - [] If you fix a bug of existing code please add a test function in the test code to show the issue was solved.

ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py

@@ -5,8 +5,10 @@
 Author: Daniel Ingram (daniel-s-ingram)
         Atsushi Sakai (@Atsushi_twi)
 
-Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 p102
-- [Robotics, Vision and Control \| SpringerLink](https://link.springer.com/book/10.1007/978-3-642-20144-8)
+Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017,
+ ISBN 978-3-319-54413-7 p102
+- [Robotics, Vision and Control]
+(https://link.springer.com/book/10.1007/978-3-642-20144-8)
 
 """
 
@@ -37,6 +39,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
     When out of bounds, rewrite x and y with last correct values
     """
     global x, y
+    x_prev, y_prev = None, None
     while True:
         try:
             if x is not None and y is not None:
@@ -47,33 +50,36 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
             else:
                 theta2_goal = np.arccos(
                     (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2))
-            theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 *
-                                                              np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))
+            tmp = np.math.atan2(l2 * np.sin(theta2_goal),
+                                (l1 + l2 * np.cos(theta2_goal)))
+            theta1_goal = np.math.atan2(y, x) - tmp
 
             if theta1_goal < 0:
                 theta2_goal = -theta2_goal
-                theta1_goal = np.math.atan2(
-                    y, x) - np.math.atan2(l2 * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))
+                tmp = np.math.atan2(l2 * np.sin(theta2_goal),
+                                    (l1 + l2 * np.cos(theta2_goal)))
+                theta1_goal = np.math.atan2(y, x) - tmp
 
             theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt
             theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt
         except ValueError as e:
-            print("Unreachable goal")
+            print("Unreachable goal"+e)
         except TypeError:
             x = x_prev
             y = y_prev
 
         wrist = plot_arm(theta1, theta2, x, y)
 
         # check goal
+        d2goal = None
         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
 
 
-def plot_arm(theta1, theta2, x, y):  # pragma: no cover
+def plot_arm(theta1, theta2, target_x, target_y):  # pragma: no cover
     shoulder = np.array([0, 0])
     elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
     wrist = elbow + \
@@ -89,8 +95,8 @@ def plot_arm(theta1, theta2, x, y):  # pragma: no cover
         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], target_x], [wrist[1], target_y], 'g--')
+        plt.plot(target_x, target_y, 'g*')
 
         plt.xlim(-2, 2)
         plt.ylim(-2, 2)