github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-16 15:15:17 -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

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 from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
class Link: class Link:
def __init__(self, dh_params): def __init__(self, dh_params):
self.dh_params_ = dh_params self.dh_params_ = dh_params
@@ -28,16 +29,22 @@ class Link:
return trans return trans
def basic_jacobian(self, trans_prev, ee_pos): @staticmethod
pos_prev = np.array([trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]]) def basic_jacobian(trans_prev, ee_pos):
z_axis_prev = np.array([trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]]) 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 return basic_jacobian
class NLinkArm: class NLinkArm:
def __init__(self, dh_params_list): def __init__(self, dh_params_list):
self.fig = plt.figure()
self.ax = Axes3D(self.fig)
self.link_list = [] self.link_list = []
for i in range(len(dh_params_list)): for i in range(len(dh_params_list)):
self.link_list.append(Link(dh_params_list[i])) self.link_list.append(Link(dh_params_list[i]))
@@ -47,7 +54,7 @@ class NLinkArm:
for i in range(len(self.link_list)): for i in range(len(self.link_list)):
trans = np.dot(trans, self.link_list[i].transformation_matrix()) trans = np.dot(trans, self.link_list[i].transformation_matrix())
return trans return trans
def forward_kinematics(self, plot=False): def forward_kinematics(self, plot=False):
trans = self.transformation_matrix() trans = self.transformation_matrix()
@@ -57,15 +64,12 @@ class NLinkArm:
alpha, beta, gamma = self.euler_angle() alpha, beta, gamma = self.euler_angle()
if plot: if plot:
self.fig = plt.figure()
self.ax = Axes3D(self.fig)
x_list = [] x_list = []
y_list = [] y_list = []
z_list = [] z_list = []
trans = np.identity(4) trans = np.identity(4)
x_list.append(trans[0, 3]) x_list.append(trans[0, 3])
y_list.append(trans[1, 3]) y_list.append(trans[1, 3])
z_list.append(trans[2, 3]) z_list.append(trans[2, 3])
@@ -74,14 +78,15 @@ class NLinkArm:
x_list.append(trans[0, 3]) x_list.append(trans[0, 3])
y_list.append(trans[1, 3]) y_list.append(trans[1, 3])
z_list.append(trans[2, 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.plot([0], [0], [0], "o")
self.ax.set_xlim(-1, 1) self.ax.set_xlim(-1, 1)
self.ax.set_ylim(-1, 1) self.ax.set_ylim(-1, 1)
self.ax.set_zlim(-1, 1) self.ax.set_zlim(-1, 1)
plt.show() plt.show()
return [x, y, z, alpha, beta, gamma] return [x, y, z, alpha, beta, gamma]
@@ -89,41 +94,45 @@ class NLinkArm:
def basic_jacobian(self): def basic_jacobian(self):
ee_pos = self.forward_kinematics()[0:3] ee_pos = self.forward_kinematics()[0:3]
basic_jacobian_mat = [] basic_jacobian_mat = []
trans = np.identity(4) trans = np.identity(4)
for i in range(len(self.link_list)): 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()) trans = np.dot(trans, self.link_list[i].transformation_matrix())
return np.array(basic_jacobian_mat).T return np.array(basic_jacobian_mat).T
def inverse_kinematics(self, ref_ee_pose, plot=False): def inverse_kinematics(self, ref_ee_pose, plot=False):
for cnt in range(500): for cnt in range(500):
ee_pose = self.forward_kinematics() ee_pose = self.forward_kinematics()
diff_pose = np.array(ref_ee_pose) - ee_pose diff_pose = np.array(ref_ee_pose) - ee_pose
basic_jacobian_mat = self.basic_jacobian() basic_jacobian_mat = self.basic_jacobian()
alpha, beta, gamma = self.euler_angle() alpha, beta, gamma = self.euler_angle()
K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)], K_zyz = np.array(
[0, math.cos(alpha), math.sin(alpha) * math.sin(beta)], [[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)],
[1, 0, math.cos(beta)]]) [0, math.cos(alpha), math.sin(alpha) * math.sin(beta)],
[1, 0, math.cos(beta)]])
K_alpha = np.identity(6) K_alpha = np.identity(6)
K_alpha[3:, 3:] = K_zyz 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.) self.update_joint_angles(theta_dot / 100.)
if plot: if plot:
self.fig = plt.figure() self.fig = plt.figure()
self.ax = Axes3D(self.fig) self.ax = Axes3D(self.fig)
x_list = [] x_list = []
y_list = [] y_list = []
z_list = [] z_list = []
trans = np.identity(4) trans = np.identity(4)
x_list.append(trans[0, 3]) x_list.append(trans[0, 3])
y_list.append(trans[1, 3]) y_list.append(trans[1, 3])
z_list.append(trans[2, 3]) z_list.append(trans[2, 3])
@@ -132,30 +141,36 @@ class NLinkArm:
x_list.append(trans[0, 3]) x_list.append(trans[0, 3])
y_list.append(trans[1, 3]) y_list.append(trans[1, 3])
z_list.append(trans[2, 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.plot([0], [0], [0], "o")
self.ax.set_xlim(-1, 1) self.ax.set_xlim(-1, 1)
self.ax.set_ylim(-1, 1) self.ax.set_ylim(-1, 1)
self.ax.set_zlim(-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() plt.show()
def euler_angle(self): def euler_angle(self):
trans = self.transformation_matrix() trans = self.transformation_matrix()
alpha = math.atan2(trans[1][2], trans[0][2]) 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 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 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]) beta = math.atan2(
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)) 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 return alpha, beta, gamma
def set_joint_angles(self, joint_angle_list): def set_joint_angles(self, joint_angle_list):
for i in range(len(self.link_list)): for i in range(len(self.link_list)):
self.link_list[i].dh_params_[0] = joint_angle_list[i] 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): def update_joint_angles(self, diff_joint_angle_list):
for i in range(len(self.link_list)): for i in range(len(self.link_list)):
self.link_list[i].dh_params_[0] += diff_joint_angle_list[i] self.link_list[i].dh_params_[0] += diff_joint_angle_list[i]
def plot(self): def plot(self):
self.fig = plt.figure() self.fig = plt.figure()
self.ax = Axes3D(self.fig) self.ax = Axes3D(self.fig)
x_list = [] x_list = []
y_list = [] y_list = []
z_list = [] z_list = []
trans = np.identity(4) trans = np.identity(4)
x_list.append(trans[0, 3]) x_list.append(trans[0, 3])
y_list.append(trans[1, 3]) y_list.append(trans[1, 3])
z_list.append(trans[2, 3]) z_list.append(trans[2, 3])
@@ -182,15 +197,16 @@ class NLinkArm:
x_list.append(trans[0, 3]) x_list.append(trans[0, 3])
y_list.append(trans[1, 3]) y_list.append(trans[1, 3])
z_list.append(trans[2, 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.plot([0], [0], [0], "o")
self.ax.set_xlabel("x") self.ax.set_xlabel("x")
self.ax.set_ylabel("y") self.ax.set_ylabel("y")
self.ax.set_zlabel("z") self.ax.set_zlabel("z")
self.ax.set_xlim(-1, 1) self.ax.set_xlim(-1, 1)
self.ax.set_ylim(-1, 1) self.ax.set_ylim(-1, 1)
self.ax.set_zlim(-1, 1) self.ax.set_zlim(-1, 1)
plt.show() 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) Author: Takayuki Murooka (takayuki5168)
""" """
import math import math
from NLinkArm import NLinkArm from NLinkArm3d import NLinkArm
import random import random
def random_val(min_val, max_val): def random_val(min_val, max_val):
return min_val + random.random() * (max_val - min_val) return min_val + random.random() * (max_val - min_val)
@@ -14,17 +15,17 @@ if __name__ == "__main__":
print("Start solving Forward Kinematics 10 times") print("Start solving Forward Kinematics 10 times")
# init NLinkArm with Denavit-Hartenberg parameters of PR2 # init NLinkArm with Denavit-Hartenberg parameters of PR2
n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.], n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.],
[math.pi/2, math.pi/2, 0., 0.], [math.pi / 2, math.pi / 2, 0., 0.],
[0., -math.pi/2, 0., .4], [0., -math.pi / 2, 0., .4],
[0., math.pi/2, 0., 0.], [0., math.pi / 2, 0., 0.],
[0., -math.pi/2, 0., .321], [0., -math.pi / 2, 0., .321],
[0., math.pi/2, 0., 0.], [0., math.pi / 2, 0., 0.],
[0., 0., 0., 0.]]) [0., 0., 0., 0.]])
# execute FK 10 times # execute FK 10 times
for i in range(10): 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))]) 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)
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) Author: Takayuki Murooka (takayuki5168)
""" """
import math import math
from NLinkArm import NLinkArm from NLinkArm3d import NLinkArm
import random import random
def random_val(min_val, max_val): def random_val(min_val, max_val):
return min_val + random.random() * (max_val - min_val) return min_val + random.random() * (max_val - min_val)
if __name__ == "__main__": if __name__ == "__main__":
print("Start solving Inverse Kinematics 10 times") print("Start solving Inverse Kinematics 10 times")
# init NLinkArm with Denavit-Hartenberg parameters of PR2 # init NLinkArm with Denavit-Hartenberg parameters of PR2
n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.], n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.],
[math.pi/2, math.pi/2, 0., 0.], [math.pi / 2, math.pi / 2, 0., 0.],
[0., -math.pi/2, 0., .4], [0., -math.pi / 2, 0., .4],
[0., math.pi/2, 0., 0.], [0., math.pi / 2, 0., 0.],
[0., -math.pi/2, 0., .321], [0., -math.pi / 2, 0., .321],
[0., math.pi/2, 0., 0.], [0., math.pi / 2, 0., 0.],
[0., 0., 0., 0.]]) [0., 0., 0., 0.]])
# execute IK 10 times # 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 self.pind = pind
def __str__(self): 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): def planning(self, sx, sy, gx, gy):
""" """
@@ -72,7 +73,10 @@ class AStarPlanner:
break break
c_id = min( 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] current = open_set[c_id]
# show graph # show graph
@@ -81,7 +85,8 @@ class AStarPlanner:
self.calc_grid_position(current.y, self.miny), "xc") self.calc_grid_position(current.y, self.miny), "xc")
# for stopping simulation with the esc key. # for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event', 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: if len(closed_set.keys()) % 10 == 0:
plt.pause(0.001) plt.pause(0.001)
@@ -104,7 +109,6 @@ class AStarPlanner:
current.cost + self.motion[i][2], c_id) current.cost + self.motion[i][2], c_id)
n_id = self.calc_grid_index(node) n_id = self.calc_grid_index(node)
# If the node is not safe, do nothing # If the node is not safe, do nothing
if not self.verify_node(node): if not self.verify_node(node):
continue continue

