Update move_to_pose for cases where alpha > pi/2 or alpha < -pi/2 (#1168)

* Update move_to_pose for cases where alpha > pi/2 or alpha < -pi/2

* Update move_to_pose

* Add move_to_pose test

* Update move_to_pose

Author: Aglargil

Control/move_to_pose/move_to_pose.py

@@ -5,15 +5,21 @@
 Author: Daniel Ingram (daniel-s-ingram)
         Atsushi Sakai (@Atsushi_twi)
         Seied Muhammad Yazdian (@Muhammad-Yazdian)
+        Wang Zheng (@Aglargil)
 
 P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
 
 """
 
 import matplotlib.pyplot as plt
 import numpy as np
-from random import random
+import sys
+import pathlib
+
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
 from utils.angle import angle_mod
+from random import random
+
 
 class PathFinderController:
     """
@@ -70,21 +76,28 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal):
         # [-pi, pi] to prevent unstable behavior e.g. difference going
         # from 0 rad to 2*pi rad with slight turn
 
+        # Ref: The velocity v always has a constant sign which depends on the initial value of α.
         rho = np.hypot(x_diff, y_diff)
-        alpha = angle_mod(np.arctan2(y_diff, x_diff) - theta)
-        beta = angle_mod(theta_goal - theta - alpha)
         v = self.Kp_rho * rho
-        w = self.Kp_alpha * alpha - self.Kp_beta * beta
 
+        alpha = angle_mod(np.arctan2(y_diff, x_diff) - theta)
+        beta = angle_mod(theta_goal - theta - alpha)
         if alpha > np.pi / 2 or alpha < -np.pi / 2:
+            # recalculate alpha to make alpha in the range of [-pi/2, pi/2]
+            alpha = angle_mod(np.arctan2(-y_diff, -x_diff) - theta)
+            beta = angle_mod(theta_goal - theta - alpha)
+            w = self.Kp_alpha * alpha - self.Kp_beta * beta
             v = -v
+        else:
+            w = self.Kp_alpha * alpha - self.Kp_beta * beta
 
         return rho, v, w
 
 
 # simulation parameters
 controller = PathFinderController(9, 15, 3)
 dt = 0.01
+MAX_SIM_TIME = 5  # seconds, robot will stop moving when time exceeds this value
 
 # Robot specifications
 MAX_LINEAR_SPEED = 15
@@ -101,37 +114,55 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
     x_diff = x_goal - x
     y_diff = y_goal - y
 
-    x_traj, y_traj = [], []
+    x_traj, y_traj, v_traj, w_traj = [x], [y], [0], [0]
 
     rho = np.hypot(x_diff, y_diff)
-    while rho > 0.001:
+    t = 0
+    while rho > 0.001 and t < MAX_SIM_TIME:
+        t += dt
         x_traj.append(x)
         y_traj.append(y)
 
         x_diff = x_goal - x
         y_diff = y_goal - y
 
-        rho, v, w = controller.calc_control_command(
-            x_diff, y_diff, theta, theta_goal)
+        rho, v, w = controller.calc_control_command(x_diff, y_diff, theta, theta_goal)
 
         if abs(v) > MAX_LINEAR_SPEED:
             v = np.sign(v) * MAX_LINEAR_SPEED
 
         if abs(w) > MAX_ANGULAR_SPEED:
             w = np.sign(w) * MAX_ANGULAR_SPEED
 
+        v_traj.append(v)
+        w_traj.append(w)
+
         theta = theta + w * dt
         x = x + v * np.cos(theta) * dt
         y = y + v * np.sin(theta) * dt
 
         if show_animation:  # pragma: no cover
             plt.cla()
-            plt.arrow(x_start, y_start, np.cos(theta_start),
-                      np.sin(theta_start), color='r', width=0.1)
-            plt.arrow(x_goal, y_goal, np.cos(theta_goal),
-                      np.sin(theta_goal), color='g', width=0.1)
+            plt.arrow(
+                x_start,
+                y_start,
+                np.cos(theta_start),
+                np.sin(theta_start),
+                color="r",
+                width=0.1,
+            )
+            plt.arrow(
+                x_goal,
+                y_goal,
+                np.cos(theta_goal),
+                np.sin(theta_goal),
+                color="g",
+                width=0.1,
+            )
             plot_vehicle(x, y, theta, x_traj, y_traj)
 
+    return x_traj, y_traj, v_traj, w_traj
+
 
 def plot_vehicle(x, y, theta, x_traj, y_traj):  # pragma: no cover
     # Corners of triangular vehicle when pointing to the right (0 radians)
@@ -144,16 +175,16 @@ def plot_vehicle(x, y, theta, x_traj, y_traj):  # pragma: no cover
     p2 = np.matmul(T, p2_i)
     p3 = np.matmul(T, p3_i)
 
-    plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-')
-    plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-')
-    plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-')
+    plt.plot([p1[0], p2[0]], [p1[1], p2[1]], "k-")
+    plt.plot([p2[0], p3[0]], [p2[1], p3[1]], "k-")
+    plt.plot([p3[0], p1[0]], [p3[1], p1[1]], "k-")
 
