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 _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):
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):
print('In GENERATE in MA')
self.main_vessel = Vessel(self.config,
width=self.config["vessel_width"])
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
print(f'Ownvessel created!')
self.moving_obstacles = [self.main_vessel]
self.static_obstacles = []
self._vessel_count = 1
#Adding moving obstacles (vessels)
curr_vessel_count = self._vessel_count
for i in range(curr_vessel_count, curr_vessel_count + MAX_VESSELS - 1):
vessel = Vessel(self.config,
width=self.config["vessel_width"],
index=i,
vessel_pos=self.main_vessel.position)
self.rewarder_dict[vessel.index] = ColavRewarder(vessel)
self.moving_obstacles.append(vessel)
print(f'vessel {i} has been created')
self._vessel_count += 1
for vessel in self.moving_obstacles:
other_vessels = [
x for x in self.moving_obstacles if x.index != vessel.index
]
vessel.obstacles = np.hstack(
[self.static_obstacles, other_vessels])
#Adding static obstacles
for _ in range(8):
obstacle = CircularObstacle(*helpers.generate_obstacle(
self.rng, self.path, self.vessel, displacement_dist_std=500))
self.static_obstacles.append(obstacle)
print('Exiting GENERATE in MA')
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 plot_actions(env,
agent,
fig_dir,
fig_prefix='',
N=500,
creategifs=True,
createpdfs=True):
env.vessel.reset([0, 0, 0])
OBST_RADIUS = 10
theta_arr = np.linspace(-np.pi / 2, np.pi / 2, N)
r_arr = np.linspace(1, 100, N)
Theta, R = np.meshgrid(theta_arr, r_arr)
surge_arr = np.zeros(Theta.shape)
steer_arr = np.zeros(Theta.shape)
for i_t, theta in enumerate(theta_arr):
for i_r, r in enumerate(r_arr):
position = (np.cos(theta) * (r + OBST_RADIUS),
np.sin(theta) * (r + OBST_RADIUS))
env.obstacles = [CircularObstacle(position, OBST_RADIUS)]
obs = env.observe()
action, _states = agent.predict(obs, deterministic=True)
action[0] = (action[0] + 1) / 2
surge = env.vessel._surge(action[0])
steer = 180 / np.pi * env.vessel._steer(action[1])
steer_arr[i_r, i_t] = steer
surge_arr[i_r, i_t] = surge
sys.stdout.write('Plotting progress: {:.2%}\r'.format(
(i_t * N + i_r + 1) / N**2))
sys.stdout.flush()
print('\t' * 10)
np.save(os.path.join(fig_dir, 'Theta'), Theta)
np.save(os.path.join(fig_dir, 'R'), R)
np.save(os.path.join(fig_dir, 'steer_arr'), steer_arr)
np.save(os.path.join(fig_dir, 'surge_arr'), steer_arr)
env.close()
if creategifs:
fig = plt.figure()
ax = fig.gca(projection='3d')
steer_surf = ax.plot_surface(Theta,
R,
steer_arr,
cmap=cm.coolwarm,
linewidth=0,
antialiased=False)
ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f'))
ax.set_ylabel(r"Obstacle Distance")
ax.set_xlabel(r"Relative Obstacle Angle")
ax.xaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.1f}°'.format(y * 180 / np.pi)))
ax.zaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.1f}°'.format(y)))
fig.colorbar(steer_surf, shrink=0.5, aspect=5)
print('Steering: Creating gif')
def rotate(angle):
ax.view_init(azim=angle)
rot_animation = animation.FuncAnimation(fig,
rotate,
frames=np.arange(0, 362, 2),
interval=100)
rot_animation.save(os.path.join(
fig_dir, fig_prefix + 'obst_avoidance_steer.gif'),
dpi=80,
writer='imagemagick')
fig = plt.figure()
ax = fig.gca(projection='3d')
surge_surf = ax.plot_surface(Theta,
R,
surge_arr,
cmap=cm.coolwarm,
linewidth=0,
antialiased=False)
ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f'))
ax.set_ylabel(r"Obstacle Distance")
ax.set_xlabel(r"Relative Obstacle Angle")
ax.xaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.1f}°'.format(y * 180 / np.pi)))
fig.colorbar(surge_surf, shrink=0.5, aspect=5)
print('Surge: Creating gif')
def rotate(angle):
ax.view_init(azim=angle)
rot_animation = animation.FuncAnimation(fig,
rotate,
frames=np.arange(0, 362, 2),
interval=100)
rot_animation.save(os.path.join(
fig_dir, fig_prefix + 'obst_avoidance_surge.gif'),
dpi=80,
writer='imagemagick')
if createpdfs:
fig = plt.figure()
ax = fig.add_subplot(111)
surge_plot = ax.contourf(Theta,
R,
surge_arr,
levels=20,
cmap='coolwarm')
cbar = fig.colorbar(surge_plot)
cbar.set_label(r"Propeller thrust force [N]")
ax.set_ylabel(r"Obstacle Distance")
ax.set_xlabel(r"Relative Obstacle Angle")
ax.xaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.1f}°'.format(y * 180 / np.pi)))
ax.yaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.0f} m'.format(y)))
plt.savefig(os.path.join(fig_dir, fig_prefix +
'policy_plot_thrust_contour.pdf'),
bbox_inches='tight')
fig = plt.figure()
ax = fig.add_subplot(111)
ax = fig.gca(projection='3d')
ax.plot_surface(Theta, R, surge_arr, cmap='coolwarm')
ax.set_ylabel(r"Obstacle Distance")
ax.set_xlabel(r"Relative Obstacle Angle")
ax.set_zlabel(r"Propeller thrust force")
ax.yaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.0f}m'.format(y)))
ax.xaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.0f}°'.format(y * 180 / np.pi)))
ax.zaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.2f}N'.format(y)))
plt.savefig(os.path.join(fig_dir, fig_prefix +
'policy_plot_thrust_surface.pdf'),
bbox_inches='tight')
fig = plt.figure()
ax = fig.add_subplot(111)
steering_plot = ax.contourf(Theta,
R,
steer_arr,
levels=20,
cmap='coolwarm')
cbar = fig.colorbar(steering_plot)
cbar.set_label(r"Rudder angle [deg]")
ax.set_ylabel(r"Obstacle Distance")
ax.set_xlabel(r"Relative Obstacle Angle")
ax.xaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.1f}°'.format(y * 180 / np.pi)))
ax.yaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.0f} m'.format(y)))
plt.savefig(os.path.join(
fig_dir, fig_prefix + 'policy_plot_steering_contour.pdf'),
bbox_inches='tight')
fig = plt.figure()
ax = fig.add_subplot(111)
ax = fig.gca(projection='3d')
ax.plot_surface(Theta, R, steer_arr, cmap='coolwarm')
ax.set_ylabel(r"Obstacle Distance")
ax.set_xlabel(r"Relative Obstacle Theta")
ax.set_zlabel(r"Rudder angle")
ax.yaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.0f}m'.format(y)))
ax.xaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.0f}°'.format(y * 180 / np.pi)))
ax.zaxis.set_major_formatter(
FuncFormatter(lambda y, _: '{:.0f}°'.format(y * 180 / np.pi)))
plt.savefig(os.path.join(
fig_dir, fig_prefix + 'policy_plot_steering_surface.pdf'),
bbox_inches='tight')