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

Merge pull request #301 from AtsushiSakai/issue300

Browse Source 
Improve B Spline path planner
This commit is contained in:
Atsushi Sakai
2020-03-13 23:10:32 +09:00
committed by GitHub
parent 869650ffa9 4107c42406
commit 4093b642e0
15 changed files with 218 additions and 389 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
.github/workflows/pythonpackage.yml vendored
View File

@@ -9,7 +9,7 @@ jobs:
strategy:
max-parallel: 4
matrix:
python-version: [3.6, 3.7]
python-version: [3.7]
steps:
- uses: actions/checkout@v1
@@ -29,6 +29,21 @@ jobs:
# exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide
flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics
- name: install coverage
run: pip install coverage
- name: Test
run: pip install coverage
- name: install mypy
run: pip install mypy
- name: mypy check
run: |
find AerialNavigation -name "*.py" | xargs mypy
find ArmNavigation -name "*.py" | xargs mypy
find Bipedal -name "*.py" | xargs mypy
find InvertedPendulumCart -name "*.py" | xargs mypy
find Localization -name "*.py" | xargs mypy
find Mapping -name "*.py" | xargs mypy
find PathPlanning -name "*.py" | xargs mypy
find PathTracking -name "*.py" | xargs mypy
find SLAM -name "*.py" | xargs mypy
- name: do all unit tests
run: bash runtests.sh

116
ArmNavigation/n_joint_arm_3d/NLinkArm.py → ArmNavigation/n_joint_arm_3d/NLinkArm3d.py
View File

