AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:41 -04:00
Code Issues Packages Projects Releases Wiki Activity

fix the dubins planning bug and code clean up

Browse Source
This commit is contained in:
Atsushi Sakai
2020-01-28 21:35:49 +09:00
parent 4e323212e5
commit 18ac00dced
4 changed files with 139 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

200
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -21,7 +21,7 @@ def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
def LSL(alpha, beta, d):
def left_straight_left(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
@@ -38,12 +38,11 @@ def LSL(alpha, beta, d):
t = mod2pi(-alpha + tmp1)
p = math.sqrt(p_squared)
q = mod2pi(beta - tmp1)
# print(np.rad2deg(t), p, np.rad2deg(q))
return t, p, q, mode
def RSR(alpha, beta, d):
def right_straight_right(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
@@ -63,7 +62,7 @@ def RSR(alpha, beta, d):
return t, p, q, mode
def LSR(alpha, beta, d):
def left_straight_right(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
@@ -82,7 +81,7 @@ def LSR(alpha, beta, d):
return t, p, q, mode
def RSL(alpha, beta, d):
def right_straight_left(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
@@ -101,7 +100,7 @@ def RSL(alpha, beta, d):
return t, p, q, mode
def RLR(alpha, beta, d):
def right_left_right(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
@@ -119,7 +118,7 @@ def RLR(alpha, beta, d):
return t, p, q, mode
def LRL(alpha, beta, d):
def left_right_left(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
@@ -137,23 +136,21 @@ def LRL(alpha, beta, d):
return t, p, q, mode
def dubins_path_planning_from_origin(ex, ey, eyaw, c, D_ANGLE):
# normalize
dx = ex
dy = ey
def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size):
dx = end_x
dy = end_y
D = math.hypot(dx, dy)
d = D * c
# print(dx, dy, D, d)
d = D * curvature
theta = mod2pi(math.atan2(dy, dx))
alpha = mod2pi(- theta)
beta = mod2pi(eyaw - theta)
# print(theta, alpha, beta, d)
beta = mod2pi(end_yaw - theta)
planners = [LSL, RSR, LSR, RSL, RLR, LRL]
planners = [left_straight_left, right_straight_right, left_straight_right, right_straight_left, right_left_right,
left_right_left]
bcost = float("inf")
bt, bp, bq, bmode = None, None, None, None
best_cost = float("inf")
bt, bp, bq, best_mode = None, None, None, None
for planner in planners:
t, p, q, mode = planner(alpha, beta, d)
@@ -161,17 +158,48 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c, D_ANGLE):
continue
cost = (abs(t) + abs(p) + abs(q))
if bcost > cost:
bt, bp, bq, bmode = t, p, q, mode
bcost = cost
if best_cost > cost:
bt, bp, bq, best_mode = t, p, q, mode
best_cost = cost
lengths = [bt, bp, bq]
# print(bmode)
px, py, pyaw = generate_course([bt, bp, bq], bmode, c, D_ANGLE)
px, py, pyaw, directions = generate_local_course(
sum(lengths), lengths, best_mode, curvature, step_size)
return px, py, pyaw, bmode, bcost
return px, py, pyaw, best_mode, best_cost
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0)):
def interpolate(ind, length, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions):
if mode == "S":
path_x[ind] = origin_x + length / max_curvature * math.cos(origin_yaw)
path_y[ind] = origin_y + length / max_curvature * math.sin(origin_yaw)
path_yaw[ind] = origin_yaw
else: # curve
ldx = math.sin(length) / max_curvature
ldy = 0.0
if mode == "L": # left turn
ldy = (1.0 - math.cos(length)) / max_curvature
elif mode == "R": # right turn
ldy = (1.0 - math.cos(length)) / -max_curvature
gdx = math.cos(-origin_yaw) * ldx + math.sin(-origin_yaw) * ldy
gdy = -math.sin(-origin_yaw) * ldx + math.cos(-origin_yaw) * ldy
path_x[ind] = origin_x + gdx
path_y[ind] = origin_y + gdy
if mode == "L": # left turn
path_yaw[ind] = origin_yaw + length
elif mode == "R": # right turn
path_yaw[ind] = origin_yaw - length
if length > 0.0:
directions[ind] = 1
else:
directions[ind] = -1
return path_x, path_y, path_yaw, directions
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, step_size=0.1):
"""
Dubins path plannner
@@ -200,7 +228,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0)
leyaw = eyaw - syaw
lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
lex, ley, leyaw, c, D_ANGLE)
lex, ley, leyaw, c, step_size)
px = [math.cos(-syaw) * x + math.sin(-syaw)
* y + sx for x, y in zip(lpx, lpy)]
@@ -211,44 +239,60 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0)
return px, py, pyaw, mode, clen
def generate_course(length, mode, c, D_ANGLE):
px = [0.0]
py = [0.0]
pyaw = [0.0]
def generate_local_course(total_length, lengths, mode, max_curvature, step_size):
n_point = math.trunc(total_length / step_size) + len(lengths) + 4
for m, l in zip(mode, length):
pd = 0.0
if m == "S":
d = 1.0 * c
else: # turning couse
d = D_ANGLE
path_x = [0.0 for _ in range(n_point)]
path_y = [0.0 for _ in range(n_point)]
path_yaw = [0.0 for _ in range(n_point)]
directions = [0.0 for _ in range(n_point)]
ind = 1
while pd < abs(l - d):
# print(pd, l)
px.append(px[-1] + d / c * math.cos(pyaw[-1]))
py.append(py[-1] + d / c * math.sin(pyaw[-1]))
if lengths[0] > 0.0:
directions[0] = 1
else:
directions[0] = -1
if m == "L": # left turn
pyaw.append(pyaw[-1] + d)
elif m == "S": # Straight
pyaw.append(pyaw[-1])
elif m == "R": # right turn
pyaw.append(pyaw[-1] - d)
ll = 0.0
for (m, l, i) in zip(mode, lengths, range(len(mode))):
if l > 0.0:
d = step_size
else:
d = -step_size
# set origin state
origin_x, origin_y, origin_yaw = path_x[ind], path_y[ind], path_yaw[ind]
ind -= 1
if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
pd = - d - ll
else:
pd = d - ll
while abs(pd) <= abs(l):
ind += 1
path_x, path_y, path_yaw, directions = interpolate(
ind, pd, m, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions)
pd += d
d = l - pd
px.append(px[-1] + d / c * math.cos(pyaw[-1]))
py.append(py[-1] + d / c * math.sin(pyaw[-1]))
ll = l - pd - d # calc remain length
if m == "L": # left turn
pyaw.append(pyaw[-1] + d)
elif m == "S": # Straight
pyaw.append(pyaw[-1])
elif m == "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
ind += 1
path_x, path_y, path_yaw, directions = interpolate(
ind, l, m, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions)
return px, py, pyaw
if len(path_x) <= 1:
return [], [], [], []
# remove unused data
while len(path_x) >= 1 and path_x[-1] == 0.0:
path_x.pop()
path_y.pop()
path_yaw.pop()
directions.pop()
return path_x, path_y, path_yaw, directions
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
@@ -288,53 +332,11 @@ def main():
plot_arrow(start_x, start_y, start_yaw)
plot_arrow(end_x, end_y, end_yaw)
# for (ix, iy, iyaw) in zip(px, py, pyaw):
# plot_arrow(ix, iy, iyaw, fc="b")
plt.legend()
plt.grid(True)
plt.axis("equal")
plt.show()
def test():
NTEST = 5
for i in range(NTEST):
start_x = (np.random.rand() - 0.5) * 10.0 # [m]
start_y = (np.random.rand() - 0.5) * 10.0 # [m]
start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad]
end_x = (np.random.rand() - 0.5) * 10.0 # [m]
end_y = (np.random.rand() - 0.5) * 10.0 # [m]
end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad]
curvature = 1.0 / (np.random.rand() * 5.0)
px, py, pyaw, mode, clen = dubins_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(px, py, label="final course " + str(mode))
# plotting
plot_arrow(start_x, start_y, start_yaw)
plot_arrow(end_x, end_y, end_yaw)
plt.legend()
plt.grid(True)
plt.axis("equal")
plt.xlim(-10, 10)
plt.ylim(-10, 10)
plt.pause(1.0)
print("Test done")
if __name__ == '__main__':
test()
main()

3
PathPlanning/RRTDubins/rrt_dubins.py
View File

@@ -137,6 +137,9 @@ class RRTDubins(RRT):
from_node.x, from_node.y, from_node.yaw,
to_node.x, to_node.y, to_node.yaw, self.curvature)
if len(px) <= 1: # cannot find a dubins path
return None
new_node = copy.deepcopy(from_node)
new_node.x = px[-1]
new_node.y = py[-1]

5
PathPlanning/RRTStarDubins/rrt_star_dubins.py
View File

@@ -114,7 +114,7 @@ class RRTStarDubins(RRTStar):
plt.clf()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
lambda event: [exit(0) if event.key == 'escape' else None])
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.node_list:
@@ -143,6 +143,9 @@ class RRTStarDubins(RRTStar):
from_node.x, from_node.y, from_node.yaw,
to_node.x, to_node.y, to_node.yaw, self.curvature)
if len(px) <= 1: # cannot find a dubins path
return None
new_node = copy.deepcopy(from_node)
new_node.x = px[-1]
new_node.y = py[-1]

36
tests/test_dubins_path_planning.py
View File

@@ -7,6 +7,18 @@ from PathPlanning.DubinsPath import dubins_path_planning
class Test(TestCase):
@staticmethod
def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw):
assert (abs(px[0] - start_x) <= 0.01)
assert (abs(py[0] - start_y) <= 0.01)
assert (abs(pyaw[0] - start_yaw) <= 0.01)
print("x", px[-1], end_x)
assert (abs(px[-1] - end_x) <= 0.01)
print("y", py[-1], end_y)
assert (abs(py[-1] - end_y) <= 0.01)
print("yaw", pyaw[-1], end_yaw)
assert (abs(pyaw[-1] - end_yaw) <= 0.01)
def test1(self):
start_x = 1.0 # [m]
start_y = 1.0 # [m]
@@ -21,14 +33,28 @@ class Test(TestCase):
px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
assert (abs(px[-1] - end_x) <= 0.5)
assert (abs(py[-1] - end_y) <= 0.5)
assert(abs(pyaw[-1] - end_yaw) <= 0.1)
self.check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw)
def test2(self):
dubins_path_planning.show_animation = False
dubins_path_planning.main()
def test3(self):
dubins_path_planning.show_animation = False
dubins_path_planning.test()
N_TEST = 10
for i in range(N_TEST):
start_x = (np.random.rand() - 0.5) * 10.0 # [m]
start_y = (np.random.rand() - 0.5) * 10.0 # [m]
start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad]
end_x = (np.random.rand() - 0.5) * 10.0 # [m]
end_y = (np.random.rand() - 0.5) * 10.0 # [m]
end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad]
curvature = 1.0 / (np.random.rand() * 5.0)
px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
self.check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw)