diff --git a/Control/move_to_pose/move_to_pose.py b/Control/move_to_pose/move_to_pose.py index 279ba0625b..34736a2e21 100644 --- a/Control/move_to_pose/move_to_pose.py +++ b/Control/move_to_pose/move_to_pose.py @@ -5,6 +5,7 @@ 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 @@ -12,8 +13,13 @@ 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,14 +76,20 @@ 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 @@ -85,6 +97,7 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal): # 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,18 +114,19 @@ 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 @@ -120,18 +134,35 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): 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,15 +193,16 @@ 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() @@ -178,10 +210,14 @@ def main(): 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() diff --git a/tests/test_move_to_pose.py b/tests/test_move_to_pose.py index 8bc11a8d24..94c3ec1102 100644 --- a/tests/test_move_to_pose.py +++ b/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__)