@@ -7,6 +7,7 @@ import math
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
class Link:
def __init__(self, dh_params):
self.dh_params_ = dh_params
@@ -28,16 +29,22 @@ class Link:
return trans
def basic_jacobian(self, trans_prev, ee_pos):
pos_prev = np.array([trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
z_axis_prev = np.array([trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])
@staticmethod
def basic_jacobian(trans_prev, ee_pos):
pos_prev = np.array(
[trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
z_axis_prev = np.array(
[trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])
basic_jacobian = np.hstack((np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))
basic_jacobian = np.hstack(
(np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))
return basic_jacobian
class NLinkArm:
def __init__(self, dh_params_list):
self.fig = plt.figure()
self.ax = Axes3D(self.fig)
self.link_list = []
for i in range(len(dh_params_list)):
self.link_list.append(Link(dh_params_list[i]))
@@ -47,7 +54,7 @@ class NLinkArm:
for i in range(len(self.link_list)):
trans = np.dot(trans, self.link_list[i].transformation_matrix())
return trans
def forward_kinematics(self, plot=False):
trans = self.transformation_matrix()
@@ -57,15 +64,12 @@ class NLinkArm:
alpha, beta, gamma = self.euler_angle()
if plot:
self.fig = plt.figure()
self.ax = Axes3D(self.fig)
x_list = []
y_list = []
z_list = []
trans = np.identity(4)
x_list.append(trans[0, 3])
y_list.append(trans[1, 3])
z_list.append(trans[2, 3])
@@ -74,14 +78,15 @@ class NLinkArm:
x_list.append(trans[0, 3])
y_list.append(trans[1, 3])
z_list.append(trans[2, 3])
self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4,
mew=0.5)
self.ax.plot([0], [0], [0], "o")
self.ax.set_xlim(-1, 1)
self.ax.set_ylim(-1, 1)
self.ax.set_zlim(-1, 1)
plt.show()
return [x, y, z, alpha, beta, gamma]
@@ -89,41 +94,45 @@ class NLinkArm:
def basic_jacobian(self):
ee_pos = self.forward_kinematics()[0:3]
basic_jacobian_mat = []
trans = np.identity(4)
trans = np.identity(4)
for i in range(len(self.link_list)):
basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ee_pos))
basic_jacobian_mat.append(
self.link_list[i].basic_jacobian(trans, ee_pos))
trans = np.dot(trans, self.link_list[i].transformation_matrix())
return np.array(basic_jacobian_mat).T
def inverse_kinematics(self, ref_ee_pose, plot=False):
for cnt in range(500):
ee_pose = self.forward_kinematics()
diff_pose = np.array(ref_ee_pose) - ee_pose
basic_jacobian_mat = self.basic_jacobian()
alpha, beta, gamma = self.euler_angle()
K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
[0, math.cos(alpha), math.sin(alpha) * math.sin(beta)],
[1, 0, math.cos(beta)]])
K_zyz = np.array(
[[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
[0, math.cos(alpha), math.sin(alpha) * math.sin(beta)],
[1, 0, math.cos(beta)]])
K_alpha = np.identity(6)
K_alpha[3:, 3:] = K_zyz
theta_dot = np.dot(np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha), np.array(diff_pose))
theta_dot = np.dot(
np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha),
np.array(diff_pose))
self.update_joint_angles(theta_dot / 100.)
if plot:
self.fig = plt.figure()
self.ax = Axes3D(self.fig)
x_list = []
y_list = []
z_list = []
trans = np.identity(4)
x_list.append(trans[0, 3])
y_list.append(trans[1, 3])
z_list.append(trans[2, 3])
@@ -132,30 +141,36 @@ class NLinkArm:
x_list.append(trans[0, 3])
y_list.append(trans[1, 3])
z_list.append(trans[2, 3])
self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4,
mew=0.5)
self.ax.plot([0], [0], [0], "o")
self.ax.set_xlim(-1, 1)
self.ax.set_ylim(-1, 1)
self.ax.set_zlim(-1, 1)
self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]], "o")
self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]],
"o")
plt.show()
def euler_angle(self):
trans = self.transformation_matrix()
alpha = math.atan2(trans[1][2], trans[0][2])
if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2):
if not (-math.pi / 2 <= alpha <= math.pi / 2):
alpha = math.atan2(trans[1][2], trans[0][2]) + math.pi
if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2):
if not (-math.pi / 2 <= alpha <= math.pi / 2):
alpha = math.atan2(trans[1][2], trans[0][2]) - math.pi
beta = math.atan2(trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha), trans[2][2])
gamma = math.atan2(-trans[0][0] * math.sin(alpha) + trans[1][0] * math.cos(alpha), -trans[0][1] * math.sin(alpha) + trans[1][1] * math.cos(alpha))
beta = math.atan2(
trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha),
trans[2][2])
gamma = math.atan2(
-trans[0][0] * math.sin(alpha) + trans[1][0] * math.cos(alpha),
-trans[0][1] * math.sin(alpha) + trans[1][1] * math.cos(alpha))
return alpha, beta, gamma
def set_joint_angles(self, joint_angle_list):
for i in range(len(self.link_list)):
self.link_list[i].dh_params_[0] = joint_angle_list[i]
@@ -163,17 +178,17 @@ class NLinkArm:
def update_joint_angles(self, diff_joint_angle_list):
for i in range(len(self.link_list)):
self.link_list[i].dh_params_[0] += diff_joint_angle_list[i]
def plot(self):
self.fig = plt.figure()
self.ax = Axes3D(self.fig)
x_list = []
y_list = []
z_list = []
trans = np.identity(4)
x_list.append(trans[0, 3])
y_list.append(trans[1, 3])
z_list.append(trans[2, 3])
@@ -182,15 +197,16 @@ class NLinkArm:
x_list.append(trans[0, 3])
y_list.append(trans[1, 3])
z_list.append(trans[2, 3])
self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5)
self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4,
mew=0.5)
self.ax.plot([0], [0], [0], "o")
self.ax.set_xlabel("x")
self.ax.set_ylabel("y")
self.ax.set_zlabel("z")
self.ax.set_zlabel("z")
self.ax.set_xlim(-1, 1)
self.ax.set_ylim(-1, 1)
self.ax.set_zlim(-1, 1)
self.ax.set_zlim(-1, 1)
plt.show()

0
ArmNavigation/n_joint_arm_3d/__init__py.py Normal file
View File

21
ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py
View File

