Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
1 change: 0 additions & 1 deletion Bipedal/bipedal_planner/bipedal_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion PathPlanning/HybridAStar/dynamic_programming_heuristic.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
5 changes: 2 additions & 3 deletions PathPlanning/InformedRRTStar/informed_rrt_star.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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

Expand Down
2 changes: 1 addition & 1 deletion PathPlanning/LQRPlanner/lqr_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion PathPlanning/LQRRRTStar/lqr_rrt_star.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
6 changes: 3 additions & 3 deletions PathPlanning/RRT/rrt_with_pathsmoothing.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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

Expand Down
6 changes: 3 additions & 3 deletions PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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

Expand Down