mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
fix dubins path length bug and clean up codes. (#527)
* fix dubins path length bug and clean up codes. * fix line length CI error * fix line length CI error * fix line length CI error * fix line length CI error * fix line length CI error * fix line length CI error * fix line length CI error * fix line length CI error
This commit is contained in:
Atsushi Sakai
@@ -14,6 +14,46 @@ from scipy.spatial.transform import Rotation as Rot
|
||||
show_animation = True
|
||||
|
||||
|
||||
def dubins_path_planning(s_x, s_y, s_yaw, g_x, g_y, g_yaw, curvature,
|
||||
step_size=0.1):
|
||||
"""
|
||||
Dubins path planner
|
||||
|
||||
:param s_x: x position of start point [m]
|
||||
:param s_y: y position of start point [m]
|
||||
:param s_yaw: yaw angle of start point [rad]
|
||||
:param g_x: x position of end point [m]
|
||||
:param g_y: y position of end point [m]
|
||||
:param g_yaw: yaw angle of end point [rad]
|
||||
:param curvature: curvature for curve [1/m]
|
||||
:param step_size: (optional) step size between two path points [m]
|
||||
:return:
|
||||
x_list: x positions of a path
|
||||
y_list: y positions of a path
|
||||
yaw_list: yaw angles of a path
|
||||
modes: mode list of a path
|
||||
lengths: length of path segments.
|
||||
"""
|
||||
|
||||
g_x -= s_x
|
||||
g_y -= s_y
|
||||
|
||||
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
|
||||
|
||||
lp_x, lp_y, lp_yaw, modes, lengths = dubins_path_planning_from_origin(
|
||||
le_xy[0], le_xy[1], le_yaw, curvature, step_size)
|
||||
|
||||
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 x_list, y_list, yaw_list, modes, lengths
|
||||
|
||||
|
||||
def mod2pi(theta):
|
||||
return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
|
||||
|
||||
@@ -148,14 +188,14 @@ def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature,
|
||||
alpha = mod2pi(- theta)
|
||||
beta = mod2pi(end_yaw - theta)
|
||||
|
||||
planners = [left_straight_left, right_straight_right, left_straight_right,
|
||||
right_straight_left, right_left_right,
|
||||
left_right_left]
|
||||
planning_funcs = [left_straight_left, right_straight_right,
|
||||
left_straight_right, right_straight_left,
|
||||
right_left_right, left_right_left]
|
||||
|
||||
best_cost = float("inf")
|
||||
bt, bp, bq, best_mode = None, None, None, None
|
||||
|
||||
for planner in planners:
|
||||
for planner in planning_funcs:
|
||||
t, p, q, mode = planner(alpha, beta, d)
|
||||
if t is None:
|
||||
continue
|
||||
@@ -166,10 +206,15 @@ def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature,
|
||||
best_cost = cost
|
||||
lengths = [bt, bp, bq]
|
||||
|
||||
x_list, y_list, yaw_list, directions = generate_local_course(
|
||||
sum(lengths), lengths, best_mode, curvature, step_size)
|
||||
x_list, y_list, yaw_list, directions = generate_local_course(sum(lengths),
|
||||
lengths,
|
||||
best_mode,
|
||||
curvature,
|
||||
step_size)
|
||||
|
||||
return x_list, y_list, yaw_list, best_mode, best_cost
|
||||
lengths = [length / curvature for length in lengths]
|
||||
|
||||
return x_list, y_list, yaw_list, best_mode, lengths
|
||||
|
||||
|
||||
def interpolate(ind, length, mode, max_curvature, origin_x, origin_y,
|
||||
@@ -203,49 +248,15 @@ def interpolate(ind, length, mode, max_curvature, origin_x, origin_y,
|
||||
return path_x, path_y, path_yaw, directions
|
||||
|
||||
|
||||
def dubins_path_planning(s_x, s_y, s_yaw, g_x, g_y, g_yaw, c, step_size=0.1):
|
||||
"""
|
||||
Dubins path planner
|
||||
|
||||
input:
|
||||
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]
|
||||
|
||||
"""
|
||||
|
||||
g_x = g_x - s_x
|
||||
g_y = g_y - s_y
|
||||
|
||||
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
|
||||
|
||||
lp_x, lp_y, lp_yaw, mode, lengths = dubins_path_planning_from_origin(
|
||||
le_xy[0], le_xy[1], le_yaw, c, step_size)
|
||||
|
||||
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 x_list, y_list, yaw_list, mode, lengths
|
||||
|
||||
|
||||
def generate_local_course(total_length, lengths, mode, max_curvature,
|
||||
def generate_local_course(total_length, lengths, modes, 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)]
|
||||
p_x = [0.0 for _ in range(n_point)]
|
||||
p_y = [0.0 for _ in range(n_point)]
|
||||
p_yaw = [0.0 for _ in range(n_point)]
|
||||
directions = [0.0 for _ in range(n_point)]
|
||||
index = 1
|
||||
ind = 1
|
||||
|
||||
if lengths[0] > 0.0:
|
||||
directions[0] = 1
|
||||
@@ -254,7 +265,7 @@ def generate_local_course(total_length, lengths, mode, max_curvature,
|
||||
|
||||
ll = 0.0
|
||||
|
||||
for (m, length, i) in zip(mode, lengths, range(len(mode))):
|
||||
for (m, length, i) in zip(modes, lengths, range(len(modes))):
|
||||
if length == 0.0:
|
||||
continue
|
||||
elif length > 0.0:
|
||||
@@ -263,54 +274,57 @@ def generate_local_course(total_length, lengths, mode, max_curvature,
|
||||
dist = -step_size
|
||||
|
||||
# set origin state
|
||||
origin_x, origin_y, origin_yaw = \
|
||||
path_x[index], path_y[index], path_yaw[index]
|
||||
origin_x, origin_y, origin_yaw = p_x[ind], p_y[ind], p_yaw[ind]
|
||||
|
||||
index -= 1
|
||||
ind -= 1
|
||||
if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
|
||||
pd = - dist - ll
|
||||
else:
|
||||
pd = dist - ll
|
||||
|
||||
while abs(pd) <= abs(length):
|
||||
index += 1
|
||||
path_x, path_y, path_yaw, directions = interpolate(
|
||||
index, pd, m, max_curvature, origin_x, origin_y, origin_yaw,
|
||||
path_x, path_y, path_yaw, directions)
|
||||
ind += 1
|
||||
p_x, p_y, p_yaw, directions = interpolate(ind, pd, m,
|
||||
max_curvature,
|
||||
origin_x,
|
||||
origin_y,
|
||||
origin_yaw,
|
||||
p_x, p_y,
|
||||
p_yaw,
|
||||
directions)
|
||||
pd += dist
|
||||
|
||||
ll = length - pd - dist # calc remain length
|
||||
|
||||
index += 1
|
||||
path_x, path_y, path_yaw, directions = interpolate(
|
||||
index, length, m, max_curvature, origin_x, origin_y, origin_yaw,
|
||||
path_x, path_y, path_yaw, directions)
|
||||
ind += 1
|
||||
p_x, p_y, p_yaw, directions = interpolate(ind, length, m,
|
||||
max_curvature,
|
||||
origin_x, origin_y,
|
||||
origin_yaw,
|
||||
p_x, p_y, p_yaw,
|
||||
directions)
|
||||
|
||||
if len(path_x) <= 1:
|
||||
if len(p_x) <= 1:
|
||||
return [], [], [], []
|
||||
|
||||
# remove unused data
|
||||
while len(path_x) >= 1 and path_x[-1] == 0.0:
|
||||
path_x.pop()
|
||||
path_y.pop()
|
||||
path_yaw.pop()
|
||||
while len(p_x) >= 1 and p_x[-1] == 0.0:
|
||||
p_x.pop()
|
||||
p_y.pop()
|
||||
p_yaw.pop()
|
||||
directions.pop()
|
||||
|
||||
return path_x, path_y, path_yaw, directions
|
||||
return p_x, p_y, p_yaw, directions
|
||||
|
||||
|
||||
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 (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)
|
||||
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), fc=fc,
|
||||
ec=ec, head_width=width, head_length=width)
|
||||
plt.plot(x, y)
|
||||
|
||||
|
||||
@@ -327,9 +341,13 @@ def main():
|
||||
|
||||
curvature = 1.0
|
||||
|
||||
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)
|
||||
path_x, path_y, path_yaw, mode, lengths = dubins_path_planning(start_x,
|
||||
start_y,
|
||||
start_yaw,
|
||||
end_x,
|
||||
end_y,
|
||||
end_yaw,
|
||||
curvature)
|
||||
|
||||
if show_animation:
|
||||
plt.plot(path_x, path_y, label="final course " + "".join(mode))
|
||||
|
||||
@@ -133,9 +133,10 @@ class RRTDubins(RRT):
|
||||
|
||||
def steer(self, from_node, to_node):
|
||||
|
||||
px, py, pyaw, mode, course_length = dubins_path_planning.dubins_path_planning(
|
||||
from_node.x, from_node.y, from_node.yaw,
|
||||
to_node.x, to_node.y, to_node.yaw, self.curvature)
|
||||
px, py, pyaw, mode, course_lengths = \
|
||||
dubins_path_planning.dubins_path_planning(
|
||||
from_node.x, from_node.y, from_node.yaw,
|
||||
to_node.x, to_node.y, to_node.yaw, self.curvature)
|
||||
|
||||
if len(px) <= 1: # cannot find a dubins path
|
||||
return None
|
||||
@@ -148,7 +149,7 @@ class RRTDubins(RRT):
|
||||
new_node.path_x = px
|
||||
new_node.path_y = py
|
||||
new_node.path_yaw = pyaw
|
||||
new_node.cost += course_length
|
||||
new_node.cost += sum([abs(c) for c in course_lengths])
|
||||
new_node.parent = from_node
|
||||
|
||||
return new_node
|
||||
|
||||
@@ -139,9 +139,10 @@ class RRTStarDubins(RRTStar):
|
||||
|
||||
def steer(self, from_node, to_node):
|
||||
|
||||
px, py, pyaw, mode, course_length = dubins_path_planning.dubins_path_planning(
|
||||
from_node.x, from_node.y, from_node.yaw,
|
||||
to_node.x, to_node.y, to_node.yaw, self.curvature)
|
||||
px, py, pyaw, mode, course_lengths = \
|
||||
dubins_path_planning.dubins_path_planning(
|
||||
from_node.x, from_node.y, from_node.yaw,
|
||||
to_node.x, to_node.y, to_node.yaw, self.curvature)
|
||||
|
||||
if len(px) <= 1: # cannot find a dubins path
|
||||
return None
|
||||
@@ -154,18 +155,20 @@ class RRTStarDubins(RRTStar):
|
||||
new_node.path_x = px
|
||||
new_node.path_y = py
|
||||
new_node.path_yaw = pyaw
|
||||
new_node.cost += course_length
|
||||
new_node.cost += sum([abs(c) for c in course_lengths])
|
||||
new_node.parent = from_node
|
||||
|
||||
return new_node
|
||||
|
||||
def calc_new_cost(self, from_node, to_node):
|
||||
|
||||
_, _, _, _, course_length = dubins_path_planning.dubins_path_planning(
|
||||
_, _, _, _, course_lengths = dubins_path_planning.dubins_path_planning(
|
||||
from_node.x, from_node.y, from_node.yaw,
|
||||
to_node.x, to_node.y, to_node.yaw, self.curvature)
|
||||
|
||||
return from_node.cost + course_length
|
||||
cost = sum([abs(c) for c in course_lengths])
|
||||
|
||||
return from_node.cost + cost
|
||||
|
||||
def get_random_node(self):
|
||||
|
||||
|
||||
@@ -1,12 +1,13 @@
|
||||
import conftest
|
||||
import numpy as np
|
||||
|
||||
import conftest
|
||||
from PathPlanning.DubinsPath import dubins_path_planning
|
||||
|
||||
np.random.seed(12345)
|
||||
|
||||
|
||||
def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw,
|
||||
end_x, end_y, end_yaw):
|
||||
def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x,
|
||||
end_y, end_yaw):
|
||||
assert (abs(px[0] - start_x) <= 0.01)
|
||||
assert (abs(py[0] - start_y) <= 0.01)
|
||||
assert (abs(pyaw[0] - start_yaw) <= 0.01)
|
||||
@@ -15,6 +16,12 @@ def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw,
|
||||
assert (abs(pyaw[-1] - end_yaw) <= 0.01)
|
||||
|
||||
|
||||
def check_path_length(px, py, lengths):
|
||||
path_len = sum(
|
||||
[np.hypot(dx, dy) for (dx, dy) in zip(np.diff(px), np.diff(py))])
|
||||
assert (abs(path_len - sum(lengths)) <= 0.1)
|
||||
|
||||
|
||||
def test_1():
|
||||
start_x = 1.0 # [m]
|
||||
start_y = 1.0 # [m]
|
||||
@@ -26,12 +33,12 @@ def test_1():
|
||||
|
||||
curvature = 1.0
|
||||
|
||||
px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
|
||||
px, py, pyaw, mode, lengths = dubins_path_planning.dubins_path_planning(
|
||||
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
|
||||
|
||||
check_edge_condition(px, py, pyaw,
|
||||
start_x, start_y, start_yaw,
|
||||
end_x, end_y, end_yaw)
|
||||
check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x,
|
||||
end_y, end_yaw)
|
||||
check_path_length(px, py, lengths)
|
||||
|
||||
|
||||
def test_2():
|
||||
@@ -53,12 +60,13 @@ def test_3():
|
||||
|
||||
curvature = 1.0 / (np.random.rand() * 5.0)
|
||||
|
||||
px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
|
||||
px, py, pyaw, mode, lengths = \
|
||||
dubins_path_planning.dubins_path_planning(
|
||||
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
|
||||
|
||||
check_edge_condition(px, py, pyaw,
|
||||
start_x, start_y, start_yaw,
|
||||
end_x, end_y, end_yaw)
|
||||
check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x,
|
||||
end_y, end_yaw)
|
||||
check_path_length(px, py, lengths)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
Reference in New Issue
Block a user