diff --git a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py index 9f1e16fc1d..dde76f19bd 100644 --- a/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py +++ b/AerialNavigation/rocket_powered_landing/rocket_powered_landing.py @@ -195,8 +195,8 @@ def B_func(self, x, u): [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): 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 b232ab33f3..3972474b8d 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): 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 e674ae8ac2..0e91be0ff6 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 diff --git a/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py b/Localization/ensemble_kalman_filter/ensemble_kalman_filter.py index 293509157b..3e2af749e7 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 diff --git a/Localization/histogram_filter/histogram_filter.py b/Localization/histogram_filter/histogram_filter.py index bdfbb13fa7..db715a6d1e 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)) @@ -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 diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 0ebb5fc3d1..d14b7c0697 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])) diff --git a/Mapping/gaussian_grid_map/gaussian_grid_map.py b/Mapping/gaussian_grid_map/gaussian_grid_map.py index b520b63944..9c1aa27320 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 diff --git a/Mapping/kmeans_clustering/kmeans_clustering.py b/Mapping/kmeans_clustering/kmeans_clustering.py index bdaa45455e..c2518b80c0 100644 --- a/Mapping/kmeans_clustering/kmeans_clustering.py +++ b/Mapping/kmeans_clustering/kmeans_clustering.py @@ -68,7 +68,7 @@ def update_clusters(self): 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 diff --git a/Mapping/raycasting_grid_map/raycasting_grid_map.py b/Mapping/raycasting_grid_map/raycasting_grid_map.py index 997687423e..a0d9060916 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)) diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py index e40fc2e4c7..2018f02514 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/Mapping/rectangle_fitting/rectangle_fitting.py @@ -162,7 +162,7 @@ def _adoptive_range_segmentation(self, ox, oy): 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) diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py index 6b0392f515..061aafbeff 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -136,7 +136,7 @@ def calc_final_path(self, ngoal, closedset): @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 @@ def calc_obstacle_map(self, ox, oy): 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 diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 37ac75ada9..8e7af39f30 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -180,8 +180,8 @@ def setup_planning(self): 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], diff --git a/PathPlanning/BezierPath/bezier_path.py b/PathPlanning/BezierPath/bezier_path.py index 516a681e3b..46faf1f310 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)], diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index a07dc48af0..aea0080112 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -6,7 +6,6 @@ """ -import math import os import sys @@ -119,9 +118,8 @@ def check_tracking_path_is_feasible(self, path): 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") diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index 0490727933..be1d223815 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 @@ -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 diff --git a/PathPlanning/CubicSpline/cubic_spline_planner.py b/PathPlanning/CubicSpline/cubic_spline_planner.py index 071b117185..239ce16738 100644 --- a/PathPlanning/CubicSpline/cubic_spline_planner.py +++ b/PathPlanning/CubicSpline/cubic_spline_planner.py @@ -140,8 +140,7 @@ def __init__(self, x, y): 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 diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index bb73ed501a..5c06f53aed 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -123,7 +123,7 @@ def calc_final_path(self, ngoal, closedset): 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 @@ def calc_obstacle_map(self, ox, oy): 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 diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index 706b5a49af..96c9c8e475 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) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index c8a4434958..891e68cbe8 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] @@ -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 diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index 5c756a9dfe..f3ae2a439b 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]) diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py index f22b058fa8..9d7e1616f3 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 diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 175749183d..065868f2d2 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -106,7 +106,7 @@ def choose_parent(self, newNode, nearInds): 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 @@ def rewire(self, newNode, nearInds): 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 @@ def distance_squared_point_to_segment(v, w, p): 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)) diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py index 351232d849..8090e3364f 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 [], [] diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index 3bcc622100..5b6f3252ae 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -126,7 +126,7 @@ def generate_final_course(self, goal_ind): 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 @@ def check_collision(node, obstacleList): 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 diff --git a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py index 3ac02993b7..c73d650951 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) 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 6dec12bd30..c7c8e0dd97 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 diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py index f977cdc2a1..b69577d3aa 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 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 0f5104e954..a16768e36b 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) diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index 0814ef62ef..e161b4616d 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 diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 38a84affce..370cac96cc 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 diff --git a/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py b/PathTracking/rear_wheel_feedback/rear_wheel_feedback.py index 285d9404e3..bf4f31b39a 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 diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index 22ccd8d659..66234b57ec 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 diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py index c16407e89f..298cc0363a 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 diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py index 73624dbf1f..b9df694830 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 diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py index b40ad6494e..41b9d52591 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: