Merge c63a80b into 190a28e

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):

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):

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

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

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

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]))
 

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
 

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

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))
 

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)

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

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],

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)],

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")

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

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

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

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)
 

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

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])

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

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,15 +224,15 @@ 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
         if (np.array_equal(v, w)):
             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))

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 [], []
 

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
 

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)