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

Using scipy.spatial.rotation matrix (#335)

Browse Source
This commit is contained in:
Atsushi Sakai
2020-06-07 20:28:29 +09:00
committed by GitHub
parent d1bde5835f
commit b8afdb10d8
26 changed files with 1002 additions and 931 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -9,6 +9,7 @@ import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
show_animation = True
@@ -136,7 +137,8 @@ def left_right_left(alpha, beta, d):
return t, p, q, mode
def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size):
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)
@@ -146,7 +148,8 @@ def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size
alpha = mod2pi(- theta)
beta = mod2pi(end_yaw - theta)
planners = [left_straight_left, right_straight_right, left_straight_right, right_straight_left, right_left_right,
planners = [left_straight_left, right_straight_right, left_straight_right,
right_straight_left, right_left_right,
left_right_left]
best_cost = float("inf")
@@ -163,13 +166,14 @@ def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size
best_cost = cost
lengths = [bt, bp, bq]
px, py, pyaw, directions = generate_local_course(
x_list, y_list, yaw_list, directions = generate_local_course(
sum(lengths), lengths, best_mode, curvature, step_size)
return px, py, pyaw, best_mode, best_cost
return x_list, y_list, yaw_list, best_mode, best_cost
def interpolate(ind, length, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions):
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)
@@ -199,54 +203,49 @@ def interpolate(ind, length, mode, max_curvature, origin_x, origin_y, origin_yaw
return path_x, path_y, path_yaw, directions
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, step_size=0.1):
def dubins_path_planning(s_x, s_y, s_yaw, g_x, g_y, g_yaw, c, step_size=0.1):
"""
Dubins path plannner
Dubins path planner
input:
sx x position of start point [m]
sy y position of start point [m]
syaw yaw angle of start point [rad]
ex x position of end point [m]
ey y position of end point [m]
eyaw yaw angle of end point [rad]
s_x x position of start point [m]
s_y y position of start point [m]
s_yaw yaw angle of start point [rad]
g_x x position of end point [m]
g_y y position of end point [m]
g_yaw yaw angle of end point [rad]
c curvature [1/m]
output:
px
py
pyaw
mode
"""
ex = ex - sx
ey = ey - sy
g_x = g_x - s_x
g_y = g_y - s_y
lex = math.cos(syaw) * ex + math.sin(syaw) * ey
ley = - math.sin(syaw) * ex + math.cos(syaw) * ey
leyaw = eyaw - syaw
l_rot = Rot.from_euler('z', s_yaw).as_matrix()[0:2, 0:2]
le_xy = np.stack([g_x, g_y]).T @ l_rot
le_yaw = g_yaw - s_yaw
lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
lex, ley, leyaw, c, step_size)
lp_x, lp_y, lp_yaw, mode, lengths = dubins_path_planning_from_origin(
le_xy[0], le_xy[1], le_yaw, c, step_size)
px = [math.cos(-syaw) * x + math.sin(-syaw)
* y + sx for x, y in zip(lpx, lpy)]
py = [- math.sin(-syaw) * x + math.cos(-syaw)
* y + sy for x, y in zip(lpx, lpy)]
pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw]
rot = Rot.from_euler('z', -s_yaw).as_matrix()[0:2, 0:2]
converted_xy = np.stack([lp_x, lp_y]).T @ rot
x_list = converted_xy[:, 0] + s_x
y_list = converted_xy[:, 1] + s_y
yaw_list = [pi_2_pi(i_yaw + s_yaw) for i_yaw in lp_yaw]
return px, py, pyaw, mode, clen
return x_list, y_list, yaw_list, mode, lengths
def generate_local_course(total_length, lengths, mode, max_curvature, step_size):
def generate_local_course(total_length, lengths, mode, max_curvature,
step_size):
n_point = math.trunc(total_length / step_size) + len(lengths) + 4
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
index = 1
if lengths[0] > 0.0:
directions[0] = 1
@@ -262,25 +261,28 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size)
d = -step_size
# set origin state
origin_x, origin_y, origin_yaw = path_x[ind], path_y[ind], path_yaw[ind]
origin_x, origin_y, origin_yaw = \
path_x[index], path_y[index], path_yaw[index]
ind -= 1
index -= 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
index += 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)
index, pd, m, max_curvature, origin_x, origin_y, origin_yaw,
path_x, path_y, path_yaw, directions)
pd += d
ll = l - pd - d # calc remain length
ind += 1
index += 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)
index, l, m, max_curvature, origin_x, origin_y, origin_yaw,
path_x, path_y, path_yaw, directions)
if len(path_x) <= 1:
return [], [], [], []
@@ -295,14 +297,15 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size)
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
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r",
ec="k"): # pragma: no cover
"""
Plot arrow
"""
if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw):
plot_arrow(ix, iy, iyaw)
for (i_x, i_y, i_yaw) in zip(x, y, yaw):
plot_arrow(i_x, i_y, i_yaw)
else:
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
fc=fc, ec=ec, head_width=width, head_length=width)
@@ -322,11 +325,12 @@ def main():
curvature = 1.0
px, py, pyaw, mode, clen = dubins_path_planning(start_x, start_y, start_yaw,
end_x, end_y, end_yaw, curvature)
path_x, path_y, path_yaw, mode, path_length = dubins_path_planning(
start_x, start_y, start_yaw,
end_x, end_y, end_yaw, curvature)
if show_animation:
plt.plot(px, py, label="final course " + "".join(mode))
plt.plot(path_x, path_y, label="final course " + "".join(mode))
# plotting
plot_arrow(start_x, start_y, start_yaw)