saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-29 16:40:46 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
b3f190ec3625ef6d2f4fc332438b11e981195c79
PathPlanning/CurvesGenerator/reeds_shepp.py
T
zhm-real 8a2b84ccf1 update
2020-07-30 00:53:49 -07:00

721 lines
18 KiB
Python
Raw Blame History

import math
import numpy as np
import matplotlib.pyplot as plt
import draw
# parameters initiation
STEP_SIZE = 0.2
MAX_LENGTH = 1000.0
PI = math.pi
# class for PATH element
class PATH:
def __init__(self, lengths, ctypes, L, x, y, yaw, directions):
self.lengths = lengths # lengths of each part of path (+: forward, -: backward) [float]
self.ctypes = ctypes # type of each part of the path [string]
self.L = L # total path length [float]
self.x = x # final x positions [m]
self.y = y # final y positions [m]
self.yaw = yaw # final yaw angles [rad]
self.directions = directions # forward: 1, backward:-1
def calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):
paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=step_size)
minL = paths[0].L
mini = 0
for i in range(len(paths)):
if paths[i].L <= minL:
minL, mini = paths[i].L, i
return paths[mini]
def calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE):
q0 = [sx, sy, syaw]
q1 = [gx, gy, gyaw]
paths = generate_path(q0, q1, maxc)
for path in paths:
x, y, yaw, directions = \
generate_local_course(path.L, path.lengths,
path.ctypes, maxc, step_size * maxc)
# convert global coordinate
path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for (ix, iy) in zip(x, y)]
path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(x, y)]
path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw]
path.directions = directions
path.lengths = [l / maxc for l in path.lengths]
path.L = path.L / maxc
return paths
def set_path(paths, lengths, ctypes):
path = PATH([], [], 0.0, [], [], [], [])
path.ctypes = ctypes
path.lengths = lengths
# check same path exist
for path_e in paths:
if path_e.ctypes == path.ctypes:
if sum([x - y for x, y in zip(path_e.lengths, path.lengths)]) <= 0.01:
return paths # not insert path
path.L = sum([abs(i) for i in lengths])
if path.L >= MAX_LENGTH:
return paths
assert path.L >= 0.01
paths.append(path)
return paths
def LSL(x, y, phi):
u, t = R(x - math.sin(phi), y - 1.0 + math.cos(phi))
if t >= 0.0:
v = M(phi - t)
if v >= 0.0:
return True, t, u, v
return False, 0.0, 0.0, 0.0
def LSR(x, y, phi):
u1, t1 = R(x + math.sin(phi), y - 1.0 - math.cos(phi))
u1 = u1 ** 2
if u1 >= 4.0:
u = math.sqrt(u1 - 4.0)
theta = math.atan2(2.0, u)
t = M(t1 + theta)
v = M(t - phi)
if t >= 0.0 and v >= 0.0:
return True, t, u, v
return False, 0.0, 0.0, 0.0
def LRL(x, y, phi):
u1, t1 = R(x - math.sin(phi), y - 1.0 + math.cos(phi))
if u1 <= 4.0:
u = -2.0 * math.asin(0.25 * u1)
t = M(t1 + 0.5 * u + PI)
v = M(phi - t + u)
if t >= 0.0 and u <= 0.0:
return True, t, u, v
return False, 0.0, 0.0, 0.0
def SCS(x, y, phi, paths):
flag, t, u, v = SLS(x, y, phi)
if flag:
paths = set_path(paths, [t, u, v], ["S", "L", "S"])
flag, t, u, v = SLS(x, -y, -phi)
if flag:
paths = set_path(paths, [t, u, v], ["S", "R", "S"])
return paths
def SLS(x, y, phi):
phi = M(phi)
if y > 0.0 and 0.0 < phi < 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)
return True, t, u, v
elif y < 0.0 and 0.0 < phi < 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)
return True, t, u, v
return False, 0.0, 0.0, 0.0
def CSC(x, y, phi, paths):
flag, t, u, v = LSL(x, y, phi)
if flag:
paths = set_path(paths, [t, u, v], ["L", "S", "L"])
flag, t, u, v = LSL(-x, y, -phi)
if flag:
paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"])
flag, t, u, v = LSL(x, -y, -phi)
if flag:
paths = set_path(paths, [t, u, v], ["R", "S", "R"])
flag, t, u, v = LSL(-x, -y, phi)
if flag:
paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"])
flag, t, u, v = LSR(x, y, phi)
if flag:
paths = set_path(paths, [t, u, v], ["L", "S", "R"])
flag, t, u, v = LSR(-x, y, -phi)
if flag:
paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"])
flag, t, u, v = LSR(x, -y, -phi)
if flag:
paths = set_path(paths, [t, u, v], ["R", "S", "L"])
flag, t, u, v = LSR(-x, -y, phi)
if flag:
paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"])
return paths
def CCC(x, y, phi, paths):
flag, t, u, v = LRL(x, y, phi)
if flag:
paths = set_path(paths, [t, u, v], ["L", "R", "L"])
flag, t, u, v = LRL(-x, y, -phi)
if flag:
paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"])
flag, t, u, v = LRL(x, -y, -phi)
if flag:
paths = set_path(paths, [t, u, v], ["R", "L", "R"])
flag, t, u, v = LRL(-x, -y, phi)
if flag:
paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"])
# backwards
xb = x * math.cos(phi) + y * math.sin(phi)
yb = x * math.sin(phi) - y * math.cos(phi)
flag, t, u, v = LRL(xb, yb, phi)
if flag:
paths = set_path(paths, [v, u, t], ["L", "R", "L"])
flag, t, u, v = LRL(-xb, yb, -phi)
if flag:
paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"])
flag, t, u, v = LRL(xb, -yb, -phi)
if flag:
paths = set_path(paths, [v, u, t], ["R", "L", "R"])
flag, t, u, v = LRL(-xb, -yb, phi)
if flag:
paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"])
return paths
def calc_tauOmega(u, v, xi, eta, phi):
delta = M(u - v)
A = math.sin(u) - math.sin(delta)
B = math.cos(u) - math.cos(delta) - 1.0
t1 = math.atan2(eta * A - xi * B, xi * A + eta * B)
t2 = 2.0 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0
if t2 < 0:
tau = M(t1 + PI)
else:
tau = M(t1)
omega = M(tau - u + v - phi)
return tau, omega
def LRLRn(x, y, phi):
xi = x + math.sin(phi)
eta = y - 1.0 - math.cos(phi)
rho = 0.25 * (2.0 + math.sqrt(xi * xi + eta * eta))
if rho <= 1.0:
u = math.acos(rho)
t, v = calc_tauOmega(u, -u, xi, eta, phi)
if t >= 0.0 and v <= 0.0:
return True, t, u, v
return False, 0.0, 0.0, 0.0
def LRLRp(x, y, phi):
xi = x + math.sin(phi)
eta = y - 1.0 - math.cos(phi)
rho = (20.0 - xi * xi - eta * eta) / 16.0
if 0.0 <= rho <= 1.0:
u = -math.acos(rho)
if u >= -0.5 * PI:
t, v = calc_tauOmega(u, u, xi, eta, phi)
if t >= 0.0 and v >= 0.0:
return True, t, u, v
return False, 0.0, 0.0, 0.0
def CCCC(x, y, phi, paths):
flag, t, u, v = LRLRn(x, y, phi)
if flag:
paths = set_path(paths, [t, u, -u, v], ["L", "R", "L", "R"])
flag, t, u, v = LRLRn(-x, y, -phi)
if flag:
paths = set_path(paths, [-t, -u, u, -v], ["L", "R", "L", "R"])
flag, t, u, v = LRLRn(x, -y, -phi)
if flag:
paths = set_path(paths, [t, u, -u, v], ["R", "L", "R", "L"])
flag, t, u, v = LRLRn(-x, -y, phi)
if flag:
paths = set_path(paths, [-t, -u, u, -v], ["R", "L", "R", "L"])
flag, t, u, v = LRLRp(x, y, phi)
if flag:
paths = set_path(paths, [t, u, u, v], ["L", "R", "L", "R"])
flag, t, u, v = LRLRp(-x, y, -phi)
if flag:
paths = set_path(paths, [-t, -u, -u, -v], ["L", "R", "L", "R"])
flag, t, u, v = LRLRp(x, -y, -phi)
if flag:
paths = set_path(paths, [t, u, u, v], ["R", "L", "R", "L"])
flag, t, u, v = LRLRp(-x, -y, phi)
if flag:
paths = set_path(paths, [-t, -u, -u, -v], ["R", "L", "R", "L"])
return paths
def LRSR(x, y, phi):
xi = x + math.sin(phi)
eta = y - 1.0 - math.cos(phi)
rho, theta = R(-eta, xi)
if rho >= 2.0:
t = theta
u = 2.0 - rho
v = M(t + 0.5 * PI - phi)
if t >= 0.0 and u <= 0.0 and v <= 0.0:
return True, t, u, v
return False, 0.0, 0.0, 0.0
def LRSL(x, y, phi):
xi = x - math.sin(phi)
eta = y - 1.0 + math.cos(phi)
rho, theta = R(xi, eta)
if rho >= 2.0:
r = math.sqrt(rho * rho - 4.0)
u = 2.0 - r
t = M(theta + math.atan2(r, -2.0))
v = M(phi - 0.5 * PI - t)
if t >= 0.0 and u <= 0.0 and v <= 0.0:
return True, t, u, v
return False, 0.0, 0.0, 0.0
def CCSC(x, y, phi, paths):
flag, t, u, v = LRSL(x, y, phi)
if flag:
paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "L"])
flag, t, u, v = LRSL(-x, y, -phi)
if flag:
paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "L"])
flag, t, u, v = LRSL(x, -y, -phi)
if flag:
paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "R"])
flag, t, u, v = LRSL(-x, -y, phi)
if flag:
paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "R"])
flag, t, u, v = LRSR(x, y, phi)
if flag:
paths = set_path(paths, [t, -0.5 * PI, u, v], ["L", "R", "S", "R"])
flag, t, u, v = LRSR(-x, y, -phi)
if flag:
paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["L", "R", "S", "R"])
flag, t, u, v = LRSR(x, -y, -phi)
if flag:
paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "L", "S", "L"])
flag, t, u, v = LRSR(-x, -y, phi)
if flag:
paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "L", "S", "L"])
# backwards
xb = x * math.cos(phi) + y * math.sin(phi)
yb = x * math.sin(phi) - y * math.cos(phi)
flag, t, u, v = LRSL(xb, yb, phi)
if flag:
paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "R", "L"])
flag, t, u, v = LRSL(-xb, yb, -phi)
if flag:
paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "R", "L"])
flag, t, u, v = LRSL(xb, -yb, -phi)
if flag:
paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "L", "R"])
flag, t, u, v = LRSL(-xb, -yb, phi)
if flag:
paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "L", "R"])
flag, t, u, v = LRSR(xb, yb, phi)
if flag:
paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "R", "L"])
flag, t, u, v = LRSR(-xb, yb, -phi)
if flag:
paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "R", "L"])
flag, t, u, v = LRSR(xb, -yb, -phi)
if flag:
paths = set_path(paths, [v, u, -0.5 * PI, t], ["L", "S", "L", "R"])
flag, t, u, v = LRSR(-xb, -yb, phi)
if flag:
paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["L", "S", "L", "R"])
return paths
def LRSLR(x, y, phi):
# formula 8.11 *** TYPO IN PAPER ***
xi = x + math.sin(phi)
eta = y - 1.0 - math.cos(phi)
rho, theta = R(xi, eta)
if rho >= 2.0:
u = 4.0 - math.sqrt(rho * rho - 4.0)
if u <= 0.0:
t = M(math.atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta))
v = M(t - phi)
if t >= 0.0 and v >= 0.0:
return True, t, u, v
return False, 0.0, 0.0, 0.0
def CCSCC(x, y, phi, paths):
flag, t, u, v = LRSLR(x, y, phi)
if flag:
paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["L", "R", "S", "L", "R"])
flag, t, u, v = LRSLR(-x, y, -phi)
if flag:
paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["L", "R", "S", "L", "R"])
flag, t, u, v = LRSLR(x, -y, -phi)
if flag:
paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["R", "L", "S", "R", "L"])
flag, t, u, v = LRSLR(-x, -y, phi)
if flag:
paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["R", "L", "S", "R", "L"])
return paths
def generate_local_course(L, lengths, mode, maxc, step_size):
point_num = int(L / step_size) + len(lengths) + 3
px = [0.0 for _ in range(point_num)]
py = [0.0 for _ in range(point_num)]
pyaw = [0.0 for _ in range(point_num)]
directions = [0 for _ in range(point_num)]
ind = 1
if lengths[0] > 0.0:
directions[0] = 1
else:
directions[0] = -1
if lengths[0] > 0.0:
d = step_size
else:
d = -step_size
pd = 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
ox, oy, oyaw = px[ind], py[ind], pyaw[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
px, py, pyaw, directions = \
interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
pd += d
ll = l - pd - d # calc remain length
ind += 1
px, py, pyaw, directions = \
interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions)
# remove unused data
while px[-1] == 0.0:
px.pop()
py.pop()
pyaw.pop()
directions.pop()
return px, py, pyaw, directions
def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions):
if m == "S":
px[ind] = ox + l / maxc * math.cos(oyaw)
py[ind] = oy + l / maxc * math.sin(oyaw)
pyaw[ind] = oyaw
else:
ldx = math.sin(l) / maxc
if m == "L":
ldy = (1.0 - math.cos(l)) / maxc
elif m == "R":
ldy = (1.0 - math.cos(l)) / (-maxc)
gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy
gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy
px[ind] = ox + gdx
py[ind] = oy + gdy
if m == "L":
pyaw[ind] = oyaw + l
elif m == "R":
pyaw[ind] = oyaw - l
if l > 0.0:
directions[ind] = 1
else:
directions[ind] = -1
return px, py, pyaw, directions
def generate_path(q0, q1, maxc):
dx = q1[0] - q0[0]
dy = q1[1] - q0[1]
dth = q1[2] - q0[2]
c = math.cos(q0[2])
s = math.sin(q0[2])
x = (c * dx + s * dy) * maxc
y = (-s * dx + c * dy) * maxc
paths = []
paths = SCS(x, y, dth, paths)
paths = CSC(x, y, dth, paths)
paths = CCC(x, y, dth, paths)
paths = CCCC(x, y, dth, paths)
paths = CCSC(x, y, dth, paths)
paths = CCSCC(x, y, dth, paths)
return paths
# utils
def pi_2_pi(theta):
while theta > PI:
theta -= 2.0 * PI
while theta < -PI:
theta += 2.0 * PI
return theta
def R(x, y):
"""
Return the polar coordinates (r, theta) of the point (x, y)
"""
r = math.hypot(x, y)
theta = math.atan2(y, x)
return r, theta
def M(theta):
"""
Regulate theta to -pi <= theta < pi
"""
phi = theta % (2.0 * PI)
if phi < -PI:
phi += 2.0 * PI
if phi > PI:
phi -= 2.0 * PI
return phi
def get_label(path):
label = ""
for m, l in zip(path.ctypes, path.lengths):
label = label + m
if l > 0.0:
label = label + "+"
else:
label = label + "-"
return label
def calc_curvature(x, y, yaw, directions):
c, ds = [], []
for i in range(1, len(x) - 1):
dxn = x[i] - x[i - 1]
dxp = x[i + 1] - x[i]
dyn = y[i] - y[i - 1]
dyp = y[i + 1] - y[i]
dn = math.hypot(dxn, dyn)
dp = math.hypot(dxp, dyp)
dx = 1.0 / (dn + dp) * (dp / dn * dxn + dn / dp * dxp)
ddx = 2.0 / (dn + dp) * (dxp / dp - dxn / dn)
dy = 1.0 / (dn + dp) * (dp / dn * dyn + dn / dp * dyp)
ddy = 2.0 / (dn + dp) * (dyp / dp - dyn / dn)
curvature = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
d = (dn + dp) / 2.0
if np.isnan(curvature):
curvature = 0.0
if directions[i] <= 0.0:
curvature = -curvature
if len(c) == 0:
ds.append(d)
c.append(curvature)
ds.append(d)
c.append(curvature)
ds.append(ds[-1])
c.append(c[-1])
return c, ds
def check_path(sx, sy, syaw, gx, gy, gyaw, maxc):
paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc)
assert len(paths) >= 1
for path in paths:
assert abs(path.x[0] - sx) <= 0.01
assert abs(path.y[0] - sy) <= 0.01
assert abs(path.yaw[0] - syaw) <= 0.01
assert abs(path.x[-1] - gx) <= 0.01
assert abs(path.y[-1] - gy) <= 0.01
assert abs(path.yaw[-1] - gyaw) <= 0.01
# course distance check
d = [math.hypot(dx, dy)
for dx, dy in zip(np.diff(path.x[0:len(path.x) - 1]),
np.diff(path.y[0:len(path.y) - 1]))]
for i in range(len(d)):
assert abs(d[i] - STEP_SIZE) <= 0.001
def main():
# choose states pairs: (x, y, yaw)
# simulation-1
# states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120),
# (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)]
# simulation-2
states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
(35, 10, 180), (32, -10, 180), (5, -12, 90)]
max_c = 0.1 # max curvature
path_x, path_y, yaw = [], [], []
for i in range(len(states) - 1):
s_x = states[i][0]
s_y = states[i][1]
s_yaw = np.deg2rad(states[i][2])
g_x = states[i + 1][0]
g_y = states[i + 1][1]
g_yaw = np.deg2rad(states[i + 1][2])
path_i = calc_optimal_path(s_x, s_y, s_yaw,
g_x, g_y, g_yaw, max_c)
path_x += path_i.x
path_y += path_i.y
yaw += path_i.yaw
# animation
plt.ion()
plt.figure(1)
for i in range(len(path_x)):
plt.clf()
plt.plot(path_x, path_y, linewidth=1, color='gray')
for x, y, theta in states:
draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet')
draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3)
plt.axis("equal")
plt.title("Simulation of Reeds-Shepp Curves")
plt.axis([-10, 42, -20, 20])
plt.draw()
plt.pause(0.001)
plt.pause(1)
if __name__ == '__main__':
main()
Reference in New Issue View Git Blame Copy Permalink