def test_angle_mod():
assert_allclose(angle.angle_mod(-4.0), 2.28318531)
assert (isinstance(angle.angle_mod(-4.0), float))
assert_allclose(angle.angle_mod([-4.0]), [2.28318531])
assert (isinstance(angle.angle_mod([-4.0]), np.ndarray))
assert_allclose(angle.angle_mod([-150.0, 190.0, 350], degree=True),
[-150., -170., -10.])
assert_allclose(angle.angle_mod(-60.0, zero_2_2pi=True, degree=True),
[300.])
def right_straight_right(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
tmp0 = d - sa + sb
mode = ["R", "S", "R"]
p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa))
if p_squared < 0:
return None, None, None, mode
tmp1 = math.atan2((ca - cb), tmp0)
t = angle_mod(alpha - tmp1, zero_2_2pi=True)
p = math.sqrt(p_squared)
q = _mod2pi(-beta + tmp1)
return t, p, q, mode
def _mod2pi(theta):
return angle_mod(theta, zero_2_2pi=True)
def plan_dubins_path(s_x,
s_y,
s_yaw,
g_x,
g_y,
g_yaw,
curvature,
step_size=0.1):
"""
Path 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
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
length list of the path segments.
"""
# calculate local goal x, y, yaw
l_rot = create_2d_rotation_matrix(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)
# Convert a local coordinate path to the global coordinate
rot = create_2d_rotation_matrix(-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 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