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