2
PathPlanning/BSplinePath/bspline_path.py
View File

@@ -11,7 +11,7 @@ import matplotlib.pyplot as plt
import scipy.interpolate as scipy_interpolate import scipy.interpolate as scipy_interpolate
def approximate_b_spline_path(x: int, y: list, n_path_points: int, def approximate_b_spline_path(x: list, y: list, n_path_points: int,
degree: int = 3) -> tuple: degree: int = 3) -> tuple:
""" """
approximate points with a B-Spline path approximate points with a B-Spline path

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: 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 matplotlib.pyplot as plt
import copy import copy
import math import math
import cubic_spline_planner
import sys import sys
import os import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../QuinticPolynomialsPlanner/") "/../QuinticPolynomialsPlanner/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../CubicSpline/")
try: try:
from quintic_polynomials_planner import QuinticPolynomial from quintic_polynomials_planner import QuinticPolynomial
import cubic_spline_planner
except ImportError: except ImportError:
raise raise
SIM_LOOP = 500 SIM_LOOP = 500
# Parameter # Parameter
@@ -38,36 +41,35 @@ MAX_CURVATURE = 1.0 # maximum curvature [1/m]
MAX_ROAD_WIDTH = 7.0 # maximum road width [m] MAX_ROAD_WIDTH = 7.0 # maximum road width [m]
D_ROAD_W = 1.0 # road width sampling length [m] D_ROAD_W = 1.0 # road width sampling length [m]
DT = 0.2 # time tick [s] DT = 0.2 # time tick [s]
MAXT = 5.0 # max prediction time [m] MAX_T = 5.0 # max prediction time [m]
MINT = 4.0 # min prediction time [m] MIN_T = 4.0 # min prediction time [m]
TARGET_SPEED = 30.0 / 3.6 # target speed [m/s] TARGET_SPEED = 30.0 / 3.6 # target speed [m/s]
D_T_S = 5.0 / 3.6 # target speed sampling length [m/s] D_T_S = 5.0 / 3.6 # target speed sampling length [m/s]
N_S_SAMPLE = 1 # sampling number of target speed N_S_SAMPLE = 1 # sampling number of target speed
ROBOT_RADIUS = 2.0 # robot radius [m] ROBOT_RADIUS = 2.0 # robot radius [m]
# cost weights # cost weights
KJ = 0.1 K_J = 0.1
KT = 0.1 K_T = 0.1
KD = 1.0 K_D = 1.0
KLAT = 1.0 K_LAT = 1.0
KLON = 1.0 K_LON = 1.0
show_animation = True show_animation = True
class quartic_polynomial: class QuarticPolynomial:
def __init__(self, xs, vxs, axs, vxe, axe, T):
def __init__(self, xs, vxs, axs, vxe, axe, time):
# calc coefficient of quartic polynomial # calc coefficient of quartic polynomial
self.a0 = xs self.a0 = xs
self.a1 = vxs self.a1 = vxs
self.a2 = axs / 2.0 self.a2 = axs / 2.0
A = np.array([[3 * T ** 2, 4 * T ** 3], A = np.array([[3 * time ** 2, 4 * time ** 3],
[6 * T, 12 * T ** 2]]) [6 * time, 12 * time ** 2]])
b = np.array([vxe - self.a1 - 2 * self.a2 * T, b = np.array([vxe - self.a1 - 2 * self.a2 * time,
axe - 2 * self.a2]) axe - 2 * self.a2])
x = np.linalg.solve(A, b) x = np.linalg.solve(A, b)
@@ -75,19 +77,19 @@ class quartic_polynomial:
self.a4 = x[1] self.a4 = x[1]
def calc_point(self, t): def calc_point(self, t):
xt = self.a0 + self.a1 * t + self.a2 * t**2 + \ xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
self.a3 * t**3 + self.a4 * t**4 self.a3 * t ** 3 + self.a4 * t ** 4
return xt return xt
def calc_first_derivative(self, t): def calc_first_derivative(self, t):
xt = self.a1 + 2 * self.a2 * 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 return xt
def calc_second_derivative(self, t): 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 return xt
@@ -97,7 +99,7 @@ class quartic_polynomial:
return xt return xt
class Frenet_path: class FrenetPath:
def __init__(self): def __init__(self):
self.t = [] self.t = []
@@ -121,15 +123,14 @@ class Frenet_path:
def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0): def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
frenet_paths = [] frenet_paths = []
# generate path to each offset goal # generate path to each offset goal
for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W): for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
# Lateral motion planning # Lateral motion planning
for Ti in np.arange(MINT, MAXT, DT): for Ti in np.arange(MIN_T, MAX_T, DT):
fp = Frenet_path() fp = FrenetPath()
# lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti) # 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) 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] fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
# Longitudinal motion planning (Velocity keeping) # 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) 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 = [lon_qp.calc_point(t) for t in fp.t]
tfp.s_d = [lon_qp.calc_first_derivative(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 Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk
# square of diff from target speed # 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.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1] ** 2
tfp.cv = KJ * Js + KT * Ti + KD * ds tfp.cv = K_J * Js + K_T * Ti + K_D * ds
tfp.cf = KLAT * tfp.cd + KLON * tfp.cv tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv
frenet_paths.append(tfp) 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): def calc_global_paths(fplist, csp):
for fp in fplist: for fp in fplist:
# calc global positions # calc global positions
@@ -174,10 +175,10 @@ def calc_global_paths(fplist, csp):
ix, iy = csp.calc_position(fp.s[i]) ix, iy = csp.calc_position(fp.s[i])
if ix is None: if ix is None:
break break
iyaw = csp.calc_yaw(fp.s[i]) i_yaw = csp.calc_yaw(fp.s[i])
di = fp.d[i] di = fp.d[i]
fx = ix + di * math.cos(iyaw + math.pi / 2.0) fx = ix + di * math.cos(i_yaw + math.pi / 2.0)
fy = iy + di * math.sin(iyaw + math.pi / 2.0) fy = iy + di * math.sin(i_yaw + math.pi / 2.0)
fp.x.append(fx) fp.x.append(fx)
fp.y.append(fy) fp.y.append(fy)
@@ -199,12 +200,11 @@ def calc_global_paths(fplist, csp):
def check_collision(fp, ob): def check_collision(fp, ob):
for i in range(len(ob[:, 0])): 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)] 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: if collision:
return False return False
@@ -213,38 +213,38 @@ def check_collision(fp, ob):
def check_paths(fplist, ob): def check_paths(fplist, ob):
ok_ind = []
okind = []
for i, _ in enumerate(fplist): for i, _ in enumerate(fplist):
if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check
continue 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 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 continue
elif not check_collision(fplist[i], ob): elif not check_collision(fplist[i], ob):
continue 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): 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_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0)
fplist = calc_global_paths(fplist, csp) fplist = calc_global_paths(fplist, csp)
fplist = check_paths(fplist, ob) fplist = check_paths(fplist, ob)
# find minimum cost path # find minimum cost path
mincost = float("inf") min_cost = float("inf")
bestpath = None best_path = None
for fp in fplist: for fp in fplist:
if mincost >= fp.cf: if min_cost >= fp.cf:
mincost = fp.cf min_cost = fp.cf
bestpath = fp best_path = fp
return bestpath return best_path
def generate_target_course(x, y): def generate_target_course(x, y):
@@ -282,7 +282,7 @@ def main():
c_speed = 10.0 / 3.6 # current speed [m/s] c_speed = 10.0 / 3.6 # current speed [m/s]
c_d = 2.0 # current lateral position [m] c_d = 2.0 # current lateral position [m]
c_d_d = 0.0 # current lateral speed [m/s] 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 s0 = 0.0 # current course position
area = 20.0 # animation area length [m] area = 20.0 # animation area length [m]
@@ -304,8 +304,9 @@ def main():
if show_animation: # pragma: no cover if show_animation: # pragma: no cover
plt.cla() plt.cla()
# for stopping simulation with the esc key. # for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event', plt.gcf().canvas.mpl_connect(
lambda event: [exit(0) if event.key == 'escape' else None]) 'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(tx, ty) plt.plot(tx, ty)
plt.plot(ob[:, 0], ob[:, 1], "xk") plt.plot(ob[:, 0], ob[:, 1], "xk")
plt.plot(path.x[1:], path.y[1:], "-or") 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 import sys
sys.path.append("../ReedsSheppPath/") sys.path.append("../ReedsSheppPath/")
try: 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 import reeds_shepp_path_planning as rs
from car import move, check_car_collision, MAX_STEER, WB, plot_car from car import move, check_car_collision, MAX_STEER, WB, plot_car
except: except: