def _generate(self): waypoints = np.vstack([[250, 100], [250, 300]]).T self.path = Path(waypoints) init_state = self.path(0) init_angle = self.path.get_direction(0) self.vessel = Vessel(self.config, np.hstack([init_state, init_angle]), width=self.config["vessel_width"]) prog = self.path.get_closest_arclength(self.vessel.position) self.path_prog_hist = np.array([prog]) self.max_path_prog = prog self.obstacles = [] self.vessel_obstacles = [] for vessel_idx in range(5): other_vessel_trajectory = [] trajectory_shift = self.rng.rand()() * 2 * np.pi trajectory_radius = self.rng.rand()() * 40 + 30 trajectory_speed = self.rng.rand()() * 0.003 + 0.003 for i in range(10000): #other_vessel_trajectory.append((10*i, (250, 400-10*i))) other_vessel_trajectory.append( (1 * i, (250 + trajectory_radius * np.cos(trajectory_speed * i + trajectory_shift), 150 + 70 * vessel_idx + trajectory_radius * np.sin(trajectory_speed * i + trajectory_shift)))) other_vessel_obstacle = VesselObstacle( width=6, trajectory=other_vessel_trajectory) self.obstacles.append(other_vessel_obstacle) self.vessel_obstacles.append(other_vessel_obstacle) for vessel_idx in range(5): other_vessel_trajectory = [] trajectory_start = self.rng.rand()() * 200 + 150 trajectory_speed = self.rng.rand()() * 0.03 + 0.03 trajectory_shift = 10 * self.rng.rand()() for i in range(10000): other_vessel_trajectory.append( (i, (245 + 2.5 * vessel_idx + trajectory_shift, trajectory_start - 10 * trajectory_speed * i))) other_vessel_obstacle = VesselObstacle( width=6, trajectory=other_vessel_trajectory) self.obstacles.append(other_vessel_obstacle) self.vessel_obstacles.append(other_vessel_obstacle) if self.render_mode == '3d': self.all_terrain = np.load(TERRAIN_DATA_PATH)[1950:2450, 5320:5820] / 7.5 #terrain = np.zeros((500, 500), dtype=float) # for x in range(10, 40): # for y in range(10, 40): # z = 0.5*np.sqrt(max(0, 15**2 - (25.0-x)**2 - (25.0-y)**2)) # terrain[x][y] = z self._viewer3d.create_world(self.all_terrain, 0, 0, 500, 500)
def _generate(self): waypoints = np.vstack([[0, 0], [0, 500]]).T self.path = Path(waypoints) init_state = self.path(0) init_angle = self.path.get_direction(0) self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) prog = self.path.get_closest_arclength(self.vessel.position) self.path_prog_hist = np.array([prog]) self.max_path_prog = prog vessel_pos = self.vessel.position trajectory_shift = -50 * deg2rad #random.uniform(-5*deg2rad, 5*deg2rad) #2*np.pi*(rng.rand() - 0.5) trajectory_radius = 200 trajectory_speed = 0.5 start_angle = 70 * deg2rad start_x = vessel_pos[0] + trajectory_radius * np.sin(start_angle) start_y = vessel_pos[1] + trajectory_radius * np.cos(start_angle) # vessel_trajectory = [[0, (vessel_pos[1], trajectory_radius+vessel_pos[0])]] # in front, ahead vessel_trajectory = [[0, (start_x, start_y)]] for i in range(1, 5000): vessel_trajectory.append( (1 * i, (start_x + trajectory_speed * np.sin(trajectory_shift) * i, start_y + trajectory_speed * np.cos(trajectory_shift) * i))) self.obstacles = [ VesselObstacle(width=30, trajectory=vessel_trajectory) ] self._update()
def _generate(self): init_state = self.path(0) init_angle = self.path.get_direction(0) self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) prog = self.path.get_closest_arclength(self.vessel.position) self.path_prog_hist = np.array([prog]) self.max_path_prog = prog self.all_obstacles = [] self.obstacles = [] for obstacle_perimeter in self.obstacle_perimeters: if len(obstacle_perimeter) > 3: obstacle = PolygonObstacle(obstacle_perimeter) if obstacle.boundary.is_valid: self.all_obstacles.append(obstacle) for vessel_width, vessel_trajectory, vessel_name in self.other_vessels: # for k in range(0, len(vessel_trajectory)-1): # vessel_obstacle = VesselObstacle(width=int(vessel_width), trajectory=vessel_trajectory[k:]) # self.all_obstacles.append(vessel_obstacle) if len(vessel_trajectory) > 2: vessel_obstacle = VesselObstacle(width=int(vessel_width), trajectory=vessel_trajectory, name=vessel_name) self.all_obstacles.append(vessel_obstacle) self._update()
def _generate(self): print('Generating') self.obstacle_perimeters = None self.all_terrain = np.load(TERRAIN_DATA_PATH) / 7.5 path_length = 1.2 * (100 + self.rng.randint(400)) while 1: x0 = self.rng.randint(1000, self.all_terrain.shape[0] - 1000) y0 = self.rng.randint(1000, self.all_terrain.shape[1] - 1000) dir = self.rng.rand() * 2 * np.pi waypoints = [[x0, x0 + path_length * np.cos(dir)], [y0, y0 + path_length * np.sin(dir)]] close_proximity = self.all_terrain[x0 - 50:x0 + 50, y0 - 50:y0 + 50] path_center = [ x0 + path_length / 2 * np.cos(dir), y0 + path_length / 2 * np.sin(dir) ] path_end = [ x0 + path_length * np.cos(dir), y0 + path_length * np.sin(dir) ] proximity = self.all_terrain[x0 - 250:x0 + 250, y0 - 250:y0 + 250] if proximity.max() > 0 and close_proximity.max() == 0: break self.path = Path(waypoints) init_state = self.path(0) init_angle = self.path.get_direction(0) self.vessel = Vessel(self.config, np.hstack([init_state, init_angle])) self.rewarder = ColregRewarder(self.vessel, test_mode=True) self._rewarder_class = ColregRewarder prog = self.path.get_closest_arclength(self.vessel.position) self.path_prog_hist = np.array([prog]) self.max_path_prog = prog self.obstacles, self.all_obstacles = [], [] for i in range(1): trajectory_speed = 0.4 + 0.2 * self.rng.rand() start_x = path_end[0] start_y = path_end[1] vessel_trajectory = [[0, (start_x, start_y)]] for t in range(1, 10000): vessel_trajectory.append( (1 * t, (start_x - trajectory_speed * np.cos(dir) * t, start_y - trajectory_speed * np.sin(dir) * t))) vessel_obstacle = VesselObstacle(width=10, trajectory=vessel_trajectory) self.obstacles.append(vessel_obstacle) self.all_obstacles.append(vessel_obstacle) print('Updating') self._update(force=True)
def _generate(self): # Initializing path nwaypoints = int(np.floor(4 * self.rng.rand() + 2)) self.path = RandomCurveThroughOrigin(self.rng, nwaypoints, length=800) # Initializing vessel init_state = self.path(0) init_angle = self.path.get_direction(0) init_state[0] += 50 * (self.rng.rand() - 0.5) init_state[1] += 50 * (self.rng.rand() - 0.5) init_angle = geom.princip(init_angle + 2 * np.pi * (self.rng.rand() - 0.5)) self.vessel = Vessel(self.config, np.hstack([init_state, init_angle]), width=self.config["vessel_width"]) prog = 0 self.path_prog_hist = np.array([prog]) self.max_path_prog = prog self.obstacles = [] # Adding moving obstacles for _ in range(self._n_moving_obst): other_vessel_trajectory = [] obst_position, obst_radius = helpers.generate_obstacle( self.rng, self.path, self.vessel, obst_radius_mean=10, displacement_dist_std=500) obst_direction = self.rng.rand() * 2 * np.pi obst_speed = np.random.choice(vessel_speed_vals, p=vessel_speed_density) for i in range(10000): other_vessel_trajectory.append( (i, (obst_position[0] + i * obst_speed * np.cos(obst_direction), obst_position[1] + i * obst_speed * np.sin(obst_direction)))) other_vessel_obstacle = VesselObstacle( width=obst_radius, trajectory=other_vessel_trajectory) self.obstacles.append(other_vessel_obstacle) # Adding static obstacles for _ in range(self._n_static_obst): obstacle = CircularObstacle(*helpers.generate_obstacle( self.rng, self.path, self.vessel, displacement_dist_std=250)) self.obstacles.append(obstacle) # Resetting rewarder instance self.rewarder = self._rewarder_class(self.vessel, self.test_mode) self._update()
def plot_trajectory(env, fig_dir, local=False, size=100, fig_prefix='', episode_dict=None): """ Plots the result of a path following episode. Parameters ---------- fig_dir : str Absolute path to a directory to store the plotted figure in. """ #print('Plotting with local = ' + str(local)) path = env.last_episode['path'] path_taken = env.last_episode['path_taken'] obstacles = env.last_episode['obstacles'] #np.savetxt(os.path.join(fig_dir, 'path_taken.txt'), path_taken) if (fig_prefix != '' and not fig_prefix[0] == '_'): fig_prefix = '_' + fig_prefix plt.style.use('ggplot') plt.rc('font', family='serif') plt.rc('xtick', labelsize=12) plt.rc('ytick', labelsize=12) plt.rc('axes', labelsize=12) plt.axis('scaled') fig = plt.figure() ax = fig.add_subplot(1, 1, 1) ax.set_aspect(1.0) if local: axis_min_x = env.vessel.position[0] - size axis_max_x = env.vessel.position[0] + size axis_min_y = env.vessel.position[1] - size axis_max_y = env.vessel.position[1] + size else: axis_min_x = path[0, :].min() - 200 axis_max_x = path[0, :].max() + 200 axis_min_y = path[1, :].min() - 200 axis_max_y = path[1, :].max() + 200 daxisx = axis_max_x - axis_min_x daxisy = axis_max_y - axis_min_y if daxisx > daxisy: d = daxisx - daxisy axis_min_y -= d / 2 axis_max_y += d / 2 if daxisx < daxisy: d = daxisy - daxisx axis_min_x -= d / 2 axis_max_x += d / 2 axis_max = max(axis_max_x, axis_max_y) axis_min = min(axis_min_x, axis_min_y) for obst in obstacles: if isinstance(obst, CircularObstacle): obst_object = plt.Circle(obst.position, obst.radius, facecolor='tab:red', edgecolor='black', linewidth=0.5, zorder=10) obst_object.set_hatch('////') obst = ax.add_patch(obst_object) elif isinstance(obst, PolygonObstacle): obst_object = plt.Polygon(np.array(obst.points), True, facecolor='#C0C0C0', edgecolor='black', linewidth=0.5, zorder=10) obst = ax.add_patch(obst_object) for obst in obstacles: if not obst.static: # if isinstance(obst, VesselObstacle): # x_arr = [elm[0] for elm in obst.path_taken[len(obst.path_taken) - min(len(obst.path_taken), 100):]] # y_arr = [elm[1] for elm in obst.path_taken[len(obst.path_taken) - min(len(obst.path_taken), 100):]] # points = np.array([x_arr, y_arr]).T.reshape(-1, 1, 2) # segments = np.concatenate([points[:-1], points[1:]], axis=1) # colors = [] # for i in range(len(x_arr)-1): # di = len(x_arr)-1 - i # if di > PLOT_COLOR_TRESHOLD: # c = (1.0, 0.0, 0.0) # else: # c = (min(1.0, di*0.01), max(0.0, 1.0-di*0.01), 0.0) # colors.append(c) # lc = LineCollection(segments, color=colors, linewidth=1.0, linestyle='--') # ax.add_collection(lc) # #ax.plot(x_arr, y_arr, dashes=[6, 2], color='red', linewidth=0.5, alpha=0.3) if (not local) and isinstance(obst, VesselObstacle): x_arr = [elm[1][0] for elm in obst.trajectory] y_arr = [elm[1][1] for elm in obst.trajectory] ax.plot(x_arr, y_arr, dashes=[6, 2], color='red', linewidth=0.5, alpha=0.3) if not local: plt.arrow(obst.init_boundary.centroid.coords[0][0], obst.init_boundary.centroid.coords[0][1], 120 * obst.dx, 120 * obst.dy, head_width=3 if local else 8, color='black', zorder=9) for obst in obstacles: if isinstance(obst, VesselObstacle): if local and ( abs(obst.position[0] - env.vessel.position[0]) > size or abs(obst.position[1] - env.vessel.position[1]) > size): continue if local: vessel_obst = VesselObstacle(width=obst.width, trajectory=[], init_position=obst.position, init_heading=obst.heading, init_update=False) vessel_obst_object = plt.Polygon(np.array( list(vessel_obst.boundary.exterior.coords)), True, facecolor='#C0C0C0', edgecolor='red', linewidth=0.5, zorder=10) else: vessel_obst = VesselObstacle(width=obst.width, trajectory=[], init_position=obst.init_position, init_heading=obst.heading, init_update=False) vessel_obst_object = plt.Polygon(np.array( list(vessel_obst.init_boundary.exterior.coords)), True, facecolor='#C0C0C0', edgecolor='red', linewidth=0.5, zorder=10) ax.add_patch(vessel_obst_object) if local and len(obst.heading_taken) >= SHADOW_LENGTH: position = obst.path_taken[-SHADOW_LENGTH] heading = obst.heading_taken[-SHADOW_LENGTH] vessel_obst = VesselObstacle(width=obst.width, trajectory=[], init_position=position, init_heading=heading, init_update=False) vessel_obst_object = plt.Polygon(np.array( list(vessel_obst.boundary.exterior.coords)), True, facecolor='none', edgecolor='red', linewidth=0.5, linestyle='--', zorder=10) ax.add_patch(vessel_obst_object) # x_arr = [elm[0] for elm in obst.path_taken[len(obst.path_taken) - min(len(obst.path_taken), SHADOW_LENGTH):]] # y_arr = [elm[1] for elm in obst.path_taken[len(obst.path_taken) - min(len(obst.path_taken), SHADOW_LENGTH):]] # points = np.array([x_arr, y_arr]).T.reshape(-1, 1, 2) # segments = np.concatenate([points[:-1], points[1:]], axis=1) # colors = [] # for i in range(len(x_arr)-1): # di = len(x_arr)-1 - i # if di > PLOT_COLOR_TRESHOLD: # c = (1.0, 0.0, 0.0) # else: # c = (max(0.0, 1-di*0.01), 0.0, 0.0) # colors.append(c) # lc = LineCollection(segments, color=colors, linewidth=0.5, linestyle='--', zorder=9) # ax.add_collection(lc) if local: vessel_obst = VesselObstacle(width=env.vessel.width, trajectory=[], init_position=env.vessel.position, init_heading=env.vessel.heading, init_update=False) vessel_obst_object = plt.Polygon(np.array( list(vessel_obst.boundary.exterior.coords)), True, facecolor='#C0C0C0', edgecolor='red', linewidth=0.5, zorder=10) ax.add_patch(vessel_obst_object) if len(env.vessel.heading_taken) >= SHADOW_LENGTH: position = env.vessel.path_taken[-SHADOW_LENGTH] heading = env.vessel.heading_taken[-SHADOW_LENGTH] vessel_obst = VesselObstacle(width=env.vessel.width, trajectory=[], init_position=position, init_heading=heading, init_update=False) vessel_obst_object = plt.Polygon(np.array( list(vessel_obst.boundary.exterior.coords)), True, facecolor='none', edgecolor='red', linewidth=0.5, linestyle='--', zorder=10) ax.add_patch(vessel_obst_object) if local and size <= 50: ax.set_ylabel(r"North (m)") ax.set_xlabel(r"East (m)") ax.xaxis.set_major_formatter( FuncFormatter(lambda y, _: '{:.0f}'.format(y * 10))) ax.yaxis.set_major_formatter( FuncFormatter(lambda y, _: '{:.0f}'.format(y * 10))) else: ax.set_ylabel(r"North (km)") ax.set_xlabel(r"East (km)") ax.xaxis.set_major_formatter( FuncFormatter(lambda y, _: '{:.1f}'.format(y / 100))) ax.yaxis.set_major_formatter( FuncFormatter(lambda y, _: '{:.1f}'.format(y / 100))) ax.set_xlim(axis_min_x, axis_max_x) ax.set_ylim(axis_min_y, axis_max_y) #ax.legend() dashmultiplier = 3 if local == False else size / 100 linemultiplier = 1 if local == False else size / 300 ax.plot(path[0, :], path[1, :], dashes=[3 * dashmultiplier, 1 * dashmultiplier], color='black', linewidth=1.0 * linemultiplier, label=r'Path', zorder=8) if episode_dict is None or local: pathcolor = 'red' L = len(env.vessel.heading_taken) if L + 5 >= SHADOW_LENGTH: ax.plot(path_taken[:L - SHADOW_LENGTH - 7, 0], path_taken[:L - SHADOW_LENGTH - 7, 1], dashes=[1 * dashmultiplier, 2 * dashmultiplier], color=pathcolor, linewidth=1.0 * linemultiplier, label=r'Path taken') ax.plot(path_taken[L - SHADOW_LENGTH + 7 + int(env.vessel.width):, 0], path_taken[L - SHADOW_LENGTH + 7 + int(env.vessel.width):, 1], dashes=[1 * dashmultiplier, 2 * dashmultiplier], color=pathcolor, linewidth=1.0 * linemultiplier, label=r'Path taken') else: ax.plot(path_taken[:, 0], path_taken[:, 1], dashes=[1 * dashmultiplier, 2 * dashmultiplier], color=pathcolor, linewidth=1.0 * linemultiplier, label=r'Path taken') # x_arr = path_taken[:, 0] # y_arr = path_taken[:, 1] # points = np.array([x_arr, y_arr]).T.reshape(-1, 1, 2) # segments = np.concatenate([points[:-1], points[1:]], axis=1) # colors = [] # for i in range(len(x_arr)-1): # di = len(x_arr)-1 - i # if di > PLOT_COLOR_TRESHOLD: # c = (1.0, 0.0, 0.0, 0.3) # else: # c = (max(0.0, 1-di/PLOT_COLOR_TRESHOLD), 0.0, 0.0, min(1.0, 0.3+di*0.7/PLOT_COLOR_TRESHOLD)) # colors.append(c) # lc = LineCollection(segments, color=colors, linewidth=1.0, linestyle='--', zorder=9) # ax.add_collection(lc) else: episode_dict_colors = [ episode_dict[value_key][1] for value_key in episode_dict ] normalize = mcolors.Normalize(vmin=min(episode_dict_colors), vmax=max(episode_dict_colors)) colormap = cm.coolwarm for value_key in episode_dict: value_path_taken = episode_dict[value_key][0]['path_taken'] ax.plot(value_path_taken[:, 0], value_path_taken[:, 1], color=colormap(normalize(episode_dict[value_key][1]))) scalarmappaple = cm.ScalarMappable(norm=normalize, cmap=colormap) scalarmappaple.set_array(episode_dict_colors) cbar = fig.colorbar(scalarmappaple) cbar.set_label(r'$ \log10\;{\lambda}$') if isinstance(env, RealWorldEnv) and not local: for x, y in zip(*env.path.init_waypoints): waypoint_marker = plt.Circle((x, y), (axis_max - axis_min) / 150, facecolor='red', linewidth=0.5, zorder=11) ax.add_patch(waypoint_marker) if not local: ax.annotate("Goal", xy=(path[0, -1], path[1, -1] + (axis_max - axis_min) / 25), fontsize=11, ha="center", zorder=20, color='white', family='sans-serif', bbox=dict(facecolor='tab:red', edgecolor='black', alpha=0.75, boxstyle='round')) ax.annotate("Start", xy=(path[0, 0], path[1, 0] - (axis_max - axis_min) / 20), fontsize=11, ha="center", zorder=20, color='white', family='sans-serif', bbox=dict(facecolor='tab:red', edgecolor='black', alpha=0.75, boxstyle='round')) fig.savefig(os.path.join(fig_dir, '{}path.pdf'.format(fig_prefix)), format='pdf', bbox_inches='tight') plt.close(fig)
def _generate(self): vessel_trajectories = [] if self.vessel_data_path is not None: df = pd.read_csv(self.vessel_data_path) vessels = dict(tuple(df.groupby('Vessel_Name'))) vessel_names = sorted(list(vessels.keys())) #print('Preprocessing traffic...') while len(vessel_trajectories) < self.n_vessels: if len(vessel_names) == 0: break vessel_idx = self.rng.randint(0, len(vessel_names)) vessel_name = vessel_names.pop(vessel_idx) vessels[vessel_name]['AIS_Timestamp'] = pd.to_datetime( vessels[vessel_name]['AIS_Timestamp']) vessels[vessel_name]['AIS_Timestamp'] -= vessels[ vessel_name].iloc[0]['AIS_Timestamp'] start_timestamp = None last_timestamp = pd.to_timedelta(0, unit='D') last_east = None last_north = None cutoff_dt = pd.to_timedelta(0.1, unit='D') path = [] for _, row in vessels[vessel_name].iterrows(): east = row['AIS_East'] / 10.0 north = row['AIS_North'] / 10.0 if row['AIS_Length_Overall'] < 12: continue if len(path) == 0: start_timestamp = row['AIS_Timestamp'] timedelta = row['AIS_Timestamp'] - last_timestamp if timedelta < cutoff_dt: if last_east is not None: dx = east - last_east dy = north - last_north distance = np.sqrt(dx**2 + dy**2) seconds = timedelta.seconds speed = distance / seconds if speed < VESSEL_SPEED_RANGE_LOWER or speed > VESSEL_SPEED_RANGE_UPPER: path = [] continue path.append((int((row['AIS_Timestamp'] - start_timestamp).total_seconds()), (east - self.x0, north - self.y0))) else: if len(path) > 1 and not np.isnan( row['AIS_Length_Overall'] ) and row['AIS_Length_Overall'] > 0: start_index = self.rng.randint(0, len(path) - 1) vessel_trajectories.append( (row['AIS_Length_Overall'] / 10.0, path[start_index:], vessel_name)) path = [] last_timestamp = row['AIS_Timestamp'] last_east = east last_north = north #if self.other_vessels: # print(vessel_name, path[0], len(path)) #print('Completed traffic preprocessing') other_vessel_indeces = self.rng.choice(list( range(len(vessel_trajectories))), min(len(vessel_trajectories), self.n_vessels), replace=False) self.other_vessels = [ vessel_trajectories[idx] for idx in other_vessel_indeces ] init_state = self.path(0) init_angle = self.path.get_direction(0) self.vessel = Vessel(self.config, np.hstack([init_state, init_angle]), width=self.config["vessel_width"]) prog = self.path.get_closest_arclength(self.vessel.position) self.path_prog_hist = np.array([prog]) self.max_path_prog = prog self.all_obstacles = [] self.obstacles = [] if self.obstacle_perimeters is not None: for obstacle_perimeter in self.obstacle_perimeters: if len(obstacle_perimeter) > 3: obstacle = PolygonObstacle(obstacle_perimeter) assert obstacle.boundary.is_valid, 'The added obstacle is invalid!' self.all_obstacles.append(obstacle) self.obstacles.append(obstacle) if self.verbose: print('Added {} obstacles'.format(len(self.obstacles))) if self.verbose: print('Generating {} vessel trajectories'.format( len(self.other_vessels))) for vessel_width, vessel_trajectory, vessel_name in self.other_vessels: # for k in range(0, len(vessel_trajectory)-1): # vessel_obstacle = VesselObstacle(width=int(vessel_width), trajectory=vessel_trajectory[k:]) # self.all_obstacles.append(vessel_obstacle) if len(vessel_trajectory) > 2: vessel_obstacle = VesselObstacle(width=int(vessel_width), trajectory=vessel_trajectory, name=vessel_name) self.all_obstacles.append(vessel_obstacle) self.obstacles.append(vessel_obstacle) # if self.render_mode == '3d': # if self.verbose: # print('Loading nearby 3D terrain...') # xlow = 0 # xhigh = self.all_terrain.shape[0] # ylow = 0 # yhigh = self.all_terrain.shape[1] # self._viewer3d.create_world(self.all_terrain, xlow, ylow, xhigh, yhigh) # if self.verbose: # print('Loaded nearby 3D terrain ({}-{}, {}-{})'.format(xlow, xhigh, ylow, yhigh)) self._update()