github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 02:28:03 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix rrt_star_dubins

Browse Source
This commit is contained in:
Atsushi Sakai
2019-07-27 15:08:25 +09:00
parent 2bf23adb27
commit d9508f6e72
3 changed files with 28 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -6,8 +6,9 @@ author Atsushi Sakai(@Atsushi_twi)
"""
import math
import numpy as np
import matplotlib.pyplot as plt
import numpy as np
show_animation = True
@@ -136,8 +137,8 @@ def LRL(alpha, beta, d):
return t, p, q, mode
def dubins_path_planning_from_origin(ex, ey, eyaw, c):
# nomalize
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)
@@ -165,12 +166,12 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c):
bcost = cost
# print(bmode)
px, py, pyaw = generate_course([bt, bp, bq], bmode, c)
px, py, pyaw = generate_course([bt, bp, bq], bmode, c, D_ANGLE)
return px, py, pyaw, bmode, bcost
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0)):
"""
Dubins path plannner
@@ -199,7 +200,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
leyaw = eyaw - syaw
lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
lex, ley, leyaw, c)
lex, ley, leyaw, c, D_ANGLE)
px = [math.cos(-syaw) * x + math.sin(-syaw)
* y + sx for x, y in zip(lpx, lpy)]
@@ -210,7 +211,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
return px, py, pyaw, mode, clen
def generate_course(length, mode, c):
def generate_course(length, mode, c, D_ANGLE):
px = [0.0]
py = [0.0]
@@ -221,7 +222,7 @@ def generate_course(length, mode, c):
if m == "S":
d = 1.0 * c
else: # turning couse
d = np.deg2rad(3.0)
d = D_ANGLE
while pd < abs(l - d):
# print(pd, l)

13
PathPlanning/RRT/rrt.py
View File

@@ -19,7 +19,7 @@ class RRT:
Class for RRT planning
"""
class Node():
class Node:
"""
RRT Node
"""
@@ -104,6 +104,13 @@ class RRT:
new_node.path_x.append(new_node.x)
new_node.path_y.append(new_node.y)
d, _ = self.calc_distance_and_angle(new_node, to_node)
if d <= self.path_resolution:
new_node.x = to_node.x
new_node.y = to_node.y
new_node.path_x[-1] = to_node.x
new_node.path_y[-1] = to_node.y
new_node.parent = from_node
return new_node
@@ -137,9 +144,7 @@ class RRT:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.node_list:
if node.parent:
plt.plot([node.x, node.parent.x],
[node.y, node.parent.y],
"-g")
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacle_list:
plt.plot(ox, oy, "ok", ms=30 * size)

38
PathPlanning/RRTStar/rrt_star.py
View File

@@ -53,7 +53,7 @@ class RRTStar(RRT):
"""
self.connect_circle_dist = connect_circle_dist
def planning(self, animation=True, search_until_maxiter=True):
def planning(self, animation=True, search_until_max_iter=True):
"""
rrt star path planning
@@ -63,7 +63,7 @@ class RRTStar(RRT):
self.node_list = [self.start]
for i in range(self.max_iter):
print(i, len(self.node_list))
print("Iter:", i, ", number of nodes:", len(self.node_list))
rnd = self.get_random_node()
nearest_ind = self.get_nearest_list_index(self.node_list, rnd)
new_node = self.steer(self.node_list[nearest_ind], rnd, self.expand_dis)
@@ -78,10 +78,10 @@ class RRTStar(RRT):
if animation and i % 5 == 0:
self.draw_graph(rnd)
if (not search_until_maxiter) and new_node: # check reaching the goal
d, _ = self.calc_distance_and_angle(new_node, self.end)
if d <= self.expand_dis:
return self.generate_final_course(len(self.node_list) - 1)
if (not search_until_max_iter) and new_node: # check reaching the goal
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
print("reached max iteration")
@@ -91,25 +91,6 @@ class RRTStar(RRT):
return None
def connect_nodes(self, from_node, to_node):
new_node = self.Node(from_node.x, from_node.y)
d, theta = self.calc_distance_and_angle(new_node, to_node)
new_node.path_x = [new_node.x]
new_node.path_y = [new_node.y]
n_expand = math.floor(d / self.path_resolution)
for _ in range(n_expand):
new_node.x += self.path_resolution * math.cos(theta)
new_node.y += self.path_resolution * math.sin(theta)
new_node.path_x.append(new_node.x)
new_node.path_y.append(new_node.y)
new_node.parent = from_node
return new_node
def choose_parent(self, new_node, near_inds):
if not near_inds:
return None
@@ -130,8 +111,8 @@ class RRTStar(RRT):
return None
min_ind = near_inds[costs.index(min_cost)]
new_node = self.steer(self.node_list[min_ind], new_node)
new_node.parent = self.node_list[min_ind]
new_node = self.steer(new_node.parent, new_node)
new_node.cost = min_cost
return new_node
@@ -162,14 +143,14 @@ class RRTStar(RRT):
for i in near_inds:
near_node = self.node_list[i]
edge_node = self.steer(new_node, near_node)
edge_node.cost = self.calc_new_cost(near_node, new_node)
edge_node.cost = self.calc_new_cost(new_node, near_node)
no_collision = self.check_collision(edge_node, self.obstacle_list)
improved_cost = near_node.cost > edge_node.cost
if no_collision and improved_cost:
near_node = edge_node
near_node.parent = new_node
near_node.cost = edge_node.cost
self.propagate_cost_to_leaves(new_node)
def calc_new_cost(self, from_node, to_node):
@@ -177,6 +158,7 @@ class RRTStar(RRT):
return from_node.cost + d
def propagate_cost_to_leaves(self, parent_node):
for node in self.node_list:
if node.parent == parent_node:
node.cost = self.calc_new_cost(parent_node, node)