@@ -3,9 +3,10 @@ Forward Kinematics for an n-link arm in 3D
Author: Takayuki Murooka (takayuki5168)
"""
import math
from NLinkArm import NLinkArm
from NLinkArm3d import NLinkArm
import random
def random_val(min_val, max_val):
return min_val + random.random() * (max_val - min_val)
@@ -14,17 +15,17 @@ if __name__ == "__main__":
print("Start solving Forward Kinematics 10 times")
# init NLinkArm with Denavit-Hartenberg parameters of PR2
n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
[math.pi/2, math.pi/2, 0., 0.],
[0., -math.pi/2, 0., .4],
[0., math.pi/2, 0., 0.],
[0., -math.pi/2, 0., .321],
[0., math.pi/2, 0., 0.],
n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.],
[math.pi / 2, math.pi / 2, 0., 0.],
[0., -math.pi / 2, 0., .4],
[0., math.pi / 2, 0., 0.],
[0., -math.pi / 2, 0., .321],
[0., math.pi / 2, 0., 0.],
[0., 0., 0., 0.]])
# execute FK 10 times
for i in range(10):
n_link_arm.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
ee_pose = n_link_arm.forward_kinematics(plot=True)
n_link_arm.set_joint_angles(
[random_val(-1, 1) for j in range(len(n_link_arm.link_list))])
ee_pose = n_link_arm.forward_kinematics(plot=True)

15
ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py
View File

@@ -3,23 +3,24 @@ Inverse Kinematics for an n-link arm in 3D
Author: Takayuki Murooka (takayuki5168)
"""
import math
from NLinkArm import NLinkArm
from NLinkArm3d import NLinkArm
import random
def random_val(min_val, max_val):
return min_val + random.random() * (max_val - min_val)
if __name__ == "__main__":
print("Start solving Inverse Kinematics 10 times")
# init NLinkArm with Denavit-Hartenberg parameters of PR2
n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.],
[math.pi/2, math.pi/2, 0., 0.],
[0., -math.pi/2, 0., .4],
[0., math.pi/2, 0., 0.],
[0., -math.pi/2, 0., .321],
[0., math.pi/2, 0., 0.],
n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.],
[math.pi / 2, math.pi / 2, 0., 0.],
[0., -math.pi / 2, 0., .4],
[0., math.pi / 2, 0., 0.],
[0., -math.pi / 2, 0., .321],
[0., math.pi / 2, 0., 0.],
[0., 0., 0., 0.]])
# execute IK 10 times

0
ArmNavigation/n_joint_arm_to_point_control/__init__.py Normal file
View File

12
PathPlanning/AStar/a_star.py
View File

