github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-16 17:35:37 -05:00
Code Issues Packages Projects Releases Wiki Activity

mypy fix test

Browse Source
This commit is contained in:
Atsushi Sakai
2020-03-09 22:57:35 +09:00
parent c36aa27d60
commit 14ffc4a2d4
13 changed files with 150 additions and 366 deletions
Download Patch File Download Diff File Expand all files Collapse all files

109
PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py
View File

@@ -6,9 +6,11 @@ author: Atsushi Sakai (@Atsushi_twi)
Ref:
- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame](https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame]
(https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
- [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame](https://www.youtube.com/watch?v=Cj6tAQe7UCY)
- [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame]
(https://www.youtube.com/watch?v=Cj6tAQe7UCY)
"""
@@ -16,19 +18,20 @@ import numpy as np
import matplotlib.pyplot as plt
import copy
import math
import cubic_spline_planner
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../QuinticPolynomialsPlanner/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../CubicSpline/")
try:
from quintic_polynomials_planner import QuinticPolynomial
import cubic_spline_planner
except ImportError:
raise
SIM_LOOP = 500
# Parameter
@@ -38,36 +41,35 @@ MAX_CURVATURE = 1.0 # maximum curvature [1/m]
MAX_ROAD_WIDTH = 7.0 # maximum road width [m]
D_ROAD_W = 1.0 # road width sampling length [m]
DT = 0.2 # time tick [s]
MAXT = 5.0 # max prediction time [m]
MINT = 4.0 # min prediction time [m]
MAX_T = 5.0 # max prediction time [m]
MIN_T = 4.0 # min prediction time [m]
TARGET_SPEED = 30.0 / 3.6 # target speed [m/s]
D_T_S = 5.0 / 3.6 # target speed sampling length [m/s]
N_S_SAMPLE = 1 # sampling number of target speed
ROBOT_RADIUS = 2.0 # robot radius [m]
# cost weights
KJ = 0.1
KT = 0.1
KD = 1.0
KLAT = 1.0
KLON = 1.0
K_J = 0.1
K_T = 0.1
K_D = 1.0
K_LAT = 1.0
K_LON = 1.0
show_animation = True
class quartic_polynomial:
def __init__(self, xs, vxs, axs, vxe, axe, T):
class QuarticPolynomial:
def __init__(self, xs, vxs, axs, vxe, axe, time):
# calc coefficient of quartic polynomial
self.a0 = xs
self.a1 = vxs
self.a2 = axs / 2.0
A = np.array([[3 * T ** 2, 4 * T ** 3],
[6 * T, 12 * T ** 2]])
b = np.array([vxe - self.a1 - 2 * self.a2 * T,
A = np.array([[3 * time ** 2, 4 * time ** 3],
[6 * time, 12 * time ** 2]])
b = np.array([vxe - self.a1 - 2 * self.a2 * time,
axe - 2 * self.a2])
x = np.linalg.solve(A, b)
@@ -75,19 +77,19 @@ class quartic_polynomial:
self.a4 = x[1]
def calc_point(self, t):
xt = self.a0 + self.a1 * t + self.a2 * t**2 + \
self.a3 * t**3 + self.a4 * t**4
xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
self.a3 * t ** 3 + self.a4 * t ** 4
return xt
def calc_first_derivative(self, t):
xt = self.a1 + 2 * self.a2 * t + \
3 * self.a3 * t**2 + 4 * self.a4 * t**3
3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3
return xt
def calc_second_derivative(self, t):
xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2
xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2
return xt
@@ -97,7 +99,7 @@ class quartic_polynomial:
return xt
class Frenet_path:
class FrenetPath:
def __init__(self):
self.t = []
@@ -121,15 +123,14 @@ class Frenet_path:
def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
frenet_paths = []
# generate path to each offset goal
for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
# Lateral motion planning
for Ti in np.arange(MINT, MAXT, DT):
fp = Frenet_path()
for Ti in np.arange(MIN_T, MAX_T, DT):
fp = FrenetPath()
# lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
@@ -141,9 +142,10 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
# Longitudinal motion planning (Velocity keeping)
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
tfp = copy.deepcopy(fp)
lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
tfp.s = [lon_qp.calc_point(t) for t in fp.t]
tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
@@ -154,11 +156,11 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk
# square of diff from target speed
ds = (TARGET_SPEED - tfp.s_d[-1])**2
ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2
tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1]**2
tfp.cv = KJ * Js + KT * Ti + KD * ds
tfp.cf = KLAT * tfp.cd + KLON * tfp.cv
tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1] ** 2
tfp.cv = K_J * Js + K_T * Ti + K_D * ds
tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv
frenet_paths.append(tfp)
@@ -166,7 +168,6 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
def calc_global_paths(fplist, csp):
for fp in fplist:
# calc global positions
@@ -174,10 +175,10 @@ def calc_global_paths(fplist, csp):
ix, iy = csp.calc_position(fp.s[i])
if ix is None:
break
iyaw = csp.calc_yaw(fp.s[i])
i_yaw = csp.calc_yaw(fp.s[i])
di = fp.d[i]
fx = ix + di * math.cos(iyaw + math.pi / 2.0)
fy = iy + di * math.sin(iyaw + math.pi / 2.0)
fx = ix + di * math.cos(i_yaw + math.pi / 2.0)
fy = iy + di * math.sin(i_yaw + math.pi / 2.0)
fp.x.append(fx)
fp.y.append(fy)
@@ -199,12 +200,11 @@ def calc_global_paths(fplist, csp):
def check_collision(fp, ob):
for i in range(len(ob[:, 0])):
d = [((ix - ob[i, 0])**2 + (iy - ob[i, 1])**2)
d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
for (ix, iy) in zip(fp.x, fp.y)]
collision = any([di <= ROBOT_RADIUS**2 for di in d])
collision = any([di <= ROBOT_RADIUS ** 2 for di in d])
if collision:
return False
@@ -213,38 +213,38 @@ def check_collision(fp, ob):
def check_paths(fplist, ob):
okind = []
ok_ind = []
for i, _ in enumerate(fplist):
if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check
continue
elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]): # Max accel check
elif any([abs(a) > MAX_ACCEL for a in
fplist[i].s_dd]): # Max accel check
continue
elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]): # Max curvature check
elif any([abs(c) > MAX_CURVATURE for c in
fplist[i].c]): # Max curvature check
continue
elif not check_collision(fplist[i], ob):
continue
okind.append(i)
ok_ind.append(i)
return [fplist[i] for i in okind]
return [fplist[i] for i in ok_ind]
def frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob):
fplist = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0)
fplist = calc_global_paths(fplist, csp)
fplist = check_paths(fplist, ob)
# find minimum cost path
mincost = float("inf")
bestpath = None
min_cost = float("inf")
best_path = None
for fp in fplist:
if mincost >= fp.cf:
mincost = fp.cf
bestpath = fp
if min_cost >= fp.cf:
min_cost = fp.cf
best_path = fp
return bestpath
return best_path
def generate_target_course(x, y):
@@ -282,7 +282,7 @@ def main():
c_speed = 10.0 / 3.6 # current speed [m/s]
c_d = 2.0 # current lateral position [m]
c_d_d = 0.0 # current lateral speed [m/s]
c_d_dd = 0.0 # current latral acceleration [m/s]
c_d_dd = 0.0 # current lateral acceleration [m/s]
s0 = 0.0 # current course position
area = 20.0 # animation area length [m]
@@ -304,8 +304,9 @@ def main():
if show_animation: # pragma: no cover
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.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(tx, ty)
plt.plot(ob[:, 0], ob[:, 1], "xk")
plt.plot(path.x[1:], path.y[1:], "-or")