# PathPlanning.DubinsPath.dubins_path_planner — PythonRobotics documentation

# 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

    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()