modified frenet optimal trajectory generator to consider the left and…

PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory_with_boundary.py

@@ -0,0 +1,375 @@
+"""
+Frenet optimal trajectory generator with boundary
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+Ref:
+
+- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame]
+(https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf)
+
+- [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame]
+(https://www.youtube.com/watch?v=Cj6tAQe7UCY)
+
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+import copy
+import math
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent))
+
+from QuinticPolynomialsPlanner.quintic_polynomials_planner import QuinticPolynomial
+from CubicSpline import cubic_spline_planner
+
+SIM_LOOP = 500
+
+
+# Parameter
+MAX_SPEED = 50.0 / 3.6  # maximum speed [m/s]
+MAX_ACCEL = 2.0  # maximum acceleration [m/ss]
+MAX_CURVATURE = 1.0  # maximum curvature [1/m]
+MAX_ROAD_WIDTH = 7.0  # maximum road width [m]
+D_ROAD_W = 1.0  # road width sampling length [m]
+DT = 0.2  # time tick [s]
+MAX_T = 5.0  # max prediction time [m]
+MIN_T = 4.0  # min prediction time [m]
+TARGET_SPEED = 30.0 / 3.6  # target speed [m/s]
+D_T_S = 5.0 / 3.6  # target speed sampling length [m/s]
+N_S_SAMPLE = 1  # sampling number of target speed
+ROBOT_RADIUS = 2.0  # robot radius [m]
+
+# cost weights
+K_J = 0.1
+K_T = 0.1
+K_D = 1.0
+K_LAT = 1.0
+K_LON = 1.0
+
+show_animation = True
+
+
+class QuarticPolynomial:
+    def __init__(self, xs, vxs, axs, vxe, axe, time):
+        # calc coefficient of quartic polynomial
+        self.a0 = xs
+        self.a1 = vxs
+        self.a2 = axs / 2.0
+
+        A = np.array([[3 * time ** 2, 4 * time ** 3],
+                      [6 * time, 12 * time ** 2]])
+        b = np.array([vxe - self.a1 - 2 * self.a2 * time,
+                      axe - 2 * self.a2])
+        x = np.linalg.solve(A, b)
+
+        self.a3 = x[0]
+        self.a4 = x[1]
+
+    def calc_point(self, t):
+        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
+            self.a3 * t ** 3 + self.a4 * t ** 4
+        return xt
+
+    def calc_first_derivative(self, t):
+        xt = self.a1 + 2 * self.a2 * t + \
+            3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3
+        return xt
+
+    def calc_second_derivative(self, t):
+        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2
+        return xt
+
+    def calc_third_derivative(self, t):
+        xt = 6 * self.a3 + 24 * self.a4 * t
+        return xt
+
+
+class FrenetPath:
+    def __init__(self):
+        self.t = []
+        self.d = []
+        self.d_d = []
+        self.d_dd = []
+        self.d_ddd = []
+        self.s = []
+        self.s_d = []
+        self.s_dd = []
+        self.s_ddd = []
+        self.cd = 0.0
+        self.cv = 0.0
+        self.cf = 0.0
+
+        self.x = []
+        self.y = []
+        self.yaw = []
+        self.ds = []
+        self.c = []
+
+
+def calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0, max_left, max_right):
+    frenet_paths = []
+
+    # generate path to each offset goal
+    for di in np.arange(-max_left, max_right, D_ROAD_W):
+
+        # Lateral motion planning
+        for Ti in np.arange(MIN_T, MAX_T, DT):
+            fp = FrenetPath()
+
+            lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
+
+            fp.t = [t for t in np.arange(0.0, Ti, DT)]
+            fp.d = [lat_qp.calc_point(t) for t in fp.t]
+            fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
+            fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
+            fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
+
+            # Longitudinal motion planning (Velocity keeping)
+            for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
+                                TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
+                tfp = copy.deepcopy(fp)
+                lon_qp = QuarticPolynomial(s0, c_speed, c_accel, tv, 0.0, Ti)
+
+                tfp.s = [lon_qp.calc_point(t) for t in fp.t]
+                tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
+                tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
+                tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
+
+                Jp = sum(np.power(tfp.d_ddd, 2))  # square of jerk
+                Js = sum(np.power(tfp.s_ddd, 2))  # square of jerk
+
+                # square of diff from target speed
+                ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2
+
+                tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1] ** 2
+                tfp.cv = K_J * Js + K_T * Ti + K_D * ds
+                tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv
+
+                # Check if the path is within the boundaries
+                if all(-max_left <= d <= max_right for d in tfp.d):
+                    frenet_paths.append(tfp)
+
+    return frenet_paths
+
+
+def calc_global_paths(fplist, csp):
+    for fp in fplist:
+
+        # calc global positions
+        for i in range(len(fp.s)):
+            ix, iy = csp.calc_position(fp.s[i])
+            if ix is None:
+                break
+            i_yaw = csp.calc_yaw(fp.s[i])
+            di = fp.d[i]
+            fx = ix + di * math.cos(i_yaw + math.pi / 2.0)
+            fy = iy + di * math.sin(i_yaw + math.pi / 2.0)
+            fp.x.append(fx)
+            fp.y.append(fy)
+
+        # calc yaw and ds
+        for i in range(len(fp.x) - 1):
+            dx = fp.x[i + 1] - fp.x[i]
+            dy = fp.y[i + 1] - fp.y[i]
+            fp.yaw.append(math.atan2(dy, dx))
+            fp.ds.append(math.hypot(dx, dy))
+
+        fp.yaw.append(fp.yaw[-1])
+        fp.ds.append(fp.ds[-1])
+
+        # calc curvature
+        for i in range(len(fp.yaw) - 1):
+            fp.c.append((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i])
+
+    return fplist
+
+
+def check_collision(fp, ob):
+    for i in range(len(ob[:, 0])):
+        d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
+             for (ix, iy) in zip(fp.x, fp.y)]
+
+        collision = any([di <= ROBOT_RADIUS ** 2 for di in d])
+
+        if collision:
+            return False
+
+    return True
+
+
+def check_paths(fplist, ob):
+    ok_ind = []
+    for i, _ in enumerate(fplist):
+        if any([v > MAX_SPEED for v in fplist[i].s_d]):  # Max speed check
+            continue
+        elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]):  # Max accel check
+            continue
+        elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]):  # Max curvature check
+            continue
+        elif not check_collision(fplist[i], ob):
+            continue
+
+        ok_ind.append(i)
+
+    return [fplist[i] for i in ok_ind]
+
+
+def frenet_optimal_planning(csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob, max_left, max_right):
+    fplist = calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0, max_left, max_right)
+    fplist = calc_global_paths(fplist, csp)
+    fplist = check_paths(fplist, ob)
+
+    # find minimum cost path
+    min_cost = float("inf")
+    best_path = None
+    for fp in fplist:
+        if min_cost >= fp.cf:
+            min_cost = fp.cf
+            best_path = fp
+
+    return best_path
+
+
+def generate_target_course(x, y):
+    csp = cubic_spline_planner.CubicSpline2D(x, y)
+    s = np.arange(0, csp.s[-1], 0.1)
+
+    rx, ry, ryaw, rk = [], [], [], []
+    for i_s in s:
+        ix, iy = csp.calc_position(i_s)
+        rx.append(ix)
+        ry.append(iy)
+        ryaw.append(csp.calc_yaw(i_s))
+        rk.append(csp.calc_curvature(i_s))
+
+    return rx, ry, ryaw, rk, csp
+
+def generate_parallel_paths(tx, ty, left_distance, right_distance):
+    left_tx = []
+    left_ty = []
+    right_tx = []
+    right_ty = []
+
+    # Convert lists to numpy arrays for easier calculations
+    tx = np.array(tx)
+    ty = np.array(ty)
+
+    for i in range(len(tx) - 1):
+        x1, y1 = tx[i], ty[i]
+        x2, y2 = tx[i + 1], ty[i + 1]
+
+        # Direction vector of the segment
+        dx = x2 - x1
+        dy = y2 - y1
+
+        # Perpendicular vector
+        perp_x = -dy
+        perp_y = dx
+
+        # Normalize perpendicular vector
+        length = np.sqrt(perp_x**2 + perp_y**2)
+        perp_x /= length
+        perp_y /= length
+
+        # Calculate offset vectors
+        left_offset_x = left_distance * perp_x
+        left_offset_y = left_distance * perp_y
+        right_offset_x = right_distance * perp_x
+        right_offset_y = right_distance * perp_y
+
+        # Apply offsets to get new paths
+        left_tx.append(x1 + left_offset_x)
+        left_ty.append(y1 + left_offset_y)
+        right_tx.append(x1 + right_offset_x)
+        right_ty.append(y1 + right_offset_y)
+
+        left_tx.append(x2 + left_offset_x)
+        left_ty.append(y2 + left_offset_y)
+        right_tx.append(x2 + right_offset_x)
+        right_ty.append(y2 + right_offset_y)
+
+    return left_tx, left_ty, right_tx, right_ty
+
+def main():
+    print(__file__ + " start!!")
+
+    # way points
+    wx = [0.0, 10.0, 20.5, 35.0, 70.5, 100.0, 110.7, 150.5]
+    wy = [0.0, -6.0, 5.0, 6.5, 0.0, -7.0, -10.0, -12.0]
+    # obstacle lists
+    ob = np.array([[20.0, 10.0],
+                   [30.0, 6.0],
+                   [30.0, 8.0],
+                   [35.0, 8.0],
+                   [50.0, 3.0]
+                   ])
+
+    tx, ty, tyaw, tc, csp = generate_target_course(wx, wy)
+
+    # initial state
+    c_speed = 10.0 / 3.6  # current speed [m/s]
+    c_accel = 0.0  # current acceleration [m/ss]
+    c_d = 2.0  # current lateral position [m]
+    c_d_d = 0.0  # current lateral speed [m/s]
+    c_d_dd = 0.0  # current lateral acceleration [m/s]
+    s0 = 0.0  # current course position
+
+    area = 50.0  # animation area length [m]
+
+    max_left = 5.0  # maximum left boundary [m]
+    max_right = 5.0  # maximum right boundary [m]
+
+    left_tx, left_ty, right_tx, right_ty = generate_parallel_paths(tx,ty, -max_left, max_right)
+    frame_number = 0
+    for i in range(SIM_LOOP):
+        path = frenet_optimal_planning(csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob, max_left, max_right)
+
+        # Check if a valid path is returned
+        if path is None:
+            print("No valid path found")
+            break
+
+        s0 = path.s[1]
+        c_d = path.d[1]
+        c_d_d = path.d_d[1]
+        c_d_dd = path.d_dd[1]
+        c_speed = path.s_d[1]
+        c_accel = path.s_dd[1]
+
+        if np.hypot(path.x[1] - tx[-1], path.y[1] - ty[-1]) <= 1.0:
+            print("Goal")
+            break
+
+        if show_animation:  # pragma: no cover
+            plt.cla()
+            # for stopping simulation with the esc key.
+            plt.gcf().canvas.mpl_connect(
+                'key_release_event',
+                lambda event: [exit(0) if event.key == 'escape' else None])
+            plt.plot(tx, ty)
+            plt.plot(left_tx, left_ty)
+            plt.plot(right_tx, right_ty)
+            plt.plot(ob[:, 0], ob[:, 1], "xk")
+            plt.plot(path.x[1:], path.y[1:], "-or")
+            plt.plot(path.x[1], path.y[1], "vc")
+            plt.xlim(path.x[1] - area, path.x[1] + area)
+            plt.ylim(path.y[1] - area, path.y[1] + area)
+            plt.title("v[km/h]:" + str(c_speed * 3.6)[0:4])
+            plt.grid(True)
+            plt.pause(0.0001)
+            # Save the current frame
+            plt.savefig(f'frames/frame_{frame_number:04d}.png')
+            frame_number += 1
+
+    print("Finish")
+    if show_animation:  # pragma: no cover
+        plt.grid(True)
+        plt.pause(0.0001)
+        plt.show()
+
+
+
+if __name__ == '__main__':
+    main()