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