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 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 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 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)
