From ccc98957db5846763e36cf8107c30ab29b306a8b Mon Sep 17 00:00:00 2001 From: Samuel Laferriere Date: Sun, 23 Aug 2020 11:27:47 -0400 Subject: [PATCH 1/2] Changed alpha/beta variables to have a nicer semantic. Makes it easier to understand. --- PathTracking/move_to_pose/move_to_pose.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index 2ae0090dda..ee6a7c0973 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -15,7 +15,7 @@ # simulation parameters Kp_rho = 9 -Kp_alpha = 15 +Kp_alpha = -18 Kp_beta = -3 dt = 0.01 @@ -53,9 +53,9 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): # from 0 rad to 2*pi rad with slight turn rho = np.hypot(x_diff, y_diff) - alpha = (np.arctan2(y_diff, x_diff) - - theta + np.pi) % (2 * np.pi) - np.pi - beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi + alpha = (theta - np.arctan2(y_diff, x_diff) + + np.pi) % (2 * np.pi) - np.pi + beta = (theta_goal - theta + np.pi) % (2 * np.pi) - np.pi v = Kp_rho * rho w = Kp_alpha * alpha + Kp_beta * beta From 291b0d5216f1453281c698d61673bebf89017dbf Mon Sep 17 00:00:00 2001 From: Samuel Laferriere Date: Sun, 23 Aug 2020 11:34:02 -0400 Subject: [PATCH 2/2] Changed back sign of alpha to be like before. I made a mistake in previous commit. --- PathTracking/move_to_pose/move_to_pose.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index ee6a7c0973..6a72640a20 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -15,7 +15,7 @@ # simulation parameters Kp_rho = 9 -Kp_alpha = -18 +Kp_alpha = 18 Kp_beta = -3 dt = 0.01 @@ -53,7 +53,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): # from 0 rad to 2*pi rad with slight turn rho = np.hypot(x_diff, y_diff) - alpha = (theta - np.arctan2(y_diff, x_diff) + alpha = (np.arctan2(y_diff, x_diff) - theta + np.pi) % (2 * np.pi) - np.pi beta = (theta_goal - theta + np.pi) % (2 * np.pi) - np.pi