def test2(max_vel=0.5): for i in range(10): trajectory_segments = [] # segment 1: lane-change curve start_pose = [0, 0, 0] end_pose = [4, 3.0, 0] # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative kappa = [0, 0, 0, 0] # NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO! # was: eta = [0, 0, (i - 5) * 20, (5 - i) * 20, 0, 0], now is: eta = [0.1, 0.1, (i - 5) * 20, (5 - i) * 20, 0, 0] trajectory_segments.append(eta3_path_segment( start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) traj = eta3_trajectory(trajectory_segments, max_vel=max_vel, max_accel=0.5) # interpolate at several points along the path times = np.linspace(0, traj.total_time, 101) state = np.empty((5, times.size)) for j, t in enumerate(times): state[:, j] = traj.calc_traj_point(t) if show_animation: # plot the path plt.plot(state[0, :], state[1, :]) plt.pause(1.0) plt.show() if show_animation: plt.close("all")
def test3(max_vel=2.0): trajectory_segments = [] # segment 1: lane-change curve start_pose = [0, 0, 0] end_pose = [4, 1.5, 0] # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative kappa = [0, 0, 0, 0] eta = [4.27, 4.27, 0, 0, 0, 0] trajectory_segments.append(eta3_path_segment( start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) # segment 2: line segment start_pose = [4, 1.5, 0] end_pose = [5.5, 1.5, 0] kappa = [0, 0, 0, 0] # NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO! # was: eta = [0, 0, 0, 0, 0, 0], now is: eta = [0.5, 0.5, 0, 0, 0, 0] trajectory_segments.append(eta3_path_segment( start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) # segment 3: cubic spiral start_pose = [5.5, 1.5, 0] end_pose = [7.4377, 1.8235, 0.6667] kappa = [0, 0, 1, 1] eta = [1.88, 1.88, 0, 0, 0, 0] trajectory_segments.append(eta3_path_segment( start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) # segment 4: generic twirl arc start_pose = [7.4377, 1.8235, 0.6667] end_pose = [7.8, 4.3, 1.8] kappa = [1, 1, 0.5, 0] eta = [7, 10, 10, -10, 4, 4] trajectory_segments.append(eta3_path_segment( start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) # segment 5: circular arc start_pose = [7.8, 4.3, 1.8] end_pose = [5.4581, 5.8064, 3.3416] kappa = [0.5, 0, 0.5, 0] eta = [2.98, 2.98, 0, 0, 0, 0] trajectory_segments.append(eta3_path_segment( start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) # construct the whole path traj = eta3_trajectory(trajectory_segments, max_vel=max_vel, max_accel=0.5, max_jerk=1) # interpolate at several points along the path times = np.linspace(0, traj.total_time, 1001) state = np.empty((5, times.size)) for i, t in enumerate(times): state[:, i] = traj.calc_traj_point(t) # plot the path if show_animation: fig, ax = plt.subplots() x, y = state[0, :], state[1, :] points = np.array([x, y]).T.reshape(-1, 1, 2) segs = np.concatenate([points[:-1], points[1:]], axis=1) lc = LineCollection(segs, cmap=plt.get_cmap('inferno')) ax.set_xlim(np.min(x) - 1, np.max(x) + 1) ax.set_ylim(np.min(y) - 1, np.max(y) + 1) lc.set_array(state[3, :]) lc.set_linewidth(3) ax.add_collection(lc) axcb = fig.colorbar(lc) axcb.set_label('velocity(m/s)') ax.set_title('Trajectory') plt.xlabel('x') plt.ylabel('y') plt.pause(1.0) fig1, ax1 = plt.subplots() ax1.plot(times, state[3, :], 'b-') ax1.set_xlabel('time(s)') ax1.set_ylabel('velocity(m/s)', color='b') ax1.tick_params('y', colors='b') ax1.set_title('Control') ax2 = ax1.twinx() ax2.plot(times, state[4, :], 'r-') ax2.set_ylabel('angular velocity(rad/s)', color='r') ax2.tick_params('y', colors='r') fig.tight_layout() plt.show()