-    plt.plot(x_traj, y_traj, 'b--')
+    plt.plot(x_traj, y_traj, "b--")
 
     # for stopping simulation with the esc key.
     plt.gcf().canvas.mpl_connect(
-        'key_release_event',
-        lambda event: [exit(0) if event.key == 'escape' else None])
+        "key_release_event", lambda event: [exit(0) if event.key == "escape" else None]
+    )
 
     plt.xlim(0, 20)
     plt.ylim(0, 20)
@@ -162,26 +193,31 @@ def plot_vehicle(x, y, theta, x_traj, y_traj):  # pragma: no cover
 
 
 def transformation_matrix(x, y, theta):
-    return np.array([
-        [np.cos(theta), -np.sin(theta), x],
-        [np.sin(theta), np.cos(theta), y],
-        [0, 0, 1]
-    ])
+    return np.array(
+        [
+            [np.cos(theta), -np.sin(theta), x],
+            [np.sin(theta), np.cos(theta), y],
+            [0, 0, 1],
+        ]
+    )
 
 
 def main():
-
     for i in range(5):
         x_start = 20.0 * random()
         y_start = 20.0 * random()
         theta_start: float = 2 * np.pi * random() - np.pi
         x_goal = 20 * random()
         y_goal = 20 * random()
         theta_goal = 2 * np.pi * random() - np.pi
-        print(f"Initial x: {round(x_start, 2)} m\nInitial y: {round(y_start, 2)} m\nInitial theta: {round(theta_start, 2)} rad\n")
-        print(f"Goal x: {round(x_goal, 2)} m\nGoal y: {round(y_goal, 2)} m\nGoal theta: {round(theta_goal, 2)} rad\n")
+        print(
+            f"Initial x: {round(x_start, 2)} m\nInitial y: {round(y_start, 2)} m\nInitial theta: {round(theta_start, 2)} rad\n"
+        )
+        print(
+            f"Goal x: {round(x_goal, 2)} m\nGoal y: {round(y_goal, 2)} m\nGoal theta: {round(theta_goal, 2)} rad\n"
+        )
         move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()

tests/test_move_to_pose.py

@@ -1,13 +1,81 @@
+import itertools
+import numpy as np
 import conftest  # Add root path to sys.path
 from Control.move_to_pose import move_to_pose as m
 
 
-def test_1():
+def test_random():
     m.show_animation = False
     m.main()
 
 
-def test_2():
+def test_stability():
+    """
+    This unit test tests the move_to_pose.py program for stability
+    """
+    m.show_animation = False
+    x_start = 5
+    y_start = 5
+    theta_start = 0
+    x_goal = 1
+    y_goal = 4
+    theta_goal = 0
+    _, _, v_traj, w_traj = m.move_to_pose(
+        x_start, y_start, theta_start, x_goal, y_goal, theta_goal
+    )
+
+    def v_is_change(current, previous):
+        return abs(current - previous) > m.MAX_LINEAR_SPEED
+
+    def w_is_change(current, previous):
+        return abs(current - previous) > m.MAX_ANGULAR_SPEED
+
+    # Check if the speed is changing too much
+    window_size = 10
+    count_threshold = 4
+    v_change = [v_is_change(v_traj[i], v_traj[i - 1]) for i in range(1, len(v_traj))]
+    w_change = [w_is_change(w_traj[i], w_traj[i - 1]) for i in range(1, len(w_traj))]
+    for i in range(len(v_change) - window_size + 1):
+        v_window = v_change[i : i + window_size]
+        w_window = w_change[i : i + window_size]
+
+        v_unstable = sum(v_window) > count_threshold
+        w_unstable = sum(w_window) > count_threshold
+
+        assert not v_unstable, (
+            f"v_unstable in window [{i}, {i + window_size}], unstable count: {sum(v_window)}"
+        )
+        assert not w_unstable, (
+            f"w_unstable in window [{i}, {i + window_size}], unstable count: {sum(w_window)}"
+        )
+
+
+def test_reach_goal():
+    """
+    This unit test tests the move_to_pose.py program for reaching the goal
+    """
+    m.show_animation = False
+    x_start = 5
+    y_start = 5
+    theta_start_list = [0, np.pi / 2, np.pi, 3 * np.pi / 2]
+    x_goal_list = [0, 5, 10]
+    y_goal_list = [0, 5, 10]
+    theta_goal = 0
+    for theta_start, x_goal, y_goal in itertools.product(
+        theta_start_list, x_goal_list, y_goal_list
+    ):
+        x_traj, y_traj, _, _ = m.move_to_pose(
+            x_start, y_start, theta_start, x_goal, y_goal, theta_goal
+        )
+        x_diff = x_goal - x_traj[-1]
+        y_diff = y_goal - y_traj[-1]
+        rho = np.hypot(x_diff, y_diff)
+        assert rho < 0.001, (
+            f"start:[{x_start}, {y_start}, {theta_start}], goal:[{x_goal}, {y_goal}, {theta_goal}], rho: {rho} is too large"
+        )
+
+
+def test_max_speed():
     """
     This unit test tests the move_to_pose.py program for a MAX_LINEAR_SPEED and
     MAX_ANGULAR_SPEED
@@ -18,5 +86,5 @@ def test_2():
     m.main()
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     conftest.run_this_test(__file__)