def _generate(self): waypoints1 = np.vstack([[0, 0], [0, 500]]).T path1 = Path(waypoints1) init_pos1 = path1(0) init_angle1 = path1.get_direction(0) init_state1 = np.hstack([init_pos1, init_angle1]) self.main_vessel = Vessel(self.config, init_state=init_state1, init_path=path1, width=2) #self.config["vessel_width"]) self.main_vessel.path = path1 self.rewarder_dict[self.main_vessel.index] = ColavRewarder( self.main_vessel) self.rewarder = self.rewarder_dict[self.main_vessel.index] prog = 0 self.path_prog_hist = np.array([prog]) self.max_path_prog = prog self.moving_obstacles = [self.main_vessel] #Adding moving obstacle waypoints2 = np.vstack([[0, 150], [0, -400]]).T path2 = Path(waypoints2) init_pos2 = path2(0) init_angle2 = path2.get_direction(0) init_state2 = np.hstack([init_pos2, init_angle2]) vessel = Vessel(self.config, init_state=init_state2, init_path=path2, index=1, width=2) #self.config["vessel_width"]) self.rewarder_dict[vessel.index] = ColavRewarder(vessel) self.moving_obstacles.append(vessel) vessel.path = path2 for vessel in self.moving_obstacles: other_vessels = [ x for x in self.moving_obstacles if x.index != vessel.index ] vessel.obstacles = np.hstack([other_vessels]) print('Generated vessels!')
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): self.path = Path([[6945 - self.x0, 6329 - self.x0], [4254 - self.y0, 5614 - self.y0]]) self.obstacle_perimeters = np.load('resources/obstacles_trondheim.npy') self.all_terrain = np.load(TERRAIN_DATA_PATH)[self.x0:8000, self.y0:6900] / 7.5 super()._generate()
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): 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): #self.path = Path([[-50, 1750], [250, 1200]]) #self.path = Path([[650, 1750], [450, 1200]]) self.path = Path([[1000, 830, 700, 960, 1080, 1125], [910, 800, 700, 550, 750, 810]]) self.obstacle_perimeters = np.load('resources/obstacles_sorbuoya.npy') self.all_terrain = np.load( TERRAIN_DATA_PATH ) / 7.5 #np.load(TERRAIN_DATA_PATH)[0000:2000, 10000:12000]/7.5 super()._generate()
def _generate(self): self.path = Path([[520, 1070, 4080, 5473, 10170, 12220], [3330, 5740, 7110, 4560, 7360, 11390]]) #South-west -> north-east self.obstacle_perimeters = np.load( 'resources/obstacles_trondheimsfjorden.npy') self.all_terrain = np.load( TERRAIN_DATA_PATH) / 7.5 #[3121:4521, 5890:7390]/7.5 super()._generate()
def _generate(self): waypoints = np.vstack([[25, 10], [25, 200]]).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 if self.render_mode == '3d': self.all_terrain = np.zeros((50, 50), dtype=float) self._viewer3d.create_world(self.all_terrain, 0, 0, 50, 50)
def _generate(self): #self.path = Path([[520, 1070, 4080, 5473, 10170, 12220], [3330, 5740, 7110, 4560, 7360, 11390]]) #South-west -> north-east self.path = Path([[ 4100 - self.x0, 4247 - self.x0, 4137 - self.x0, 3937 - self.x0, 3217 - self.x0 ], [ 6100 - self.y0, 6100 - self.y0, 6860 - self.y0, 6910 - self.y0, 6690 - self.y0 ]]) self.obstacle_perimeters = np.load('resources/obstacles_entrance.npy') self.all_terrain = np.load( TERRAIN_DATA_PATH) / 7.5 #[3121:4521, 5890:7390]/7.5 super()._generate()
def _generate(self): self.path = Path([[0, 1100], [0, 1100]]) 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 obst_arclength = 30 for o in range(20): obst_radius = 10 + 10 * o**1.5 obst_arclength += obst_radius * 2 + 30 obst_position = self.path(obst_arclength) self.obstacles.append(CircularObstacle(obst_position, obst_radius))
def _generate(self): waypoint_array = [] for t in range(500): x = t * np.cos(t / 100) y = 2 * t waypoint_array.append([x, y]) waypoints = np.vstack(waypoint_array).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 obst_arclength = 30 obst_radius = 5 while True: obst_arclength += 2 * obst_radius if (obst_arclength >= self.path.length): break obst_displacement_dist = 140 - 120 / ( 1 + np.exp(-0.005 * obst_arclength)) obst_position = self.path(obst_arclength) obst_displacement_angle = self.path.get_direction( obst_arclength) - np.pi / 2 obst_displacement = obst_displacement_dist * np.array([ np.cos(obst_displacement_angle), np.sin(obst_displacement_angle) ]) self.obstacles.append( CircularObstacle(obst_position + obst_displacement, obst_radius)) self.obstacles.append( CircularObstacle(obst_position - obst_displacement, obst_radius))
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 N_obst = 20 N_dist = 100 for n in range(N_obst + 1): obst_radius = 25 angle = np.pi/4 + n/N_obst * np.pi/2 obst_position = np.array([np.cos(angle)*N_dist, np.sin(angle)*N_dist]) self.obstacles.append(CircularObstacle(obst_position, obst_radius))
def _generate(self): self.path = Path([[0, 0, 50, 50], [0, 500, 600, 1000]]) 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 obst_arclength = 50 for o in range(9): obst_radius = 20 obst_arclength += obst_radius * 2 + 170 obst_position = self.path(obst_arclength) obst_displacement = np.array( [obst_radius * (-1)**(o + 1), obst_radius]) self.obstacles.append( CircularObstacle(obst_position + obst_displacement, obst_radius))
def plot_vector_field(env, agent, fig_dir, fig_prefix='', xstep=2.0, ystep=5.0, obstacle=True): OBST_POSITION = [0, 50] OBST_RADIUS = 10 if obstacle: obstacles = [CircularObstacle(OBST_POSITION, OBST_RADIUS)] else: obstacles = [] env.reset() plt.axis('scaled') fig = plt.figure() ax = fig.add_subplot(1, 1, 1) waypoints = np.vstack([[0, 0], [0, 100]]).T env.config["min_goal_distance"] = 0 env.config["min_goal_progress"] = 1 env.config["sensor_rotation"] = False env.path = Path(waypoints) env.obstacles = obstacles path = env.path(np.linspace(0, env.path.length, 1000)) axis_min_x = -30 axis_max_x = 30 axis_min_y = -10 axis_max_y = 100 axis_max = max(axis_max_x, axis_max_y) axis_min = min(axis_min_x, axis_min_y) X = np.arange(axis_min_x, axis_max_x, xstep) Y = np.arange(axis_min_y, axis_max_y, ystep) U, V = np.meshgrid(X, Y) i = 0 for xidx, x in enumerate(X): for yidx, y in enumerate(Y): dist = np.sqrt((x - OBST_POSITION[0])**2 + (y - OBST_POSITION[1])**2) if (dist <= OBST_RADIUS): U[yidx][xidx] = 0 V[yidx][xidx] = 0 continue psi = np.pi / 2 rudder_angle = 1 fil_psi = psi for _ in range(50): #env.reset() env.path = Path(waypoints) env.obstacles = obstacles env.vessel.reset([x, y, psi], [0, 0, 0]) env.target_arclength = env.path.length obs, reward, done, info = env.step([0, 0]) #obs = env.observe() action, _states = agent.predict(obs, deterministic=True) last_rudder_angle = rudder_angle rudder_angle = action[1] thruster = action[0] psi -= rudder_angle * 0.1 fil_psi = 0.8 * fil_psi + 0.2 * psi U[yidx][xidx] = thruster * np.cos(fil_psi) V[yidx][xidx] = thruster * np.sin(fil_psi) sys.stdout.write('Simulating behavior {:.2%} ({}/{})\r'.format( (i + 1) / (len(X) * len(Y)), (i + 1), (len(X) * len(Y)))) sys.stdout.flush() i += 1 fig, ax = plt.subplots() ax.set_aspect(1.0) q = ax.quiver(X, Y, U, V) plt.style.use('ggplot') plt.rc('font', family='serif') plt.rc('xtick', labelsize=8) plt.rc('ytick', labelsize=8) plt.rc('axes', labelsize=8) ax.plot(path[0, :], path[1, :], dashes=[6, 2], linewidth=1.0, color='red', label=r'Path') 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))) ax.set_xlim(axis_min_x - 5, axis_max_x + 5) ax.set_ylim(axis_min_y - 5, axis_max_y + 5) for obst in env.obstacles: circle = plt.Circle(obst.position, obst.radius, facecolor='tab:red', edgecolor='black', linewidth=0.5, zorder=10) obst = ax.add_patch(circle) obst.set_hatch('////') goal = plt.Circle((path[0, -1], path[1, -1]), (axis_max - axis_min) / 100, facecolor='black', linewidth=0.5, zorder=11) ax.add_patch(goal) ax.annotate( "Goal", xy=(path[0, -1] - (axis_max - axis_min) / 15, path[1, -1]), fontsize=12, ha="center", zorder=20, color='black', ) #ax.legend() fig.savefig(os.path.join(fig_dir, fig_prefix + 'vector_field.pdf'), format='pdf', bbox_inches='tight') plt.close(fig)
def plot_streamlines(env, agent, fig_dir, fig_prefix='', N=11): OBST_POSITION = [0, 50] OBST_RADIUS = 25 env.reset() plt.axis('scaled') fig = plt.figure() ax = fig.add_subplot(1, 1, 1) waypoints = np.vstack([[0, 0], [0, 100]]).T env.path = Path(waypoints) env.obstacles = [CircularObstacle(OBST_POSITION, OBST_RADIUS)] env.config["min_goal_distance"] = 0 env.config["min_goal_progress"] = 0 path = env.path(np.linspace(0, env.path.length, 1000)) plt.style.use('ggplot') plt.rc('font', family='serif') plt.rc('xtick', labelsize=8) plt.rc('ytick', labelsize=8) plt.rc('axes', labelsize=8) plt.axis('scaled') fig = plt.figure() ax = fig.add_subplot(1, 1, 1) ax.set_aspect(1.0) axis_min_x = path[0, :].min() - 75 axis_max_x = path[0, :].max() + 75 axis_min_y = path[1, :].min() - 25 axis_max_y = path[1, :].max() + 25 axis_max = max(axis_max_x, axis_max_y) axis_min = min(axis_min_x, axis_min_y) ax.plot(path[0, :], path[1, :], color='black', linewidth=1.5, label=r'Path') 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))) ax.set_xlim(axis_min_x, axis_max_x) ax.set_ylim(axis_min_y, axis_max_y) for i, dx in enumerate(np.linspace(-2 * OBST_RADIUS, 2 * OBST_RADIUS, N)): env.vessel.reset([dx, 0, np.pi / 2]) obs = env.observe() while 1: action, _states = agent.predict(obs, deterministic=True) obs, reward, done, info = env.step(action) if done or info['progress'] >= 1.0: break sys.stdout.write( 'Simulating episode {}/{}, progress {:.2%}\r'.format( (i + 1), N, info['progress'])) sys.stdout.flush() path_taken = env.vessel.path_taken ax.plot(path_taken[:, 0], path_taken[:, 1], dashes=[6, 2], color='red', linewidth=1.0, label=r'$x_0 = ' + str(dx) + r'$') for obst in env.obstacles: circle = plt.Circle(obst.position, obst.radius, facecolor='tab:red', edgecolor='black', linewidth=0.5, zorder=10) obst = ax.add_patch(circle) obst.set_hatch('////') goal = plt.Circle((path[0, -1], path[1, -1]), (axis_max - axis_min) / 100, facecolor='black', linewidth=0.5, zorder=11) ax.add_patch(goal) ax.annotate( "Goal", xy=(path[0, -1] + (axis_max - axis_min) / 15, path[1, -1]), fontsize=12, ha="center", zorder=20, color='black', ) #ax.legend() fig.savefig(os.path.join(fig_dir, fig_prefix + 'streamlines.pdf'), format='pdf', bbox_inches='tight') plt.close(fig)
def _generate(self): #self.path = Path([[-50, 1750], [250, 1200]]) self.path = Path([[650, 1750], [450, 1200]]) self.obstacle_perimeters = np.load('../resources/obstacles_sorbuoya.npy') self.all_terrain = np.load(TERRAIN_DATA_PATH)[0000:2000, 10000:12000]/7.5 super()._generate()
def _generate(self): #self.path = Path([[520, 1070, 4080, 5473, 10170, 12220], [3330, 5740, 7110, 4560, 7360, 11390]], smooth=False) #South-west -> north-east self.path = Path([[4177-self.x0, 4137-self.x0, 3217-self.x0], [6700-self.y0, 7075-self.y0, 6840-self.y0]], smooth=False) self.obstacle_perimeters = np.load('../resources/obstacles_entrance.npy') self.all_terrain = np.load(TERRAIN_DATA_PATH)[3121:4521, 5890:7390]/7.5 super()._generate()
def _generate(self): self.path = Path([[1900, 1000], [2500, 500]]) self.obstacle_perimeters = np.load('../resources/obstacles_trondheim.npy') self.all_terrain = np.load(TERRAIN_DATA_PATH)[5000:8000, 1900:4900]/7.5 super()._generate()