From 4ef4658f82c4baa4784b430e67847ed24aa8dd48 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:18:00 +0100 Subject: [PATCH] Replaced sqrt(x**2+y**2) with hypot in PathTracking/move_to_pose/move_to_pose.py --- 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 0814ef62..e161b461 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -40,7 +40,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): x_traj, y_traj = [], [] - rho = np.sqrt(x_diff**2 + y_diff**2) + rho = np.hypot(x_diff, y_diff) while rho > 0.001: x_traj.append(x) y_traj.append(y) @@ -52,7 +52,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): # [-pi, pi] to prevent unstable behavior e.g. difference going # from 0 rad to 2*pi rad with slight turn - rho = np.sqrt(x_diff**2 + y_diff**2) + 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