From 652c32fba9eadfeb1e2200b2f7e475de1acfc5bb Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:10:53 +0100 Subject: [PATCH 01/38] Replaced sqrt(x**2+y**2) with hypot in SLAM/GraphBasedSLAM/graph_based_slam.py --- SLAM/GraphBasedSLAM/graph_based_slam.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index b40ad649..41b9d525 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -216,7 +216,7 @@ def observation(xTrue, xd, u, RFID): dx = RFID[i, 0] - xTrue[0, 0] dy = RFID[i, 1] - xTrue[1, 0] - d = math.sqrt(dx**2 + dy**2) + d = math.hypot(dx, dy) angle = pi_2_pi(math.atan2(dy, dx)) - xTrue[2, 0] phi = pi_2_pi(math.atan2(dy, dx)) if d <= MAX_RANGE: From 1d03241d5d6319fc1cdc42f6a11b560ee23dc0d2 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:11:28 +0100 Subject: [PATCH 02/38] Replaced sqrt(x**2+y**2) with hypot in SLAM/FastSLAM2/fast_slam2.py --- SLAM/FastSLAM2/fast_slam2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py index 73624dbf..b9df6948 100644 --- a/SLAM/FastSLAM2/fast_slam2.py +++ b/SLAM/FastSLAM2/fast_slam2.py @@ -304,7 +304,7 @@ def observation(xTrue, xd, u, RFID): dx = RFID[i, 0] - xTrue[0, 0] dy = RFID[i, 1] - xTrue[1, 0] - d = math.sqrt(dx ** 2 + dy ** 2) + d = math.hypot(dx, dy) angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0]) if d <= MAX_RANGE: dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise From f1926bdd9e08733f7dce997fab3f0e6f4e97476f Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:11:57 +0100 Subject: [PATCH 03/38] Replaced sqrt(x**2+y**2) with hypot in SLAM/FastSLAM2/fast_slam1.py --- SLAM/FastSLAM1/fast_slam1.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index c16407e8..298cc036 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/SLAM/FastSLAM1/fast_slam1.py @@ -279,7 +279,7 @@ def observation(xTrue, xd, u, RFID): dx = RFID[i, 0] - xTrue[0, 0] dy = RFID[i, 1] - xTrue[1, 0] - d = math.sqrt(dx ** 2 + dy ** 2) + d = math.hypot(dx, dy) angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0]) if d <= MAX_RANGE: dn = d + np.random.randn() * Qsim[0, 0] ** 0.5 # add noise From 02f14e425efab343a26acbbf2feeb139cbe337b6 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:13:20 +0100 Subject: [PATCH 04/38] Replaced sqrt(x**2+y**2) with hypot in SLAM/EKFSLAM/ekf_slam.py --- SLAM/EKFSLAM/ekf_slam.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index 22ccd8d6..66234b57 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -76,7 +76,7 @@ def observation(xTrue, xd, u, RFID): dx = RFID[i, 0] - xTrue[0, 0] dy = RFID[i, 1] - xTrue[1, 0] - d = math.sqrt(dx ** 2 + dy ** 2) + d = math.hypot(dx, dy) angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0]) if d <= MAX_RANGE: dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise From 41bcdf8573e4cd3b1773934f8b30d0363edda0f3 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:14:48 +0100 Subject: [PATCH 05/38] Replaced sqrt(x**2+y**2) with hypot in PathTracking/rear_wheel_feedback/rear_wheel_feedback.py --- PathTracking/rear_wheel_feedback/rear_wheel_feedback.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index 285d9404..bf4f31b3 100644 --- a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py +++ b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py @@ -136,7 +136,7 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): # check goal dx = state.x - goal[0] dy = state.y - goal[1] - if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis: + if math.hypot(dx, dy) <= goal_dis: print("Goal") goal_flag = True break From e500f65d7ff6f8ddc82b5c9375fe8e423f8af387 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:16:12 +0100 Subject: [PATCH 06/38] Replaced sqrt(x**2+y**2) with hypot in PathTracking/pure_pursuit/pure_pursuit.py --- PathTracking/pure_pursuit/pure_pursuit.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 38a84aff..370cac96 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -77,7 +77,7 @@ def calc_distance(state, point_x, point_y): dx = state.rear_x - point_x dy = state.rear_y - point_y - return math.sqrt(dx ** 2 + dy ** 2) + return math.hypot(dx, dy) def calc_target_index(state, cx, cy): @@ -88,7 +88,7 @@ def calc_target_index(state, cx, cy): # search nearest point index dx = [state.rear_x - icx for icx in cx] dy = [state.rear_y - icy for icy in cy] - d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] + d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)] ind = d.index(min(d)) old_nearest_point_index = ind else: @@ -110,7 +110,7 @@ def calc_target_index(state, cx, cy): while Lf > L and (ind + 1) < len(cx): dx = cx[ind] - state.rear_x dy = cy[ind] - state.rear_y - L = math.sqrt(dx ** 2 + dy ** 2) + L = math.hypot(dx, dy) ind += 1 return ind From 4ef4658f82c4baa4784b430e67847ed24aa8dd48 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:18:00 +0100 Subject: [PATCH 07/38] 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 From 9f5b0a25cc4c0727087dddde445862e727f283f5 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:19:05 +0100 Subject: [PATCH 08/38] Replaced sqrt(x**2+y**2) with hypot in PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py --- .../model_predictive_speed_and_steer_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py index 0f5104e9..a16768e3 100644 --- a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py +++ b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py @@ -348,7 +348,7 @@ def check_goal(state, goal, tind, nind): # check goal dx = state.x - goal[0] dy = state.y - goal[1] - d = math.sqrt(dx ** 2 + dy ** 2) + d = math.hypot(dx, dy) isgoal = (d <= GOAL_DIS) From db78f07c0bc084748632bf36af713d249bf0647c Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:19:41 +0100 Subject: [PATCH 09/38] Replaced sqrt(x**2+y**2) with hypot in PathTracking/lqr_steer_control/lqr_steer_control.py --- PathTracking/lqr_steer_control/lqr_steer_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py index f977cdc2..b69577d3 100644 --- a/PathTracking/lqr_steer_control/lqr_steer_control.py +++ b/PathTracking/lqr_steer_control/lqr_steer_control.py @@ -191,7 +191,7 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): # check goal dx = state.x - goal[0] dy = state.y - goal[1] - if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis: + if math.hypot(dx, dy) <= goal_dis: print("Goal") break From bc2b7c969cd37b572372c5cdaa36b680a02f7c56 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:20:12 +0100 Subject: [PATCH 10/38] Replaced sqrt(x**2+y**2) with hypot in PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py --- PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py index 6dec12bd..c7c8e0dd 100644 --- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py +++ b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py @@ -212,7 +212,7 @@ def do_simulation(cx, cy, cyaw, ck, speed_profile, goal): # check goal dx = state.x - goal[0] dy = state.y - goal[1] - if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis: + if math.hypot(dx, dy) <= goal_dis: print("Goal") break From 26cff8e69fdbe7c11a55b83a3668e85e24aa952d Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:20:58 +0100 Subject: [PATCH 11/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/RRT/rrt.py --- PathPlanning/RRT/rrt.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 3bcc6221..5b6f3252 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -126,7 +126,7 @@ class RRT: def calc_dist_to_goal(self, x, y): dx = x - self.end.x dy = y - self.end.y - return math.sqrt(dx ** 2 + dy ** 2) + return math.hypot(dx, dy) def get_random_node(self): if random.randint(0, 100) > self.goal_sample_rate: @@ -186,7 +186,7 @@ class RRT: def calc_distance_and_angle(from_node, to_node): dx = to_node.x - from_node.x dy = to_node.y - from_node.y - d = math.sqrt(dx ** 2 + dy ** 2) + d = math.hypot(dx, dy) theta = math.atan2(dy, dx) return d, theta From 5221a9df2a82d9a527930c59057cfaa398c6f403 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:21:29 +0100 Subject: [PATCH 12/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/InformedRRTStar/informed_rrt_star.py --- PathPlanning/InformedRRTStar/informed_rrt_star.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 17574918..065868f2 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -106,7 +106,7 @@ class InformedRRTStar: for i in nearInds: dx = newNode.x - self.node_list[i].x dy = newNode.y - self.node_list[i].y - d = math.sqrt(dx ** 2 + dy ** 2) + d = math.hypot(dx, dy) theta = math.atan2(dy, dx) if self.check_collision(self.node_list[i], theta, d): dList.append(self.node_list[i].cost + d) @@ -224,7 +224,7 @@ class InformedRRTStar: if self.check_collision(nearNode, theta, d): nearNode.parent = n_node - 1 nearNode.cost = scost - + @staticmethod def distance_squared_point_to_segment(v, w, p): # Return minimum distance between line segment vw and point p @@ -232,7 +232,7 @@ class InformedRRTStar: return (p-v).dot(p-v) # v == w case l2 = (w-v).dot(w-v) # i.e. |w-v|^2 - avoid a sqrt # Consider the line extending the segment, parameterized as v + t (w - v). - # We find projection of point p onto the line. + # We find projection of point p onto the line. # It falls where t = [(p-v) . (w-v)] / |w-v|^2 # We clamp t from [0,1] to handle points outside the segment vw. t = max(0, min(1, (p - v).dot(w - v) / l2)) From 06cbcc3ee55598f54b612c2f842cfb052f8b751e Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:22:14 +0100 Subject: [PATCH 13/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py --- .../GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py index f22b058f..9d7e1616 100644 --- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py +++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py @@ -118,7 +118,7 @@ def find_sweep_direction_and_start_posi(ox, oy): for i in range(len(ox) - 1): dx = ox[i + 1] - ox[i] dy = oy[i + 1] - oy[i] - d = np.sqrt(dx ** 2 + dy ** 2) + d = np.hypot(dx, dy) if d > max_dist: max_dist = d From 1eb3ebbf26be7f000275f2a897d46a008613814e Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:22:47 +0100 Subject: [PATCH 14/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/DynamicWindowApproach/dynamic_window_approach.py --- PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index c8a44349..5c3d128d 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -164,7 +164,7 @@ def calc_obstacle_cost(trajectory, ob, config): oy = ob[:, 1] dx = trajectory[:, 0] - ox[:, None] dy = trajectory[:, 1] - oy[:, None] - r = np.sqrt(np.square(dx) + np.square(dy)) + r = np.hypot(dx, dy) if config.robot_type == RobotType.rectangle: yaw = trajectory[:, 2] From 00f8fd37d367aa6409b70e945356f1aa5abc4fde Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:24:27 +0100 Subject: [PATCH 15/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/ClosedLoopRRTStar/pure_pursuit.py --- PathPlanning/ClosedLoopRRTStar/pure_pursuit.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index 04907279..32faa51c 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py @@ -68,17 +68,17 @@ def calc_target_index(state, cx, cy): dx = [state.x - icx for icx in cx] dy = [state.y - icy for icy in cy] - d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] + d = np.hypot(dx, dy) mindis = min(d) - ind = d.index(mindis) + ind = np.argmin(d) L = 0.0 while Lf > L and (ind + 1) < len(cx): dx = cx[ind + 1] - cx[ind] dy = cy[ind + 1] - cy[ind] - L += math.sqrt(dx ** 2 + dy ** 2) + L += math.hypot(dx, dy) ind += 1 # print(mindis) @@ -121,7 +121,7 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal): # check goal dx = state.x - goal[0] dy = state.y - goal[1] - if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis: + if math.hypot(dx, dy) <= goal_dis: find_goal = True break From 1fcfa72133522ce004b99ad079d9f7361ee8f981 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:30:04 +0100 Subject: [PATCH 16/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py --- PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index a07dc48a..aea00801 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -6,7 +6,6 @@ author: AtsushiSakai(@Atsushi_twi) """ -import math import os import sys @@ -119,9 +118,8 @@ class ClosedLoopRRTStar(RRTStarReedsShepp): print("final angle is bad") find_goal = False - travel = sum([abs(iv) * unicycle_model.dt for iv in v]) - origin_travel = sum([math.sqrt(dx ** 2 + dy ** 2) - for (dx, dy) in zip(np.diff(cx), np.diff(cy))]) + travel = unicycle_model.dt * sum(np.abs(v)) + origin_travel = sum(np.hypot(np.diff(cx), np.diff(cy))) if (travel / origin_travel) >= self.invalid_travel_ratio: print("path is too long") From 0aaf6406d82f7067af460160f4d75495d59fa80f Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:30:39 +0100 Subject: [PATCH 17/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/BezierPath/bezier_path.py --- PathPlanning/BezierPath/bezier_path.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/BezierPath/bezier_path.py b/PathPlanning/BezierPath/bezier_path.py index 516a681e..46faf1f3 100644 --- a/PathPlanning/BezierPath/bezier_path.py +++ b/PathPlanning/BezierPath/bezier_path.py @@ -26,7 +26,7 @@ def calc_4points_bezier_path(sx, sy, syaw, ex, ey, eyaw, offset): :param offset: (float) :return: (numpy array, numpy array) """ - dist = np.sqrt((sx - ex) ** 2 + (sy - ey) ** 2) / offset + dist = np.hypot(sx - ex, sy - ey) / offset control_points = np.array( [[sx, sy], [sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)], From 72a28b4d4b850e3c2ac7e13ffacc2ea01736f0db Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:31:05 +0100 Subject: [PATCH 18/38] Replaced sqrt(x**2+y**2) with hypot in Mapping/rectangle_fitting/rectangle_fitting.py --- Mapping/rectangle_fitting/rectangle_fitting.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py index e40fc2e4..2018f025 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/Mapping/rectangle_fitting/rectangle_fitting.py @@ -162,7 +162,7 @@ class LShapeFitting(): C = set() R = self.R0 + self.Rd * np.linalg.norm([ox[i], oy[i]]) for j, _ in enumerate(ox): - d = np.sqrt((ox[i] - ox[j])**2 + (oy[i] - oy[j])**2) + d = np.hypot(ox[i] - ox[j], oy[i] - oy[j]) if d <= R: C.add(j) S.append(C) From 2cb9e37792180fed4d73ce5db35a071ab10d2d17 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:32:02 +0100 Subject: [PATCH 19/38] Replaced sqrt(x**2+y**2) with hypot in Mapping/kmeans_clustering/kmeans_clustering.py --- Mapping/kmeans_clustering/kmeans_clustering.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Mapping/kmeans_clustering/kmeans_clustering.py b/Mapping/kmeans_clustering/kmeans_clustering.py index bdaa4545..c2518b80 100644 --- a/Mapping/kmeans_clustering/kmeans_clustering.py +++ b/Mapping/kmeans_clustering/kmeans_clustering.py @@ -68,7 +68,7 @@ class Clusters: dx = [icx - px for icx in self.center_x] dy = [icy - py for icy in self.center_y] - dist_list = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)] + dist_list = [math.hypot(idx, idy) for (idx, idy) in zip(dx, dy)] min_dist = min(dist_list) min_id = dist_list.index(min_dist) self.labels[ip] = min_id From 4545084da430bd353239565715bb22dde1c1c96f Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:32:40 +0100 Subject: [PATCH 20/38] Replaced sqrt(x**2+y**2) with hypot in Localization/particle_filter/particle_filter.py --- Localization/particle_filter/particle_filter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 0ebb5fc3..d14b7c06 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -47,7 +47,7 @@ def observation(xTrue, xd, u, RF_ID): dx = xTrue[0, 0] - RF_ID[i, 0] dy = xTrue[1, 0] - RF_ID[i, 1] - d = math.sqrt(dx ** 2 + dy ** 2) + d = math.hypot(dx, dy) if d <= MAX_RANGE: dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise zi = np.array([[dn, RF_ID[i, 0], RF_ID[i, 1]]]) @@ -116,7 +116,7 @@ def pf_localization(px, pw, z, u): for i in range(len(z[:, 0])): dx = x[0, 0] - z[i, 1] dy = x[1, 0] - z[i, 2] - pre_z = math.sqrt(dx ** 2 + dy ** 2) + pre_z = math.hypot(dx, dy) dz = pre_z - z[i, 0] w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0])) From f66a10f42eca2a57cb8004f16403fd67e0788d58 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:33:21 +0100 Subject: [PATCH 21/38] Replaced sqrt(x**2+y**2) with hypot in Localization/histogram_filter/histogram_filter.py --- Localization/histogram_filter/histogram_filter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index bdfbb13f..8404646c 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -126,7 +126,7 @@ def observation(xTrue, u, RFID): dx = xTrue[0, 0] - RFID[i, 0] dy = xTrue[1, 0] - RFID[i, 1] - d = math.sqrt(dx ** 2 + dy ** 2) + d = math.hypot(dx, dy) if d <= MAX_RANGE: # add noise to range observation dn = d + np.random.randn() * NOISE_RANGE From e289d1f9a9ea1a1284ab3f4e34cd89c5c21c72b8 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:33:52 +0100 Subject: [PATCH 22/38] Replaced sqrt(x**2+y**2) with hypot in Localization/ensemble_kalman_filter/ensemble_kalman_filter.py --- Localization/ensemble_kalman_filter/ensemble_kalman_filter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py index 29350915..3e2af749 100644 --- a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py +++ b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py @@ -44,7 +44,7 @@ def observation(xTrue, xd, u, RFID): dx = RFID[i, 0] - xTrue[0, 0] dy = RFID[i, 1] - xTrue[1, 0] - d = math.sqrt(dx ** 2 + dy ** 2) + d = math.hypot(dx, dy) angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0]) if d <= MAX_RANGE: dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise From 84aead6149b885b44ad661b92cd8fa2ac6431a41 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:35:06 +0100 Subject: [PATCH 23/38] Replaced sqrt(x**2+y**2) with hypot in ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py --- .../two_joint_arm_to_point_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py index e674ae8a..0e91be0f 100644 --- a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py +++ b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py @@ -55,7 +55,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0): wrist = plot_arm(theta1, theta2, x, y) # check goal - d2goal = np.math.sqrt((wrist[0] - x)**2 + (wrist[1] - y)**2) + d2goal = np.hypot(wrist[0] - x, wrist[1] - y) if abs(d2goal) < GOAL_TH and x is not None: return theta1, theta2 From d979bb24d9cf6272a24bd000f5642cc1443c7b86 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:35:44 +0100 Subject: [PATCH 24/38] Replaced sqrt(x**2+y**2) with hypot in ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py --- .../n_joint_arm_to_point_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py index b232ab33..3972474b 100644 --- a/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py +++ b/ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py @@ -148,7 +148,7 @@ def jacobian_inverse(link_lengths, joint_angles): def distance_to_goal(current_pos, goal_pos): x_diff = goal_pos[0] - current_pos[0] y_diff = goal_pos[1] - current_pos[1] - return np.array([x_diff, y_diff]).T, np.math.sqrt(x_diff**2 + y_diff**2) + return np.array([x_diff, y_diff]).T, np.hypot(x_diff, y_diff) def ang_diff(theta1, theta2): From d26eb04ae2c2f1047271e913977f27dfc129aad3 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:36:25 +0100 Subject: [PATCH 25/38] Removed useless zero in erialNavigation/rocket_powered_landing/rocket_powered_landing.py --- .../rocket_powered_landing/rocket_powered_landing.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py index 9f1e16fc..dde76f19 100644 --- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py +++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py @@ -195,8 +195,8 @@ class Rocket_Model_6DoF: [0, 0, 0], [0, 0, 0], [0, 0, 0], - [0, 0, 1.00000000000000], - [0, -1.00000000000000, 0] + [0, 0, 1.0], + [0, -1.0, 0] ]) def euler_to_quat(self, a): From e467f4fad1d055b947f565ac5d27e6dd21528621 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:07 +0100 Subject: [PATCH 26/38] Replaced sqrt(x**2+y**2) with hypot in Localization/histogram_filter/histogram_filter.py --- Localization/histogram_filter/histogram_filter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index 8404646c..db715a6d 100644 --- a/Localization/histogram_filter/histogram_filter.py +++ b/Localization/histogram_filter/histogram_filter.py @@ -68,7 +68,7 @@ def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std): # predicted range x = ix * gmap.xy_reso + gmap.minx y = iy * gmap.xy_reso + gmap.miny - d = math.sqrt((x - z[iz, 1]) ** 2 + (y - z[iz, 2]) ** 2) + d = math.hypot(x - z[iz, 1], y - z[iz, 2]) # likelihood pdf = (1.0 - norm.cdf(abs(d - z[iz, 0]), 0.0, std)) From ccf047135e558332f9e460e21f8263bfb3e2974a Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:07 +0100 Subject: [PATCH 27/38] Replaced sqrt(x**2+y**2) with hypot in Mapping/gaussian_grid_map/gaussian_grid_map.py --- Mapping/gaussian_grid_map/gaussian_grid_map.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Mapping/gaussian_grid_map/gaussian_grid_map.py b/Mapping/gaussian_grid_map/gaussian_grid_map.py index b520b639..9c1aa273 100644 --- a/Mapping/gaussian_grid_map/gaussian_grid_map.py +++ b/Mapping/gaussian_grid_map/gaussian_grid_map.py @@ -31,7 +31,7 @@ def generate_gaussian_grid_map(ox, oy, xyreso, std): # Search minimum distance mindis = float("inf") for (iox, ioy) in zip(ox, oy): - d = math.sqrt((iox - x)**2 + (ioy - y)**2) + d = math.hypot(iox - x, ioy - y) if mindis >= d: mindis = d From 1273934a7ea2723375422dc29081a014f649dbb8 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:07 +0100 Subject: [PATCH 28/38] Replaced sqrt(x**2+y**2) with hypot in Mapping/raycasting_grid_map/raycasting_grid_map.py --- Mapping/raycasting_grid_map/raycasting_grid_map.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Mapping/raycasting_grid_map/raycasting_grid_map.py b/Mapping/raycasting_grid_map/raycasting_grid_map.py index 99768742..a0d90609 100644 --- a/Mapping/raycasting_grid_map/raycasting_grid_map.py +++ b/Mapping/raycasting_grid_map/raycasting_grid_map.py @@ -57,7 +57,7 @@ def precasting(minx, miny, xw, yw, xyreso, yawreso): px = ix * xyreso + minx py = iy * xyreso + miny - d = math.sqrt(px**2 + py**2) + d = math.hypot(px, py) angle = atan_zero_to_twopi(py, px) angleid = int(math.floor(angle / yawreso)) @@ -85,7 +85,7 @@ def generate_ray_casting_grid_map(ox, oy, xyreso, yawreso): for (x, y) in zip(ox, oy): - d = math.sqrt(x**2 + y**2) + d = math.hypot(x, y) angle = atan_zero_to_twopi(y, x) angleid = int(math.floor(angle / yawreso)) From d433748ef8ab6fba34f7f7882253e2c1f0c31611 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:08 +0100 Subject: [PATCH 29/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/AStar/a_star.py --- PathPlanning/AStar/a_star.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py index 6b0392f5..061aafbe 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -136,7 +136,7 @@ class AStarPlanner: @staticmethod def calc_heuristic(n1, n2): w = 1.0 # weight of heuristic - d = w * math.sqrt((n1.x - n2.x) ** 2 + (n1.y - n2.y) ** 2) + d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) return d def calc_grid_position(self, index, minp): @@ -199,7 +199,7 @@ class AStarPlanner: for iy in range(self.ywidth): y = self.calc_grid_position(iy, self.miny) for iox, ioy in zip(ox, oy): - d = math.sqrt((iox - x) ** 2 + (ioy - y) ** 2) + d = math.hypot(iox - x, ioy - y) if d <= self.rr: self.obmap[ix][iy] = True break From ed9b84dfc3310e6a45e19c05c38a485560da5c15 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:08 +0100 Subject: [PATCH 30/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py --- PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 37ac75ad..8e7af39f 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -180,8 +180,8 @@ class BITStar(object): cBest = self.g_scores[self.goalId] # Computing the sampling space - cMin = math.sqrt(pow(self.start[0] - self.goal[0], 2) - + pow(self.start[1] - self.goal[1], 2)) / 1.5 + cMin = math.hypot(self.start[0] - self.goal[0], + self.start[1] - self.goal[1]) / 1.5 xCenter = np.array([[(self.start[0] + self.goal[0]) / 2.0], [(self.start[1] + self.goal[1]) / 2.0], [0]]) a1 = np.array([[(self.goal[0] - self.start[0]) / cMin], From cf538756e69713fe5d3d23a9c99a09ba5f62c35d Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:08 +0100 Subject: [PATCH 31/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/ClosedLoopRRTStar/pure_pursuit.py --- PathPlanning/ClosedLoopRRTStar/pure_pursuit.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index 32faa51c..be1d2238 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py @@ -161,7 +161,7 @@ def set_stop_point(target_speed, cx, cy, cyaw): for i in range(len(cx) - 1): dx = cx[i + 1] - cx[i] dy = cy[i + 1] - cy[i] - d.append(math.sqrt(dx ** 2.0 + dy ** 2.0)) + d.append(math.hypot(dx, dy)) iyaw = cyaw[i] move_direction = math.atan2(dy, dx) is_back = abs(move_direction - iyaw) >= math.pi / 2.0 From 4c73cada9b1fccbcf929a4520735c590f188a85c Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:08 +0100 Subject: [PATCH 32/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/CubicSpline/cubic_spline_planner.py --- PathPlanning/CubicSpline/cubic_spline_planner.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/PathPlanning/CubicSpline/cubic_spline_planner.py b/PathPlanning/CubicSpline/cubic_spline_planner.py index 071b1171..239ce167 100644 --- a/PathPlanning/CubicSpline/cubic_spline_planner.py +++ b/PathPlanning/CubicSpline/cubic_spline_planner.py @@ -140,8 +140,7 @@ class Spline2D: def __calc_s(self, x, y): dx = np.diff(x) dy = np.diff(y) - self.ds = [math.sqrt(idx ** 2 + idy ** 2) - for (idx, idy) in zip(dx, dy)] + self.ds = np.hypot(dx, dy) s = [0] s.extend(np.cumsum(self.ds)) return s From 4c6fe30fb33bff9b85238f26e95e8ae7b9a973dd Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:08 +0100 Subject: [PATCH 33/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/Dijkstra/dijkstra.py --- PathPlanning/Dijkstra/dijkstra.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index bb73ed50..5c06f53a 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -123,7 +123,7 @@ class Dijkstra: def calc_heuristic(self, n1, n2): w = 1.0 # weight of heuristic - d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2) + d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) return d def calc_position(self, index, minp): @@ -178,7 +178,7 @@ class Dijkstra: for iy in range(self.ywidth): y = self.calc_position(iy, self.miny) for iox, ioy in zip(ox, oy): - d = math.sqrt((iox - x)**2 + (ioy - y)**2) + d = math.hypot(iox - x, ioy - y) if d <= self.rr: self.obmap[ix][iy] = True break From 367c2c6dc635644b3fa37b5f921b3ebabb1e97c8 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:08 +0100 Subject: [PATCH 34/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/DubinsPath/dubins_path_planning.py --- PathPlanning/DubinsPath/dubins_path_planning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index 706b5a49..96c9c8e4 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -141,7 +141,7 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c, D_ANGLE): # normalize dx = ex dy = ey - D = math.sqrt(dx ** 2.0 + dy ** 2.0) + D = math.hypot(dx, dy) d = D * c # print(dx, dy, D, d) From 60387ee5f8b5f7898dbe73cc671f59503042a182 Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:08 +0100 Subject: [PATCH 35/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/DynamicWindowApproach/dynamic_window_approach.py --- PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 5c3d128d..891e68cb 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -279,7 +279,7 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle): plt.pause(0.0001) # check reaching goal - dist_to_goal = math.sqrt((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2) + dist_to_goal = math.hypot(x[0] - goal[0], x[1] - goal[1]) if dist_to_goal <= config.robot_radius: print("Goal!!") break From 077120201b41a8c4808a81170679aac7cd077acd Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:08 +0100 Subject: [PATCH 36/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py --- .../FrenetOptimalTrajectory/frenet_optimal_trajectory.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index 5c756a9d..f3ae2a43 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -230,7 +230,7 @@ def calc_global_paths(fplist, csp): dx = fp.x[i + 1] - fp.x[i] dy = fp.y[i + 1] - fp.y[i] fp.yaw.append(math.atan2(dy, dx)) - fp.ds.append(math.sqrt(dx**2 + dy**2)) + fp.ds.append(math.hypot(dx, dy)) fp.yaw.append(fp.yaw[-1]) fp.ds.append(fp.ds[-1]) From d20f1e336e9b2536eacea224d12c59858a9c74bd Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:08 +0100 Subject: [PATCH 37/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py --- .../ProbabilisticRoadMap/probabilistic_road_map.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py index 351232d8..8090e336 100644 --- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py +++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py @@ -97,7 +97,7 @@ def is_collision(sx, sy, gx, gy, rr, okdtree): dx = gx - sx dy = gy - sy yaw = math.atan2(gy - sy, gx - sx) - d = math.sqrt(dx**2 + dy**2) + d = math.hypot(dx, dy) if d >= MAX_EDGE_LEN: return True @@ -171,7 +171,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): road_map: ??? [m] sample_x: ??? [m] sample_y: ??? [m] - + @return: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found """ @@ -182,7 +182,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): openset[len(road_map) - 2] = nstart path_found = True - + while True: if not openset: print("Cannot find path") @@ -213,7 +213,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): n_id = road_map[c_id][i] dx = sample_x[n_id] - current.x dy = sample_y[n_id] - current.y - d = math.sqrt(dx**2 + dy**2) + d = math.hypot(dx, dy) node = Node(sample_x[n_id], sample_y[n_id], current.cost + d, c_id) @@ -226,7 +226,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): openset[n_id].pind = c_id else: openset[n_id] = node - + if path_found is False: return [], [] From c63a80b0ef148d5fb4bf75b1b0da41ba994c84ad Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 7 Dec 2019 22:53:08 +0100 Subject: [PATCH 38/38] Replaced sqrt(x**2+y**2) with hypot in PathPlanning/VoronoiRoadMap/voronoi_road_map.py --- PathPlanning/VoronoiRoadMap/voronoi_road_map.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py index 3ac02993..c73d6509 100644 --- a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py +++ b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py @@ -95,7 +95,7 @@ def is_collision(sx, sy, gx, gy, rr, okdtree): dx = gx - sx dy = gy - sy yaw = math.atan2(gy - sy, gx - sx) - d = math.sqrt(dx**2 + dy**2) + d = math.hypot(dx, dy) if d >= MAX_EDGE_LEN: return True @@ -203,7 +203,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y): n_id = road_map[c_id][i] dx = sample_x[n_id] - current.x dy = sample_y[n_id] - current.y - d = math.sqrt(dx**2 + dy**2) + d = math.hypot(dx, dy) node = Node(sample_x[n_id], sample_y[n_id], current.cost + d, c_id)