Source code for PathPlanning.DubinsPath.dubins_path_planner

"""

Dubins path planner sample code

author Atsushi Sakai(@Atsushi_twi)

"""
import sys
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))

from math import sin, cos, atan2, sqrt, acos, pi, hypot
import numpy as np
from utils.angle import angle_mod, rot_mat_2d

show_animation = True


[docs]def plan_dubins_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, curvature, step_size=0.1, selected_types=None): """ Plan dubins path Parameters ---------- s_x : float x position of the start point [m] s_y : float y position of the start point [m] s_yaw : float yaw angle of the start point [rad] g_x : float x position of the goal point [m] g_y : float y position of the end point [m] g_yaw : float yaw angle of the end point [rad] curvature : float curvature for curve [1/m] step_size : float (optional) step size between two path points [m]. Default is 0.1 selected_types : a list of string or None selected path planning types. If None, all types are used for path planning, and minimum path length result is returned. You can select used path plannings types by a string list. e.g.: ["RSL", "RSR"] Returns ------- x_list: array x positions of the path y_list: array y positions of the path yaw_list: array yaw angles of the path modes: array mode list of the path lengths: array arrow_length list of the path segments. Examples -------- You can generate a dubins path. >>> start_x = 1.0 # [m] >>> start_y = 1.0 # [m] >>> start_yaw = np.deg2rad(45.0) # [rad] >>> end_x = -3.0 # [m] >>> end_y = -3.0 # [m] >>> end_yaw = np.deg2rad(-45.0) # [rad] >>> curvature = 1.0 >>> path_x, path_y, path_yaw, mode, _ = plan_dubins_path( start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) >>> plt.plot(path_x, path_y, label="final course " + "".join(mode)) >>> plot_arrow(start_x, start_y, start_yaw) >>> plot_arrow(end_x, end_y, end_yaw) >>> plt.legend() >>> plt.grid(True) >>> plt.axis("equal") >>> plt.show() .. image:: dubins_path.jpg """ if selected_types is None: planning_funcs = _PATH_TYPE_MAP.values() else: planning_funcs = [_PATH_TYPE_MAP[ptype] for ptype in selected_types] # calculate local goal x, y, yaw l_rot = rot_mat_2d(s_yaw) le_xy = np.stack([g_x - s_x, g_y - s_y]).T @ l_rot local_goal_x = le_xy[0] local_goal_y = le_xy[1] local_goal_yaw = g_yaw - s_yaw lp_x, lp_y, lp_yaw, modes, lengths = _dubins_path_planning_from_origin( local_goal_x, local_goal_y, local_goal_yaw, curvature, step_size, planning_funcs) # Convert a local coordinate path to the global coordinate rot = rot_mat_2d(-s_yaw) 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 = angle_mod(np.array(lp_yaw) + s_yaw) return x_list, y_list, yaw_list, modes, lengths
def _mod2pi(theta): return angle_mod(theta, zero_2_2pi=True) def _calc_trig_funcs(alpha, beta): sin_a = sin(alpha) sin_b = sin(beta) cos_a = cos(alpha) cos_b = cos(beta) cos_ab = cos(alpha - beta) return sin_a, sin_b, cos_a, cos_b, cos_ab def _LSL(alpha, beta, d): sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta) mode = ["L", "S", "L"] p_squared = 2 + d ** 2 - (2 * cos_ab) + (2 * d * (sin_a - sin_b)) if p_squared < 0: # invalid configuration return None, None, None, mode tmp = atan2((cos_b - cos_a), d + sin_a - sin_b) d1 = _mod2pi(-alpha + tmp) d2 = sqrt(p_squared) d3 = _mod2pi(beta - tmp) return d1, d2, d3, mode def _RSR(alpha, beta, d): sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta) mode = ["R", "S", "R"] p_squared = 2 + d ** 2 - (2 * cos_ab) + (2 * d * (sin_b - sin_a)) if p_squared < 0: return None, None, None, mode tmp = atan2((cos_a - cos_b), d - sin_a + sin_b) d1 = _mod2pi(alpha - tmp) d2 = sqrt(p_squared) d3 = _mod2pi(-beta + tmp) return d1, d2, d3, mode def _LSR(alpha, beta, d): sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta) p_squared = -2 + d ** 2 + (2 * cos_ab) + (2 * d * (sin_a + sin_b)) mode = ["L", "S", "R"] if p_squared < 0: return None, None, None, mode d1 = sqrt(p_squared) tmp = atan2((-cos_a - cos_b), (d + sin_a + sin_b)) - atan2(-2.0, d1) d2 = _mod2pi(-alpha + tmp) d3 = _mod2pi(-_mod2pi(beta) + tmp) return d2, d1, d3, mode def _RSL(alpha, beta, d): sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta) p_squared = d ** 2 - 2 + (2 * cos_ab) - (2 * d * (sin_a + sin_b)) mode = ["R", "S", "L"] if p_squared < 0: return None, None, None, mode d1 = sqrt(p_squared) tmp = atan2((cos_a + cos_b), (d - sin_a - sin_b)) - atan2(2.0, d1) d2 = _mod2pi(alpha - tmp) d3 = _mod2pi(beta - tmp) return d2, d1, d3, mode def _RLR(alpha, beta, d): sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta) mode = ["R", "L", "R"] tmp = (6.0 - d ** 2 + 2.0 * cos_ab + 2.0 * d * (sin_a - sin_b)) / 8.0 if abs(tmp) > 1.0: return None, None, None, mode d2 = _mod2pi(2 * pi - acos(tmp)) d1 = _mod2pi(alpha - atan2(cos_a - cos_b, d - sin_a + sin_b) + d2 / 2.0) d3 = _mod2pi(alpha - beta - d1 + d2) return d1, d2, d3, mode def _LRL(alpha, beta, d): sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta) mode = ["L", "R", "L"] tmp = (6.0 - d ** 2 + 2.0 * cos_ab + 2.0 * d * (- sin_a + sin_b)) / 8.0 if abs(tmp) > 1.0: return None, None, None, mode d2 = _mod2pi(2 * pi - acos(tmp)) d1 = _mod2pi(-alpha - atan2(cos_a - cos_b, d + sin_a - sin_b) + d2 / 2.0) d3 = _mod2pi(_mod2pi(beta) - alpha - d1 + _mod2pi(d2)) return d1, d2, d3, mode _PATH_TYPE_MAP = {"LSL": _LSL, "RSR": _RSR, "LSR": _LSR, "RSL": _RSL, "RLR": _RLR, "LRL": _LRL, } def _dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size, planning_funcs): dx = end_x dy = end_y d = hypot(dx, dy) * curvature theta = _mod2pi(atan2(dy, dx)) alpha = _mod2pi(-theta) beta = _mod2pi(end_yaw - theta) best_cost = float("inf") b_d1, b_d2, b_d3, b_mode = None, None, None, None for planner in planning_funcs: d1, d2, d3, mode = planner(alpha, beta, d) if d1 is None: continue cost = (abs(d1) + abs(d2) + abs(d3)) if best_cost > cost: # Select minimum length one. b_d1, b_d2, b_d3, b_mode, best_cost = d1, d2, d3, mode, cost lengths = [b_d1, b_d2, b_d3] x_list, y_list, yaw_list = _generate_local_course(lengths, b_mode, curvature, step_size) lengths = [length / curvature for length in lengths] return x_list, y_list, yaw_list, b_mode, lengths def _interpolate(length, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw): if mode == "S": path_x.append(origin_x + length / max_curvature * cos(origin_yaw)) path_y.append(origin_y + length / max_curvature * sin(origin_yaw)) path_yaw.append(origin_yaw) else: # curve ldx = sin(length) / max_curvature ldy = 0.0 if mode == "L": # left turn ldy = (1.0 - cos(length)) / max_curvature elif mode == "R": # right turn ldy = (1.0 - cos(length)) / -max_curvature gdx = cos(-origin_yaw) * ldx + sin(-origin_yaw) * ldy gdy = -sin(-origin_yaw) * ldx + cos(-origin_yaw) * ldy path_x.append(origin_x + gdx) path_y.append(origin_y + gdy) if mode == "L": # left turn path_yaw.append(origin_yaw + length) elif mode == "R": # right turn path_yaw.append(origin_yaw - length) return path_x, path_y, path_yaw def _generate_local_course(lengths, modes, max_curvature, step_size): p_x, p_y, p_yaw = [0.0], [0.0], [0.0] for (mode, length) in zip(modes, lengths): if length == 0.0: continue # set origin state origin_x, origin_y, origin_yaw = p_x[-1], p_y[-1], p_yaw[-1] current_length = step_size while abs(current_length + step_size) <= abs(length): p_x, p_y, p_yaw = _interpolate(current_length, mode, max_curvature, origin_x, origin_y, origin_yaw, p_x, p_y, p_yaw) current_length += step_size p_x, p_y, p_yaw = _interpolate(length, mode, max_curvature, origin_x, origin_y, origin_yaw, p_x, p_y, p_yaw) return p_x, p_y, p_yaw def main(): print("Dubins path planner sample start!!") import matplotlib.pyplot as plt from utils.plot import plot_arrow start_x = 1.0 # [m] start_y = 1.0 # [m] start_yaw = np.deg2rad(45.0) # [rad] end_x = -3.0 # [m] end_y = -3.0 # [m] end_yaw = np.deg2rad(-45.0) # [rad] curvature = 1.0 path_x, path_y, path_yaw, mode, lengths = plan_dubins_path(start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) if show_animation: plt.plot(path_x, path_y, label="".join(mode)) plot_arrow(start_x, start_y, start_yaw) plot_arrow(end_x, end_y, end_yaw) plt.legend() plt.grid(True) plt.axis("equal") plt.show() if __name__ == '__main__': main()