def select_action_from_state(self, env, state):
if self.in_sim:
env.set_waypoint(self.curr_waypoint)
if env.total_time < 1:
return Action(0, 0)
boat_x, boat_y, boat_speed, boat_angle, boat_ang_vel, obstacles = state
boat_speed = env.speed
waypoint = [
env.waypoints[self.curr_waypoint].lon,
env.waypoints[self.curr_waypoint].lat
]
dist = LatLon.dist(env.boat_coords, env.waypoints[self.curr_waypoint])
if abs(dist) < 0.05:
self.curr_waypoint = (self.curr_waypoint + 1) % len(env.waypoints)
self.running_error = 0
self.accumulator = 0
self.last_dist = 0
self.running_error += abs(dist)
ocean_current_x, ocean_current_y = self.estimate_currents(env, state)
control = self.new_control(boat_angle, boat_ang_vel, waypoint[0],
boat_x, waypoint[1], boat_y, boat_speed,
ocean_current_x, ocean_current_y, True)
self.last_dist = dist
self.last_alpha = control[1]
self.last_accel = control[0]
return Action(2, [control[1], control[0]])
def control(self, boat_angle, delta_x, delta_y, boat_speed, boat_ang_vel, gains):
"""
Get controls needed to steer boat to a particular point.
This function is used to follow the path planned using A*
"""
boat_angle_deg = np.deg2rad(boat_angle)
R = np.array([ [-np.sin(boat_angle_deg), np.cos(boat_angle_deg) , 0],
[-np.cos(boat_angle_deg), -np.sin(boat_angle_deg), 0],
[0, 0, 1]]).reshape((3, 3))
R_inv = R.T
angle = self.get_required_angle_change(boat_angle, delta_x, delta_y)
eta_d = np.array([delta_x, delta_y, angle]).reshape((3, 1))
targ_v = gains * np.matmul(R_inv, eta_d)
curr_v = np.array([boat_speed, 0, boat_ang_vel]).reshape((3, 1))
diff_v = targ_v - curr_v
control = gains * diff_v
control[2][0] = np.rad2deg(control[2][0])
control = np.clip(control, np.array([-self.a_max, 0, -self.max_alpha_mag]).reshape(3, 1), np.array([self.a_max, 0, self.max_alpha_mag]).reshape(3, 1))
dist = np.sqrt((delta_x ** 2) + (delta_y ** 2))
# print(f"self.running_dist_err: {self.running_dist_err}, self.running_angle_err: {self.running_angle_err}")
if self.print_info:
print(f"dist: {round(dist, 5)}, curr_vel: {round(boat_speed, 5)}, accel: {round(control[0][0], 5)}, alpha: {round(control[2][0], 5)}")
return Action(2, [control[2][0], control[0][0]])
def select_action_from_state(self, env, state):
if self.in_sim:
env.set_waypoint(self.curr_waypoint)
# if env.total_time < 1:
# # return Action(0, self.a_max)
# return Action(0, 0)
boat_x, boat_y, boat_speed, desired_speed, boat_angle, boat_ang_vel, ocean_current_x, ocean_current_y, obstacles = state
waypoint = [
env.waypoints[self.curr_waypoint].lon,
env.waypoints[self.curr_waypoint].lat
]
dist = LatLon.dist(LatLon(boat_y, boat_x),
LatLon(waypoint[1], waypoint[0]))
if abs(dist) < 0.05 and abs(boat_speed) < 5:
self.curr_waypoint = (self.curr_waypoint + 1) % len(env.waypoints)
self.running_error = 0
self.running_error += abs(dist)
angle = np.arctan2(boat_x - waypoint[0],
boat_y - waypoint[1]) * 180 / np.pi
alpha = self.compute_angular_accel(boat_ang_vel, boat_angle, angle)
accel = self.compute_accel(dist, boat_speed, boat_angle, angle)
return Action(2, [alpha, accel])
def simulation(args, controller_conn):
"""
Simulates movements of the boat.
This function is to be executed in the process simulates the boat. It creates
an instance of the SimpleBoatSim class, repeatedly publishes state
information and receives actions taken by the boat.
"""
env = SimpleBoatSim(current_level=int(args.current_level), state_mode=args.state_mode, max_obstacles=int(args.max_obstacles), apply_drag_forces=(not bool(args.no_drag)))
state = env.reset()
env.set_waypoints(controller_conn.recv())
controller_conn.send(format_state(state, env))
while True:
events = pygame.event.get()
to_send = False
if controller_conn.poll():
action = controller_conn.recv()
to_send = True
else:
action = Action(0, 0)
state, _, end_sim, _ = env.step(action)
if to_send:
controller_conn.send(format_state(state, env))
if not args.no_render:
env.render()
if end_sim:
# This can be replaced with env.close() to end the simulation.
state = env.reset()
def control(self, boat_angle, delta_x, delta_y, boat_speed, boat_ang_vel):
boat_angle_deg = np.deg2rad(boat_angle)
R = np.array([[-np.sin(boat_angle_deg),
np.cos(boat_angle_deg), 0],
[-np.cos(boat_angle_deg), -np.sin(boat_angle_deg), 0],
[0, 0, 1]]).reshape((3, 3))
R_inv = R.T
angle = self.get_required_angle_change(boat_angle, delta_x, delta_y)
err_vec = np.array([self.running_dist_err, 0,
self.running_angle_err]).reshape((3, 1))
# print(err_vec)
eta_d = np.array([delta_x, delta_y, angle]).reshape((3, 1))
targ_v = (self.p_scale * np.matmul(R_inv, eta_d)) + (self.i_scale *
err_vec)
curr_v = np.array([boat_speed, 0, boat_ang_vel]).reshape((3, 1))
diff_v = targ_v - curr_v
control = self.p_scale * diff_v
control[2][0] = np.rad2deg(control[2][0])
control = np.clip(
control,
np.array([-self.a_max, 0, -self.max_alpha_mag]).reshape(3, 1),
np.array([self.a_max, 0, self.max_alpha_mag]).reshape(3, 1))
dist = np.sqrt((delta_x**2) + (delta_y**2))
if self.last_dist is not None and self.last_angle is not None:
delta = abs(dist) - abs(self.last_dist)
delta_angle = abs(angle) - abs(self.last_angle)
d_increment = np.exp(-1600 * delta**2) * dist
a_increment = np.exp(-1600 * delta_angle**2) * angle
raw_angle = (np.arctan2(-delta_x, -delta_y) * 180 /
np.pi) - (boat_angle)
raw_angle %= 360
raw_angle = min(raw_angle, raw_angle - 360, key=abs)
if abs(raw_angle) < 90:
self.running_dist_err += d_increment
else:
self.running_dist_err -= d_increment
self.running_angle_err += a_increment
self.last_dist = dist
self.last_angle = angle
# print(f"self.running_dist_err: {self.running_dist_err}, self.running_angle_err: {self.running_angle_err}")
if self.print_info:
print(
f"dist: {round(dist, 5)}, curr_vel: {round(boat_speed, 5)}, accel: {round(control[0][0], 5)}, alpha: {round(control[2][0], 5)}"
)
return Action(2, [control[2][0], control[0][0]])
def select_action_from_state(self, env, state):
"""
Main method that calculates Voronoi diagram, finds shortest path,
and outputs which actions need to be taken.
"""
if self.in_sim:
env.set_waypoint(self.curr_waypoint)
if env.total_time < 1:
self.path = []
env.voronoi_graph = None
env.voronoi_path = None
return Action(0, 0)
boat_x, boat_y, boat_speed, _, boat_angle, boat_ang_vel, ocean_current_x, ocean_current_y, obstacles = state
boat_latlon = LatLon(boat_y, boat_x)
waypoint_laton = env.waypoints[self.curr_waypoint]
dist = LatLon.dist(boat_latlon, waypoint_laton)
if dist < 0.05:
self.curr_waypoint = (self.curr_waypoint + 1) % len(env.waypoints)
self.path = None
return Action(0, 0)
self.voronoi_graph = self.compute_voronoi(obstacles, boat_latlon,
waypoint_laton)
_, self.path = self.compute_shortest_path(self.voronoi_graph)
env.voronoi_graph = self.voronoi_graph
env.voronoi_path = self.path
if self.path is None:
return Action(0, 0)
target_point = self.voronoi_graph.points[self.path[1]]
boat_xy = list(latlon_to_xy(boat_latlon))
min_dist_to_obs = self.compute_distance_to_closest_obstacle(
boat_latlon, obstacles)
return self.control(boat_angle, target_point[0] - boat_xy[0],
target_point[1] - boat_xy[1], boat_speed,
boat_ang_vel, min_dist_to_obs)
def get_user_input(self, env):
for event in pygame.event.get():
if event.type == KEYDOWN:
if event.key == K_ESCAPE or event.type == QUIT:
env.close()
if event.key == K_UP:
return Action(0, 40)
if event.key == K_DOWN:
return Action(0, -40)
if event.key == K_LEFT:
return Action(1, 60)
if event.key == K_RIGHT:
return Action(1, -60)
if event.type == QUIT:
env.close()
return Action(0, 0)
def select_action_from_state(self, env, state):
if self.in_sim:
env.set_waypoint(self.curr_waypoint)
boat_x, boat_y, boat_speed, _, boat_angle, boat_ang_vel, ocean_current_x, ocean_current_y, obstacles = state
waypoint = [
env.waypoints[self.curr_waypoint].lon,
env.waypoints[self.curr_waypoint].lat
]
dist = LatLon.dist(LatLon(boat_y, boat_x),
LatLon(waypoint[1], waypoint[0]))
if abs(dist) < 0.05:
self.curr_waypoint = (self.curr_waypoint + 1) % len(env.waypoints)
delta_x, delta_y = self.get_distances(waypoint, boat_x, boat_y)
angle = self.get_required_angle_change(boat_angle, delta_x, delta_y)
angle = angle % 360
print(min(angle, angle - 360, key=abs))
# Accelerating by specified values for one frame
for event in pygame.event.get():
if event.type == KEYDOWN:
if event.key == K_ESCAPE or event.type == QUIT:
env.close()
if event.key == K_UP:
return Action(0, 60)
if event.key == K_DOWN:
return Action(0, -60)
if event.key == K_LEFT:
return Action(1, 60 * 60)
if event.key == K_RIGHT:
return Action(1, -60 * 60)
if event.type == QUIT:
env.close()
return Action(0, 0)
def select_action_from_state(self, env, state):
if self.in_sim:
env.set_waypoint(self.curr_waypoint)
if env.total_time < 1:
return Action(0, 0)
boat_x, boat_y, boat_speed, boat_angle, boat_ang_vel, obstacles = state
boat_speed = env.speed
waypoint = [env.waypoints[self.curr_waypoint].lon, env.waypoints[self.curr_waypoint].lat]
dist = LatLon.dist(env.boat_coords, env.waypoints[self.curr_waypoint])
if abs(dist) < 0.25:
self.curr_waypoint = (self.curr_waypoint + 1) % len(env.waypoints)
self.running_error = 0
self.df.to_csv("logs/log.csv")
sys.exit(0)
#
self.running_error += abs(dist)
#
# angle = np.arctan2(boat_x - waypoint[0], boat_y - waypoint[1]) * 180 / np.pi
#
# alpha = self.compute_angular_accel(boat_ang_vel, boat_angle, angle)
# accel = self.compute_accel(dist, boat_speed, boat_angle, angle)
# theta_i, ang_vel, x_targ, x_curr, y_targ, y_curr, v_i, v_cx, v_cy
boat_lon, boat_lat = state[0], state[1]
ocean_current_x, ocean_current_y = env.compute_ocean_current(LatLon(boat_y, boat_x))
# ocean_current_x = 0
# ocean_current_y = 0
control = self.new_control(boat_angle, boat_ang_vel, waypoint[0], boat_x, waypoint[1], boat_y, boat_speed, ocean_current_x, ocean_current_y)
# print("control: " + str(control))
return Action(2, [control[1], control[0]])
def select_action_from_state(self, env, state):
if self.in_sim:
env.set_waypoint(self.curr_waypoint)
# boat_x, boat_y, boat_speed, _, boat_angle, boat_ang_vel, ocean_current_x, ocean_current_y, obstacles = state
# print(state)
boat_x, boat_y, boat_speed, omega, theta_m, boat_ang_vel, obstacles = state
self.complementary_filter.update_magnetometer(theta_m)
self.complementary_filter.update_gyro(omega)
print(self.complementary_filter.get_heading())
# waypoint = [env.waypoints[self.curr_waypoint].lon, env.waypoints[self.curr_waypoint].lat]
# dist = LatLon.dist(LatLon(boat_y, boat_x), LatLon(waypoint[1], waypoint[0]))
#
# if abs(dist) < 0.05:
# self.curr_waypoint = (self.curr_waypoint + 1) % len(env.waypoints)
# Accelerating by specified values for one frame
for event in pygame.event.get():
if event.type == KEYDOWN:
if event.key == K_ESCAPE or event.type == QUIT:
env.close()
if event.key == K_UP:
return Action(0, 60)
if event.key == K_DOWN:
return Action(0, -60)
if event.key == K_LEFT:
return Action(1, 60 * 60)
if event.key == K_RIGHT:
return Action(1, -60 * 60)
if event.type == QUIT:
env.close()
return Action(0, 0)
def select_action_from_state(self, env, state):
if self.in_sim:
env.set_waypoint(self.curr_waypoint)
# if env.total_time < 1:
# return Action(0, 0)
boat_x, boat_y, boat_speed, _, boat_angle, boat_ang_vel, ocean_current_x, ocean_current_y, obstacles = state
ocean_current_x, ocean_current_y = self.estimate_currents_theoretical(
state)
waypoint = [
env.waypoints[self.curr_waypoint].lon,
env.waypoints[self.curr_waypoint].lat
]
dist = LatLon.dist(LatLon(boat_y, boat_x),
LatLon(waypoint[1], waypoint[0]))
if abs(dist) < 0.05:
self.curr_waypoint = (self.curr_waypoint + 1) % len(env.waypoints)
self.last_dist = None
self.accumulator = 0
control = self.new_control(boat_angle, boat_ang_vel, waypoint[0],
boat_x, waypoint[1], boat_y, boat_speed,
ocean_current_x, ocean_current_y)
control[0] = np.clip(control[0], -self.a_max, self.a_max)
control[1] = np.clip(control[1], -self.max_alpha_mag,
self.max_alpha_mag)
self.last_a = control[0]
self.last_alpha = control[1]
self.last_dist = dist
self.last_state = state
return Action(2, [control[1], control[0]])
def select_action_from_state(self, env, state):
return Action(0, 0)
def select_action_from_state(self, env, state):
"""
Main method that performs A* search, selects subgoal to go to,
and outputs which actions need to be taken.
"""
if self.in_sim:
env.set_waypoint(self.curr_waypoint)
if env.total_time < 1:
self.path = None
self.start_x = None
self.start_y = None
self.subgoal_idx = 0
return Action(0, 0)
boat_x, boat_y, boat_speed, _, boat_angle, boat_ang_vel, ocean_current_x, ocean_current_y, obstacles = state
waypoint = [
env.waypoints[self.curr_waypoint].lon,
env.waypoints[self.curr_waypoint].lat
]
if self.start_x is None:
self.start_x = boat_x
self.start_y = boat_y
delta_x, delta_y = self.get_distances(waypoint, self.start_x,
self.start_y)
boat_x, boat_y = self.get_distances([boat_x, boat_y], self.start_x,
self.start_y)
dist = np.sqrt((delta_x - boat_x)**2 + (delta_y - boat_y)**2)
if dist < 0.05:
self.curr_waypoint = (self.curr_waypoint + 1) % len(env.waypoints)
self.path = None
self.start_x = None
self.start_y = None
self.cmd_idx = 0
return Action(0, 0)
if self.path is None:
curr = self.accel_a_star(boat_x, boat_y, delta_x, delta_y, state)
path_rev = []
path_cstates_rev = []
while curr is not None:
path_rev.append((curr.accel, curr.alpha))
path_cstates_rev.append(curr)
curr = curr.prev
self.path = path_rev
print(self.path)
path_cstates_rev.reverse()
self.draw(delta_x, delta_y, path_cstates_rev, state, controls=True)
self.cmd_idx = 0
self.cmd_start_t = env.total_time
if env.total_time - self.cmd_start_t >= self.delta_t:
self.cmd_idx += 1
print("switching")
self.cmd_start_t = env.total_time
if self.cmd_idx < len(self.path):
control = self.path[len(self.path) - 1 - self.cmd_idx]
return Action(2, [control[1], control[0]])
return Action(0, 0)
def select_action_from_state(self, env, state):
"""
Main method that performs A* search, selects subgoal to go to,
and outputs which actions need to be taken.
"""
if self.in_sim:
env.set_waypoint(self.curr_waypoint)
# if env.total_time < 1:
# self.path = []
# self.start_lon = None
# self.start_lat = None
# self.subgoal_idx = 0
# self.last_subgoal_idx = 0
# return Action(0, 0)
boat_x, boat_y, boat_speed, _, boat_angle, boat_ang_vel, ocean_current_x, ocean_current_y, obstacles = state
waypoint = [env.waypoints[self.curr_waypoint].lon, env.waypoints[self.curr_waypoint].lat]
if self.start_lon is None:
self.start_lon = boat_x
self.start_lat = boat_y
delta_x, delta_y = self.get_distances(waypoint, self.start_lon, self.start_lat)
boat_x, boat_y = self.get_distances([boat_x, boat_y], self.start_lon, self.start_lat)
dist = np.sqrt((delta_x - boat_x)**2 + (delta_y - boat_y)**2)
if dist < 0.05:
self.curr_waypoint = (self.curr_waypoint + 1) % len(env.waypoints)
self.path = []
self.start_lon = None
self.start_lat = None
self.subgoal_idx = 0
self.last_subgoal_idx = 0
return Action(0, 0)
# new path and whether it is different from previously planned path
path_with, changed = self.a_star(boat_x, boat_y, delta_x, delta_y, state)
if changed:
self.last_subgoal_idx = 0 # 'restarting' on a new path
# if the subgoal is too close, select the one after it
pt_idx = self.select_sub_waypoint(path_with, boat_x, boat_y)
pt = (boat_x, boat_y)
if pt_idx != -1: # path planner could find a path
self.last_subgoal_idx = pt_idx
pt = path_with[pt_idx]
self.path = path_with
else: # no path found
pt = self.dodge(delta_x, delta_y, boat_x, boat_y, state)
self.last_subgoal_idx = 0
self.path = []
# print(f"path: {[self.start_lon, self.start_lat] + self.path}")
if self.replot and len(path_with) > 0 and self.in_sim:
path_to_plot = [LatLon(self.start_lat, self.start_lon).add_dist(boat_x, boat_y)] + [LatLon(self.start_lat, self.start_lon).add_dist(p[0], p[1]) for p in path_with[pt_idx:]]
env.plot_path(path_to_plot)
return self.control(boat_angle, pt[0] - boat_x, pt[1] - boat_y, boat_speed, boat_ang_vel, gains=self.compute_gains(state, boat_x, boat_y))