@@ -41,7 +41,8 @@ class AStarPlanner:
self.pind = pind
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
return str(self.x) + "," + str(self.y) + "," + str(
self.cost) + "," + str(self.pind)
def planning(self, sx, sy, gx, gy):
"""
@@ -72,7 +73,10 @@ class AStarPlanner:
break
c_id = min(
open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o]))
open_set,
key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal,
open_set[
o]))
current = open_set[c_id]
# show graph
@@ -81,7 +85,8 @@ class AStarPlanner:
self.calc_grid_position(current.y, self.miny), "xc")
# 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 len(closed_set.keys()) % 10 == 0:
plt.pause(0.001)
@@ -104,7 +109,6 @@ class AStarPlanner:
current.cost + self.motion[i][2], c_id)
n_id = self.calc_grid_index(node)
# If the node is not safe, do nothing
if not self.verify_node(node):
continue

70
PathPlanning/BSplinePath/bspline_path.py
View File

@@ -1,6 +1,6 @@
"""
Path Plannting with B-Spline
Path Planner with B-Spline
author: Atsushi Sakai (@Atsushi_twi)
@@ -8,44 +8,72 @@ author: Atsushi Sakai (@Atsushi_twi)
import numpy as np
import matplotlib.pyplot as plt
import scipy.interpolate as si
# parameter
N = 3 # B Spline order
import scipy.interpolate as scipy_interpolate
def bspline_planning(x, y, sn):
def approximate_b_spline_path(x: list, y: list, n_path_points: int,
degree: int = 3) -> tuple:
"""
approximate points with a B-Spline path
:param x: x position list of approximated points
:param y: y position list of approximated points
:param n_path_points: number of path points
:param degree: (Optional) B Spline curve degree
:return: x and y position list of the result path
"""
t = range(len(x))
x_tup = si.splrep(t, x, k=N)
y_tup = si.splrep(t, y, k=N)
x_tup = scipy_interpolate.splrep(t, x, k=degree)
y_tup = scipy_interpolate.splrep(t, y, k=degree)
x_list = list(x_tup)
xl = x.tolist()
x_list[1] = xl + [0.0, 0.0, 0.0, 0.0]
x_list[1] = x + [0.0, 0.0, 0.0, 0.0]
y_list = list(y_tup)
yl = y.tolist()
y_list[1] = yl + [0.0, 0.0, 0.0, 0.0]
y_list[1] = y + [0.0, 0.0, 0.0, 0.0]
ipl_t = np.linspace(0.0, len(x) - 1, sn)
rx = si.splev(ipl_t, x_list)
ry = si.splev(ipl_t, y_list)
ipl_t = np.linspace(0.0, len(x) - 1, n_path_points)
rx = scipy_interpolate.splev(ipl_t, x_list)
ry = scipy_interpolate.splev(ipl_t, y_list)
return rx, ry
def interpolate_b_spline_path(x: list, y: list, n_path_points: int,
degree: int = 3) -> tuple:
"""
interpolate points with a B-Spline path
:param x: x positions of interpolated points
:param y: y positions of interpolated points
:param n_path_points: number of path points
:param degree: B-Spline degree
:return: x and y position list of the result path
"""
ipl_t = np.linspace(0.0, len(x) - 1, len(x))
spl_i_x = scipy_interpolate.make_interp_spline(ipl_t, x, k=degree)
spl_i_y = scipy_interpolate.make_interp_spline(ipl_t, y, k=degree)
travel = np.linspace(0.0, len(x) - 1, n_path_points)
return spl_i_x(travel), spl_i_y(travel)
def main():
print(__file__ + " start!!")
# way points
x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0])
y = np.array([0.0, -3.0, 1.0, 1.0, 3.0])
sn = 100 # sampling number
way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0]
way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0]
n_course_point = 100 # sampling number
rx, ry = bspline_planning(x, y, sn)
rax, ray = approximate_b_spline_path(way_point_x, way_point_y,
n_course_point)
rix, riy = interpolate_b_spline_path(way_point_x, way_point_y,
n_course_point)
# show results
plt.plot(x, y, '-og', label="Waypoints")
plt.plot(rx, ry, '-r', label="B-Spline path")
plt.plot(way_point_x, way_point_y, '-og', label="way points")
plt.plot(rax, ray, '-r', label="Approximated B-Spline path")
plt.plot(rix, riy, '-b', label="Interpolated B-Spline path")
plt.grid(True)
plt.legend()
plt.axis("equal")

0
PathPlanning/CubicSpline/__init__.py Normal file
View File

239
PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py
View File

