From 489ee5c0e367a64f19b6cf3875ea6282e78bf889 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Thu, 26 Jan 2023 21:56:42 +0900 Subject: [PATCH] fix github scanning alerts (#784) --- .../two_joint_arm_to_point_control.py | 2 +- Bipedal/bipedal_planner/bipedal_planner.py | 1 - PathPlanning/HybridAStar/dynamic_programming_heuristic.py | 2 +- PathPlanning/InformedRRTStar/informed_rrt_star.py | 5 ++--- PathPlanning/LQRPlanner/lqr_planner.py | 2 +- PathPlanning/LQRRRTStar/lqr_rrt_star.py | 2 +- PathPlanning/RRT/rrt_with_pathsmoothing.py | 6 +++--- PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py | 6 +++--- 8 files changed, 12 insertions(+), 14 deletions(-) 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 9f835c8b321..012a7f72f15 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 @@ -45,7 +45,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0): if x is not None and y is not None: x_prev = x y_prev = y - if np.sqrt(x**2 + y**2) > (l1 + l2): + if np.hypot(x, y) > (l1 + l2): theta2_goal = 0 else: theta2_goal = np.arccos( diff --git a/Bipedal/bipedal_planner/bipedal_planner.py b/Bipedal/bipedal_planner/bipedal_planner.py index 8ca5ffcf784..c34357df678 100644 --- a/Bipedal/bipedal_planner/bipedal_planner.py +++ b/Bipedal/bipedal_planner/bipedal_planner.py @@ -47,7 +47,6 @@ def walk(self, t_sup=0.8, z_c=0.8, a=10, b=1, plot=False): return # set up plotter - com_trajectory_for_plot, ax = None, None if plot: fig = plt.figure() ax = Axes3D(fig) diff --git a/PathPlanning/HybridAStar/dynamic_programming_heuristic.py b/PathPlanning/HybridAStar/dynamic_programming_heuristic.py index 8a78b15d5ff..09bcd556a69 100644 --- a/PathPlanning/HybridAStar/dynamic_programming_heuristic.py +++ b/PathPlanning/HybridAStar/dynamic_programming_heuristic.py @@ -150,7 +150,7 @@ def calc_obstacle_map(ox, oy, resolution, vr): y = iy + min_y # print(x, y) 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 <= vr / resolution: obstacle_map[ix][iy] = True break diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 25c50a0cd22..289e33fe4b1 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -192,7 +192,7 @@ def get_path_len(path): @staticmethod def line_cost(node1, node2): - return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2) + return math.hypot(node1.x - node2.x, node1.y - node2.y) @staticmethod def get_nearest_list_index(nodes, rnd): @@ -222,8 +222,7 @@ def rewire(self, newNode, nearInds): for i in nearInds: nearNode = self.node_list[i] - d = math.sqrt((nearNode.x - newNode.x) ** 2 - + (nearNode.y - newNode.y) ** 2) + d = math.hypot(nearNode.x - newNode.x, nearNode.y - newNode.y) s_cost = newNode.cost + d diff --git a/PathPlanning/LQRPlanner/lqr_planner.py b/PathPlanning/LQRPlanner/lqr_planner.py index ba01526a2c4..0f58f93ea39 100644 --- a/PathPlanning/LQRPlanner/lqr_planner.py +++ b/PathPlanning/LQRPlanner/lqr_planner.py @@ -47,7 +47,7 @@ def lqr_planning(self, sx, sy, gx, gy, show_animation=True): rx.append(x[0, 0] + gx) ry.append(x[1, 0] + gy) - d = math.sqrt((gx - rx[-1]) ** 2 + (gy - ry[-1]) ** 2) + d = math.hypot(gx - rx[-1], gy - ry[-1]) if d <= self.GOAL_DIST: found_path = True break diff --git a/PathPlanning/LQRRRTStar/lqr_rrt_star.py b/PathPlanning/LQRRRTStar/lqr_rrt_star.py index e3f18a722ad..0ed08123eab 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -179,7 +179,7 @@ def sample_path(self, wx, wy, step): dx = np.diff(px) dy = np.diff(py) - clen = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)] + clen = [math.hypot(idx, idy) for (idx, idy) in zip(dx, dy)] return px, py, clen diff --git a/PathPlanning/RRT/rrt_with_pathsmoothing.py b/PathPlanning/RRT/rrt_with_pathsmoothing.py index 616c73a6979..2ed6a366d15 100644 --- a/PathPlanning/RRT/rrt_with_pathsmoothing.py +++ b/PathPlanning/RRT/rrt_with_pathsmoothing.py @@ -23,7 +23,7 @@ def get_path_length(path): for i in range(len(path) - 1): dx = path[i + 1][0] - path[i][0] dy = path[i + 1][1] - path[i][1] - d = math.sqrt(dx * dx + dy * dy) + d = math.hypot(dx, dy) le += d return le @@ -36,7 +36,7 @@ def get_target_point(path, targetL): for i in range(len(path) - 1): dx = path[i + 1][0] - path[i][0] dy = path[i + 1][1] - path[i][1] - d = math.sqrt(dx * dx + dy * dy) + d = math.hypot(dx, dy) le += d if le >= targetL: ti = i - 1 @@ -67,7 +67,7 @@ def line_collision_check(first, second, obstacleList): return False for (ox, oy, size) in obstacleList: - d = abs(a * ox + b * oy + c) / (math.sqrt(a * a + b * b)) + d = abs(a * ox + b * oy + c) / (math.hypot(a, b)) if d <= size: return False diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 5bc04c8ee39..1577d048d35 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -57,13 +57,13 @@ def straight_left_straight(x, y, phi): xd = - y / math.tan(phi) + x t = xd - math.tan(phi / 2.0) u = phi - v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0) + v = math.hypot(x - xd, y) - math.tan(phi / 2.0) return True, t, u, v elif y < 0.0 < phi < math.pi * 0.99: xd = - y / math.tan(phi) + x t = xd - math.tan(phi / 2.0) u = phi - v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0) + v = -math.hypot(x - xd, y) - math.tan(phi / 2.0) return True, t, u, v return False, 0.0, 0.0, 0.0 @@ -103,7 +103,7 @@ def straight_curve_straight(x, y, phi, paths, step_size): def polar(x, y): - r = math.sqrt(x ** 2 + y ** 2) + r = math.hypot(x, y) theta = math.atan2(y, x) return r, theta