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!')
class TestHeadOn(BaseEnvironment): def _generate(self): waypoints = np.vstack([[0, 0], [0, 250]]).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 start_angle = random.uniform(-5 * deg2rad, 5 * deg2rad) trajectory_shift = 5 * deg2rad #random.uniform(-5*deg2rad+start_angle, 5*deg2rad+start_angle) #2*np.pi*(rng.rand() - 0.5) trajectory_radius = 150 trajectory_speed = 0.5 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, (start_x, start_y)]] for i in range(1, 5000): vessel_trajectory.append( (1 * i, (start_x - trajectory_speed * np.sin(start_angle) * i, start_y - trajectory_speed * np.cos(start_angle) * i))) self.obstacles = [ VesselObstacle(width=30, trajectory=vessel_trajectory) ] self._update()
class TestScenario2(BaseEnvironment): 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))
class DebugScenario(BaseEnvironment): def _generate(self): waypoints = np.vstack([[250, 100], [250, 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 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)
class EmptyScenario(BaseEnvironment): 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)
class TestScenario1(BaseEnvironment): 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))
class TestScenario3(BaseEnvironment): 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))
class TestScenario0(BaseEnvironment): 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 navigate(self, path: Path) -> np.ndarray: """ Calculates and returns navigation states representing the vessel's attitude with respect to the desired path. Returns ------- navigation_states : np.ndarray """ # Calculating path arclength at reference point, i.e. the point closest to the vessel vessel_arclength = path.get_closest_arclength(self.position) # Calculating tangential path direction at reference point path_direction = path.get_direction(vessel_arclength) cross_track_error = geom.Rzyx(0, 0, -path_direction).dot( np.hstack([path(vessel_arclength) - self.position, 0]))[1] # Calculating tangential path direction at look-ahead point target_arclength = min( path.length, vessel_arclength + self.config["look_ahead_distance"]) look_ahead_path_direction = path.get_direction(target_arclength) look_ahead_heading_error = float( geom.princip(look_ahead_path_direction - self.heading)) # Calculating vector difference between look-ahead point and vessel position target_vector = path(target_arclength) - self.position # Calculating heading error target_heading = np.arctan2(target_vector[1], target_vector[0]) heading_error = float(geom.princip(target_heading - self.heading)) # Calculating path progress progress = vessel_arclength / path.length self._progress = progress # Concatenating states self._last_navi_state_dict = { 'surge_velocity': self.velocity[0], 'sway_velocity': self.velocity[1], 'yaw_rate': self.yaw_rate, 'look_ahead_heading_error': look_ahead_heading_error, 'heading_error': heading_error, 'cross_track_error': cross_track_error / 100, 'target_heading': target_heading, 'look_ahead_path_direction': look_ahead_path_direction, 'path_direction': path_direction, 'vessel_arclength': vessel_arclength, 'target_arclength': target_arclength } navigation_states = np.array([ self._last_navi_state_dict[state] for state in Vessel.NAVIGATION_FEATURES ]) # Deciding if vessel has reached the goal goal_distance = linalg.norm(path.end - self.position) reached_goal = goal_distance <= self.config[ "min_goal_distance"] or progress >= self.config["min_path_progress"] self._reached_goal = reached_goal return navigation_states
class FilmScenario(RealWorldEnv): def __init__(self, *args, **kw): self.x0, self.y0 = 0, 0 self.vessel_data_path = None self._rewarder_class = ColregRewarder self.n_vessels = 999999 super().__init__(*args, **kw) 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)