def generate_path(states, k0): # x, y, yaw, s, km, kf lookup_table = get_lookup_table() result = [] for state in states: bestp = search_nearest_one_from_lookuptable(state[0], state[1], state[2], lookup_table) target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) init_p = np.matrix( [math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).T x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p) if x is not None: print("find good path") result.append( [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) print("finish path generation") return result
def generate_path(target_states, k0): # x, y, yaw, s, km, kf lookup_table = get_lookup_table() result = [] for state in target_states: bestp = search_nearest_one_from_lookuptable(state[0], state[1], state[2], lookup_table) print("x,y,yaw,s,km,kf") print("bestp : ", bestp) target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) # State class constructor init_p = np.array( [math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).reshape(3, 1) # 거리, s, km print("init_p : ", init_p) x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p) if x is not None: print("find good path") result.append( [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) print("finish path generation") return result
def generate_lookup_table(): states = calc_states_list() k0 = 0.0 # x, y, yaw, s, km, kf lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]] for state in states: bestp = search_nearest_one_from_lookuptable(state[0], state[1], state[2], lookuptable) target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) init_p = np.matrix( [math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).T print("bestp", bestp) print("initp", init_p) if init_p[0] == 0.0: print("hello") init_p[0] = 0.1 cur_states = [0, 0, 0, 0] x, y, yaw, p = planner.optimize_trajectory(cur_states, target, k0, init_p) if x is not None: print("find good path") lookuptable.append( [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) print("finish lookup table generation") save_lookup_table("lookuptables.csv", lookuptable) # print("table[3]", table[4]) # print("table[4]", table[4]) # print("table[5]", table[5]) for table in lookuptable: if table[3] != 0: xc, yc, yawc = motion_model.generate_trajectory( cur_states, table[3], table[4], table[5], k0) plt.plot(xc, yc, "-r") # xc, yc, yawc = motion_model.generate_trajectory(cur_states, # table[3], -table[4], -table[5], k0) # plt.plot(xc, yc, "-r") plt.grid(True) plt.axis("equal") plt.show() print("Done")
def generate_lookup_table(): states = calc_states_list() # calculate target states k0 = 0.0 # target_x, target_y, target_yaw -> s, km, kf lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]] print("Lookup Table is being generated......") for idx, state in enumerate(states): bestp = search_nearest_one_from_lookuptable(state[0], state[1], state[2], lookuptable) target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) # init_p = np.matrix( # [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T init_p = np.matrix([bestp[3], bestp[4], bestp[5]]).T x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p) if x is not None: print("find good path") lookuptable.append( [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) ## print percent of progress percent = 100 * (idx + 1.0) / len(states) print("Complete %{}...".format(percent)) print("finish lookup table generation") save_lookup_table("lookuptable.csv", lookuptable) for table in lookuptable: xc, yc, yawc = motion_model.generate_trajectory( table[3], table[4], table[5], k0) plt.plot(xc, yc, "-r") xc, yc, yawc = motion_model.generate_trajectory( table[3], -table[4], -table[5], k0 ) # symmetrical, this is why target_yaw inlcude only -30 degree and exclude +30 degree # similar for target_y, note that not for target_x # also note that here change the sign of steering make the sign of target_yaw # and target_y change at the same time plt.plot(xc, yc, "-r") plt.grid(True) plt.axis("equal") plt.show() print("Done")
def generate_lookup_table(): states = calc_states_list() k0 = 0.0 # x, y, yaw, s, km, kf lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]] for state in states: bestp = search_nearest_one_from_lookuptable(state[0], state[1], state[2], lookuptable) target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) init_p = np.array( [math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).reshape(3, 1) x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p) if x is not None: print("find good path") lookuptable.append( [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) print("finish lookup table generation") save_lookup_table("lookuptable.csv", lookuptable) for table in lookuptable: xc, yc, yawc = motion_model.generate_trajectory( table[3], table[4], table[5], k0) plt.plot(xc, yc, "-r") xc, yc, yawc = motion_model.generate_trajectory( table[3], -table[4], -table[5], k0) plt.plot(xc, yc, "-r") plt.grid(True) plt.axis("equal") plt.show() print("Done")
def generate_lookup_table(): states = calc_states_list() k0 = 0.0 # x, y, yaw, s, km, kf lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]] for state in states: bestp = search_nearest_one_from_lookuptable( state[0], state[1], state[2], lookuptable) target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) init_p = np.array( [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).reshape(3, 1) x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p) if x is not None: print("find good path") lookuptable.append( [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) print("finish lookup table generation") save_lookup_table("lookuptable.csv", lookuptable) for table in lookuptable: xc, yc, yawc = motion_model.generate_trajectory( table[3], table[4], table[5], k0) plt.plot(xc, yc, "-r") xc, yc, yawc = motion_model.generate_trajectory( table[3], -table[4], -table[5], k0) plt.plot(xc, yc, "-r") plt.grid(True) plt.axis("equal") plt.show() print("Done")
def generate_path(target_states, k0): # x, y, yaw, s, km, kf lookup_table = get_lookup_table() result = [] for state in target_states: bestp = search_nearest_one_from_lookuptable( state[0], state[1], state[2], lookup_table) target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) init_p = np.matrix( [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p) if x is not None: print("find good path") result.append( [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) print("finish path generation") return result