From ba91d866fb1cf2ac27879e1f2f7df33246f28068 Mon Sep 17 00:00:00 2001 From: MaheshKumar92 <54904641+MaheshKumar92@users.noreply.github.com> Date: Sun, 4 Aug 2024 10:59:02 +0530 Subject: [PATCH] modified frenet optimal trajectory generator to consider the left and right boundary. Plotting the same also. --- ...frenet_optimal_trajectory_with_boundary.py | 375 ++++++++++++++++++ 1 file changed, 375 insertions(+) create mode 100644 PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory_with_boundary.py diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory_with_boundary.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory_with_boundary.py new file mode 100644 index 0000000000..b9e5c4cf1a --- /dev/null +++ b/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()