@@ -1,239 +0,0 @@
"""
cubic spline planner
Author: Atsushi Sakai
"""
import math
import numpy as np
import bisect
class Spline:
"""
Cubic Spline class
"""
def __init__(self, x, y):
self.b, self.c, self.d, self.w = [], [], [], []
self.x = x
self.y = y
self.nx = len(x) # dimension of x
h = np.diff(x)
# calc coefficient c
self.a = [iy for iy in y]
# calc coefficient c
A = self.__calc_A(h)
B = self.__calc_B(h)
self.c = np.linalg.solve(A, B)
# print(self.c1)
# calc spline coefficient b and d
for i in range(self.nx - 1):
self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))
tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \
(self.c[i + 1] + 2.0 * self.c[i]) / 3.0
self.b.append(tb)
def calc(self, t):
"""
Calc position
if t is outside of the input x, return None
"""
if t < self.x[0]:
return None
elif t > self.x[-1]:
return None
i = self.__search_index(t)
dx = t - self.x[i]
result = self.a[i] + self.b[i] * dx + \
self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0
return result
def calcd(self, t):
"""
Calc first derivative
if t is outside of the input x, return None
"""
if t < self.x[0]:
return None
elif t > self.x[-1]:
return None
i = self.__search_index(t)
dx = t - self.x[i]
result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
return result
def calcdd(self, t):
"""
Calc second derivative
"""
if t < self.x[0]:
return None
elif t > self.x[-1]:
return None
i = self.__search_index(t)
dx = t - self.x[i]
result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
return result
def __search_index(self, x):
"""
search data segment index
"""
return bisect.bisect(self.x, x) - 1
def __calc_A(self, h):
"""
calc matrix A for spline coefficient c
"""
A = np.zeros((self.nx, self.nx))
A[0, 0] = 1.0
for i in range(self.nx - 1):
if i != (self.nx - 2):
A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
A[i + 1, i] = h[i]
A[i, i + 1] = h[i]
A[0, 1] = 0.0
A[self.nx - 1, self.nx - 2] = 0.0
A[self.nx - 1, self.nx - 1] = 1.0
# print(A)
return A
def __calc_B(self, h):
"""
calc matrix B for spline coefficient c
"""
B = np.zeros(self.nx)
for i in range(self.nx - 2):
B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \
h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]
# print(B)
return B
class Spline2D:
"""
2D Cubic Spline class
"""
def __init__(self, x, y):
self.s = self.__calc_s(x, y)
self.sx = Spline(self.s, x)
self.sy = Spline(self.s, y)
def __calc_s(self, x, y):
dx = np.diff(x)
dy = np.diff(y)
self.ds = [math.sqrt(idx ** 2 + idy ** 2)
for (idx, idy) in zip(dx, dy)]
s = [0]
s.extend(np.cumsum(self.ds))
return s
def calc_position(self, s):
"""
calc position
"""
x = self.sx.calc(s)
y = self.sy.calc(s)
return x, y
def calc_curvature(self, s):
"""
calc curvature
"""
dx = self.sx.calcd(s)
ddx = self.sx.calcdd(s)
dy = self.sy.calcd(s)
ddy = self.sy.calcdd(s)
k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
return k
def calc_yaw(self, s):
"""
calc yaw
"""
dx = self.sx.calcd(s)
dy = self.sy.calcd(s)
yaw = math.atan2(dy, dx)
return yaw
def calc_spline_course(x, y, ds=0.1):
sp = Spline2D(x, y)
s = list(np.arange(0, sp.s[-1], ds))
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
rx.append(ix)
ry.append(iy)
ryaw.append(sp.calc_yaw(i_s))
rk.append(sp.calc_curvature(i_s))
return rx, ry, ryaw, rk, s
def main(): # pragma: no cover
print("Spline 2D test")
import matplotlib.pyplot as plt
x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]
sp = Spline2D(x, y)
s = np.arange(0, sp.s[-1], 0.1)
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
rx.append(ix)
ry.append(iy)
ryaw.append(sp.calc_yaw(i_s))
rk.append(sp.calc_curvature(i_s))
plt.subplots(1)
plt.plot(x, y, "xb", label="input")
plt.plot(rx, ry, "-r", label="spline")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
plt.subplots(1)
plt.plot(s, rk, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.show()
if __name__ == '__main__': # pragma: no cover
main()

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")

0
PathPlanning/HybridAStar/__init__.py Normal file
View File

0
PathPlanning/HybridAStar/a_star.py → PathPlanning/HybridAStar/a_star_heuristic.py
View File

2
PathPlanning/HybridAStar/hybrid_a_star.py
View File

@@ -14,7 +14,7 @@ import matplotlib.pyplot as plt
import sys
sys.path.append("../ReedsSheppPath/")
try:
from a_star import dp_planning # , calc_obstacle_map
from a_star_heuristic import dp_planning # , calc_obstacle_map
import reeds_shepp_path_planning as rs
from car import move, check_car_collision, MAX_STEER, WB, plot_car
except:

2
mypy.ini Normal file
View File

@@ -0,0 +1,2 @@
[mypy]
ignore_missing_imports = True