def test_optimize_path(x=25.0, y=5.0): # target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0)) target = motion_model.State(x=x, y=y, yaw=np.deg2rad(0.0), s=0.0, k=0.0) initial_state = motion_model.State(x=0.0, y=0.0, yaw=np.deg2rad(0.0), s=0.0, k=0.0) init_p = np.array([x, 0.0, 0.0, 0.0]) traj, params, foundPath = optimizePath(target, initial_state, init_p) if foundPath: x = [traj[i].x for i in range(len(traj))] y = [traj[i].y for i in range(len(traj))] yaw = [traj[i].yaw for i in range(len(traj))] for i in range(len(x)): plot_arrow(x[i], y[i], yaw[i], length=0.1) # if plot_trajectories: # show_trajectory(target, x, y) # plt.axis("equal") # plt.grid(True) # plt.show() return traj else: return []
def test_optimize_speed_profile(initialSpeed=10., initial_aceleration=0., final_s=30., final_t=20., maxSpeed=20., minSpeed=1., max_acc=4.): speeds = np.arange(maxSpeed, minSpeed, -1.) curvatures = np.arange(max_acc, -max_acc, -1.) # for l in range(curvatures.shape[0]): for k in range(speeds.shape[0]): initial_state = motion_model.State(x=0., y=0., yaw=1. / initialSpeed, s=0.0, k=initial_aceleration) target = motion_model.State(x=final_s, y=final_t, yaw=1. / speeds[k], s=0.0, k=0.) # initialize parameters init_p = np.array( [final_s / math.cos(1.0 / initialSpeed), 0.0, 0.0, 0.0]) traj, params, foundPath = optimizePath(target, initial_state, init_p) if foundPath: x = [traj[i].x for i in range(len(traj))] y = [traj[i].y for i in range(len(traj))] yaw = [traj[i].yaw for i in range(len(traj))] curvatures = [traj[i].k for i in range(len(traj))] pathValid = True for j in range(len(curvatures)): if math.fabs(curvatures[j]) > max_acc or yaw[j] < ( 1. / maxSpeed) or yaw[j] < 0.: pathValid = False break if pathValid: for i in range(len(x)): plot_arrow(x[i], y[i], yaw[i], length=0.1) if plot_trajectories: show_trajectory(target, x, y) plt.axis("equal") plt.grid(True) # plt.show() return traj return []
def GetTrajectory(path, speed, acc): if len(path) == 0: return [] traj = [] newState = motion_model.State(x=path[0].x, y=path[0].y, yaw=path[0].yaw, s=path[0].s, k=path[0].k) newState.v = speed newState.a = acc newState.t = 0 traj.append(newState) lastCurvature = path[0].k if acc == -1: acc = ((speed - speedDiscretization)**2 - speed**2) / (2 * path[-1].s) elif acc == 1: acc = ((speed + speedDiscretization)**2 - speed**2) / (2 * path[-1].s) for i in range(1, len(path)): state = path[i] if speed**2 + 2 * acc * state.s < 0: return [] else: v = np.sqrt(speed**2 + 2 * acc * state.s) if acc == 0.0: t = state.s / speed else: t = (v - speed) / acc if v >= initialSpeed and t > 0 and v <= maxSpeed: newState = motion_model.State(x=state.x, y=state.y, yaw=state.yaw, s=state.s, k=state.k) newState.v = v newState.a = acc newState.t = t if np.abs(state.k - lastCurvature) / t > maxCurvatureRate: return [] else: traj.append(newState) else: return [] return traj
def generate_path(target_states, k0): """ 产生路径 :param target_states: 目标状态 :param k0: 出事curvature :return: """ # x,y,yaw,s,km,kf (lookuptable[i]) lookup_table = get_loopup_table() result = [] for state in target_states: # from current state to target_state bestp = search_nearest_one_from_lookuptable( state[0], state[1], state[2], lookup_table) # best control factor --- (s,km,kf) target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) # initial state init_p = np.array( [math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).reshape(3, 1) x, y, yaw, p = planner.optimization_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(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 test_optimize_trajectory(): # target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(00.0)) target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(90.0)) k0 = 0.0 init_p = np.matrix([6.0, 0.0, 0.0]).T x, y, yaw, p = optimize_trajectory(target, k0, init_p) show_trajectory(target, x, y) # plt.plot(x, y, "-r") plot_arrow(target.x, target.y, target.yaw) plt.axis("equal") plt.grid(True) plt.show()
def test_optimize_trajectory(): # target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0)) target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(90.0)) k0 = 0.0 init_p = np.array([6.0, 0.0, 0.0]).reshape(3, 1) x, y, yaw, p = optimize_trajectory(target, k0, init_p) if show_animation: show_trajectory(target, x, y) plot_arrow(target.x, target.y, target.yaw) plt.axis("equal") plt.grid(True) plt.show()
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 test_optimize_trajectory(): # pragma: no cover # Target (preset) target = mm.State(x=10.0, y=0.0, yaw=np.deg2rad(60.0)) k0 = 0.0 # initial curvature: 0 1/m init_p = np.array([6.0, 0.0, np.deg2rad(45)]).reshape(3, 1) x, y, yaw, p = optimization_trajectory(target, k0, init_p) if show_animation: show_trajectory(target, x, y, yaw) plot_arrow(target.x, target.y, target.yaw) plt.axis("equal") plt.grid(True) plt.show() plt.plot(x, y, "r--") plt.show()
def test_optimize_trajectory(): mcfg = motion_model.ModelConfig() # target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(00.0)) target = motion_model.State(x=1.0, y=0.0, yaw=math.radians(30.0)) k0 = 0.0 init_p_len = math.sqrt(target.x**2 + target.y**2) init_p = np.matrix([init_p_len, 0.0, 0.0]).T init_p = limitP(init_p, mcfg) x, y, yaw, p = optimize_trajectory(target, k0, init_p) p = limitP(p, mcfg) show_trajectory(target, x, y) matplotrecorder.save_movie("animation.mp4", 0.1) # plt.plot(x, y, "-r") plot_arrow(target.x, target.y, target.yaw) plt.axis("equal") plt.grid(True) plt.show()