def compute_objective_latlon(self,
input,
theta_i,
ang_vel,
x_targ,
x_curr,
y_targ,
y_curr,
v_i,
v_cx,
v_cy,
t=1):
a = input[0]
alpha = input[1]
theta = theta_i + ang_vel * t + .5 * alpha * (t**2)
# delta_x = x_targ - x_curr
dx_vel = -(v_i * t + .5 * a * (t**2)) * np.sin(np.deg2rad(theta))
dx_curr = -v_cx * t
dx_total = dx_vel - dx_curr
# delta_y = y_targ - y_curr
dy_vel = -(v_i * t + .5 * a * (t**2)) * np.cos(np.deg2rad(theta))
dy_curr = -v_cy * t
dy_total = dy_vel - dy_curr
out = LatLon.dist(LatLon(y_targ, x_targ),
LatLon(y_curr, x_curr).add_dist(dx_total,
dy_total))**2
return out
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 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
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.running_dist_err = 0
self.running_angle_err = 0
delta_x, delta_y = self.get_distances(waypoint, boat_x, boat_y)
return self.control(boat_angle, delta_x, delta_y, boat_speed,
boat_ang_vel)
def compute_objective_integrated(self,
input,
theta_i,
ang_vel,
x_targ,
x_curr,
y_targ,
y_curr,
v_i,
v_cx,
v_cy,
t=1):
a = input[0]
alpha = input[1]
def theta(time):
return theta_i + ang_vel * time + .5 * alpha * (time**2)
# handle x first
def delta_x(time):
return -(v_i + a * time) * np.sin(np.deg2rad(theta(time))) - v_cx
delta_x_tot = integrate.quad(delta_x, 0, t)[0]
def delta_y(time):
return -(v_i + a * time) * np.cos(np.deg2rad(theta(time))) - v_cy
delta_y_tot = integrate.quad(delta_y, 0, t)[0]
return LatLon.dist(
LatLon(y_curr, x_curr).add_dist(delta_x_tot, delta_y_tot),
LatLon(y_targ, x_targ)
) #+ 0.07 * np.abs(ang_vel + alpha * t) + 0.1 * np.abs(v_i + a * t)
def compute_distance_to_closest_obstacle(self, boat_latlon, obstacles):
min_dist = None
for obs in obstacles:
obs_latlon = LatLon(obs[2], obs[1])
dist = LatLon.dist(obs_latlon, boat_latlon)
if min_dist is None or dist < min_dist:
min_dist = dist
return min_dist
def compute_gains(self, state, boat_x, boat_y):
boat_pos = LatLon(self.start_lat, self.start_lon).add_dist(boat_x, boat_y)
obstacles = state[-1]
m = 0
for o in obstacles:
d = LatLon.dist(boat_pos, LatLon(o[2], o[1]))
if d < self.clearance + o[0]:
print(f"too close: {d - o[0]}")
return (2*self.clearance/(d - o[0]))*self.p_scale
return self.p_scale
def waypoint_is_valid(self, x, y, r):
obs_latlon = xy_to_latlon(x, y)
if LatLon.dist(self.boat_coords, obs_latlon) < 2:
return False
for w in self.waypoints:
if LatLon.dist(
w, obs_latlon) < r * (SCREEN_WIDTH_M / SCREEN_WIDTH) + 2:
return False
return True
def check_intersecting(self, x, y, state):
"""
Checks if a point x and y away from LatLon(self.start_lat, self.start_lon)
intersects with any objects
"""
obstacles = state[-1]
test_pos = LatLon(self.start_lat, self.start_lon).add_dist(x, y)
for obs in obstacles:
obs_pos = (obs[1], obs[2])
if LatLon.dist(LatLon(obs_pos[1], obs_pos[0]), test_pos) < obs[0] + self.clearance:
return True
return False
def estimate_currents_theoretical(self, state, print_current_info=False):
if self.last_state is None:
return 0, 0
boat_x, boat_y, _, boat_speed, boat_angle, boat_ang_vel, ocean_current_x, ocean_current_y, obstacles = state
old_boat_x, old_boat_y, _, old_boat_speed, old_boat_angle, old_boat_ang_vel, old_ocean_current_x, old_ocean_current_y, old_obstacles = self.last_state
# where should we have gone?
t = 1 / 60
def theta(time):
return old_boat_angle + old_boat_ang_vel * time + .5 * self.last_alpha * (
time**2)
# handle x first
def delta_x(time):
return -(old_boat_speed + self.last_a * time) * np.sin(
np.deg2rad(theta(time)))
delta_x_tot = integrate.quad(delta_x, 0, t)[0]
def delta_y(time):
return -(old_boat_speed + self.last_a * time) * np.cos(
np.deg2rad(theta(time)))
delta_y_tot = integrate.quad(delta_y, 0, t)[0]
predicted = LatLon(old_boat_y,
old_boat_x).add_dist(delta_x_tot, delta_y_tot)
err_x = LatLon.dist(predicted, LatLon(predicted.lat, boat_x))
if predicted.lon > boat_x:
err_x *= -1
err_y = LatLon.dist(predicted, LatLon(boat_y, predicted.lon))
if predicted.lat > boat_y:
err_y *= -1
curr_x, curr_y = err_x / t, err_y / t
# curr_x, curr_y = 0 / t, 0 / t
if print_current_info:
print(
f"estimate: {(curr_x, curr_y)}, truth: {(ocean_current_x, ocean_current_y)}, err: {(100*(curr_x - ocean_current_x), 100*(curr_y - ocean_current_y))}"
)
return curr_x, curr_y
def compute_objective_theoretical(self,
input,
currPos,
targPos,
theta_i,
ang_vel,
v_i,
v_cx,
v_cy,
t=1):
a = input[0]
alpha = input[1]
def theta(time):
return theta_i + ang_vel * time + .5 * alpha * (time**2)
# handle x first
def delta_x(time):
return -(v_i + a * time) * np.sin(np.deg2rad(theta(time))) - v_cx
delta_x_tot = integrate.quad(delta_x, 0, t)[0]
def delta_y(time):
return -(v_i + a * time) * np.cos(np.deg2rad(theta(time))) - v_cy
delta_y_tot = integrate.quad(delta_y, 0, t)[0]
final_ang_vel = ang_vel + alpha * t
final_speed = v_i + a * t
return LatLon.dist(
currPos.add_dist(delta_x_tot, delta_y_tot),
targPos)**2 + self.ang_vel_penalty * np.abs(
final_ang_vel) + self.vel_penalty * np.abs(final_speed)
def estimate_currents(self, env, state):
# a = 1
# b = -(2*ocean_current_x*np.sin(np.deg2rad(boat_angle)) + 2*ocean_current_y*np.cos(np.deg2rad(boat_angle)))
# c = ocean_current_x**2 + ocean_current_y**2 - (VEL_SCALE**2)*(boat_speed**2)
#
# boat_dx = VEL_SCALE * boat_speed * np.sin(np.pi * boat_angle / 180)
# boat_dy = VEL_SCALE * boat_speed * np.cos(np.pi * boat_angle / 180)
#
# ocean_current_x /= VEL_SCALE
# ocean_current_y /= VEL_SCALE
#
# # print(b**2 - 4*a*c)
# boat_speed = (-b + np.sqrt(max(b**2 - 4*a*c, 0))) / (2*a)
# boat_speed /= VEL_SCALE
#
# intended_boat_dx = VEL_SCALE * boat_speed * np.sin(np.pi * boat_angle / 180)
# intended_boat_dy = VEL_SCALE * boat_speed * np.cos(np.pi * boat_angle / 180)
#
# projection = (intended_boat_dx * boat_dx + intended_boat_dy * boat_dy) / (VEL_SCALE * boat_speed)
# if projection < 0:
# boat_speed *= -1
boat_lon, boat_lat = state[0], state[1]
ocean_current_x, ocean_current_y = env.compute_ocean_current(
LatLon(boat_lat, boat_lon))
return ocean_current_x, ocean_current_y
def latlon_to_xy(pos):
left_point = LatLon(pos.lat, TOP_LEFT_LATLON.lon)
top_point = LatLon(TOP_LEFT_LATLON.lat, pos.lon)
x = PIXELS_PER_METER * LatLon.dist(pos, left_point)
y = PIXELS_PER_METER * LatLon.dist(pos, top_point)
# LatLon.dist always returns positive values, so based on whether pos is
# above or below the top left, change sign
if pos.lat < TOP_LEFT_LATLON.lat:
y *= -1
if pos.lon < TOP_LEFT_LATLON.lon:
x *= -1
return (x, y)
def format_state(args, state, env):
if args.state_mode == "sensor":
return state
boat_x, boat_y, boat_speed, boat_angle, boat_ang_vel, obstacles = state
currents = env.compute_ocean_current(LatLon(boat_y, boat_x))
return boat_x, boat_y, boat_speed, env.speed, boat_angle, boat_ang_vel, currents[
0], currents[1], obstacles
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 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 compute_objective_simple(self,
input,
theta_i,
ang_vel,
x_targ,
x_curr,
y_targ,
y_curr,
v_i,
v_cx,
v_cy,
t=1):
a = input[0]
alpha = input[1]
targ = LatLon(y_targ, x_targ)
curr = LatLon(y_curr, x_curr)
theta_i %= 360
if theta_i > 180:
theta_i = -1 * (360 - theta_i)
delta_x = LatLon.dist(LatLon(y_curr, x_curr), LatLon(y_curr, x_targ))
if x_targ > x_curr:
delta_x *= -1
delta_y = LatLon.dist(LatLon(y_curr, x_curr), LatLon(y_targ, x_curr))
if y_targ > y_curr:
delta_y *= -1
dist = LatLon.dist(curr, targ)
target_heading = np.rad2deg(np.arctan2(delta_x + v_cx, delta_y + v_cy))
k1 = 0.75
accel_error = (np.sqrt(v_cx**2 + v_cy**2) + v_i + a * t - k1 * dist)**2
k2 = 1
diff = (target_heading - theta_i)
diff2 = (target_heading + 180 - theta_i) % 360
#
# if np.abs(diff2) < np.abs(diff):
# accel_error = (np.sqrt(v_cx**2 + v_cy**2) + v_i + a * t + k1*dist)**2
# diff = diff2
heading_error = (ang_vel + alpha * t - k2 * diff)**2
return accel_error + heading_error
def check_intersecting(self, x, y, state):
"""
Checks if a point x and y away from LatLon(self.start_y, self.start_x)
intersects with any objects
"""
boat_x, boat_y, obstacles = state[0], state[1], state[-1]
test_pos = LatLon(self.start_y, self.start_x).add_dist(x, y)
for obs in obstacles:
obs_pos = (obs[1], obs[2])
# delta_x, delta_y = self.get_distances(obs_pos, test_pos.lon, test_pos.lat)
# print(delta_x, delta_y)
if LatLon.dist(LatLon(obs_pos[1], obs_pos[0]),
test_pos) < obs[0] + 2 * BOAT_HEIGHT:
# print("returned true")
return True
return False
def new_control(self, theta_i, ang_vel, x_targ, x_curr, y_targ, y_curr,
v_i, v_cx, v_cy):
currPos = LatLon(y_curr, x_curr)
targPos = LatLon(y_targ, x_targ)
dist = LatLon.dist(currPos, targPos)
obj_fun = self.compute_objective
bounds = Bounds([-self.a_max, -self.max_alpha_mag],
[self.a_max, self.max_alpha_mag])
t = 1.0
delta = 0
penalty = 0
if self.last_dist is not None:
delta = abs(dist) - abs(self.last_dist)
self.accumulator += np.exp(-1600 * delta**2) * self.a_rate
penalty = self.a_constant * self.accumulator
t = max(t - penalty, 1e-3)
guess = self.initial_control(theta_i, ang_vel, x_targ, x_curr, y_targ,
y_curr, v_i, v_cx, v_cy, t)
guess = np.array([self.last_a, self.last_alpha])
solved = minimize(
obj_fun,
guess, (currPos, targPos, theta_i, ang_vel, v_i, v_cx, v_cy, t),
method='slsqp',
bounds=bounds,
options={'maxiter': 200})
out = solved.x
# out = guess
if self.print_info:
print(
f"dist: {round(LatLon.dist(currPos, targPos), 5)}, curr_vel: {round(v_i, 5)}, accel: {round(out[0], 5)}, alpha: {round(out[1], 5)}, init alpha: {guess[1]}, t: {round(t, 5)}"
)
return out
def compute_objective(self, input, theta_i, ang_vel, x_targ, x_curr, y_targ, y_curr, v_i, v_cx, v_cy, t=1):
# t = max(t - self.i_constant * self.running_error, 1e-3)
a = input[0]
alpha = input[1]
theta = theta_i + ang_vel * t + .5 * alpha * (t**2)
# delta_x = x_targ - x_curr
delta_x = LatLon.dist(LatLon(y_curr, x_curr), LatLon(y_curr, x_targ))
if x_targ < x_curr:
delta_x *= -1
dx_vel = (v_i * t + .5 * a *( t ** 2)) * np.sin(np.deg2rad(theta))
dx_curr = v_cx * t
dx_total = delta_x + dx_vel - dx_curr
# delta_y = y_targ - y_curr
delta_y = LatLon.dist(LatLon(y_curr, x_curr), LatLon(y_targ, x_curr))
if y_targ < y_curr:
delta_y *= -1
dy_vel = (v_i * t + .5 * a * (t ** 2)) * np.cos(np.deg2rad(theta))
dy_curr = v_cy * t
dy_total = delta_y + dy_vel - dy_curr
return (dx_total) ** 2 + (dy_total) ** 2
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 estimate_currents(self, state, print_current_info=False):
if self.last_state is None:
return 0, 0
boat_x, boat_y, _, boat_speed, boat_angle, boat_ang_vel, ocean_current_x, ocean_current_y, obstacles = state
old_boat_x, old_boat_y, _, old_boat_speed, old_boat_angle, old_boat_ang_vel, old_ocean_current_x, old_ocean_current_y, old_obstacles = self.last_state
# where should we have gone?
t = 1 / 60
theta_final = old_boat_angle + old_boat_ang_vel * t + .5 * self.last_alpha * (
t**2)
delta_x_tot = -(0.5 * self.last_a * t**2 +
old_boat_speed * t) * np.sin(np.deg2rad(theta_final))
delta_y_tot = -(0.5 * self.last_a * t**2 +
old_boat_speed * t) * np.cos(np.deg2rad(theta_final))
predicted = LatLon(old_boat_y,
old_boat_x).add_dist(delta_x_tot, delta_y_tot)
err_x = LatLon.dist(predicted, LatLon(predicted.lat, boat_x))
if predicted.lon > boat_x:
err_x *= -1
err_y = LatLon.dist(predicted, LatLon(boat_y, predicted.lon))
if predicted.lat > boat_y:
err_y *= -1
curr_x, curr_y = err_x / t, err_y / t
# curr_x, curr_y = 0 / t, 0 / t
if print_current_info:
print(
f"estimate: {(curr_x, curr_y)}, truth: {(ocean_current_x, ocean_current_y)}, err: {(100*(curr_x - ocean_current_x), 100*(curr_y - ocean_current_y))}"
)
return curr_x, curr_y
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):
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 new_control(self, theta_i, ang_vel, x_targ, x_curr, y_targ, y_curr, v_i, v_cx, v_cy):
# obj_fun = self.compute_objective_func(theta_i, ang_vel, x_targ, x_curr, y_targ, y_curr, v_i, v_cx, v_cy)
obj_fun = self.compute_objective
dist = LatLon.dist(LatLon(y_curr, x_curr), LatLon(y_targ, x_targ))
angle = np.arctan2(x_curr - x_targ, y_curr - y_targ) * 180 / np.pi
alpha_init = self.compute_angular_accel(ang_vel, theta_i, angle)
accel_init = self.compute_accel(dist, v_i, theta_i, angle)
# print("before dist")
# print(self.compute_objective(np.array([accel_init, alpha_init]),
# theta_i, ang_vel, x_targ, x_curr, y_targ, y_curr, v_i, v_cx, v_cy))
bounds = Bounds([-self.a_max, -self.max_alpha_mag], [self.a_max, self.max_alpha_mag])
solved = minimize(obj_fun, np.array([accel_init, alpha_init]),
(theta_i, ang_vel, x_targ, x_curr, y_targ, y_curr, v_i, v_cx, v_cy), method='trust-constr', bounds=bounds,
options={'maxiter': 3})
# print("solved fun")
# print(solved.fun)
x = solved.x
# print(f"before accel: {x[0]}, before alpha: {x[1]}")
solved = [np.clip(x[0], -self.a_max, self.a_max), np.clip(x[1], -self.max_alpha_mag, self.max_alpha_mag)]
print(f"dist: {round(dist, 5)}, curr_vel: {round(v_i, 5)}, accel: {round(solved[0], 5)}, alpha: {round(solved[1], 5)}, running_error: {round(self.running_error, 5)}")
logging_dict = dict(zip("theta_i, ang_vel, x_targ, x_curr, y_targ, y_curr, v_i, v_cx, v_cy".split(', '), [theta_i, ang_vel, x_targ, x_curr, y_targ, y_curr, v_i, v_cx, v_cy]))
logging_dict["accel_init"] = accel_init
logging_dict["accel_init"] = alpha_init
logging_dict["accel_solved"] = solved[0]
logging_dict["alpha_solved"] = solved[1]
self.df = self.df.append(logging_dict, ignore_index=True)
return solved # (accel, alpha)
def format_state(state, env):
boat_x, boat_y, boat_speed, boat_angle, boat_ang_vel, obstacles = state
currents = env.compute_ocean_current(LatLon(boat_y, boat_x))
out_dict = {
"state": {
"lat": boat_y,
"lon": boat_x,
"speed": boat_speed,
"angle": boat_angle,
"ang_vel": boat_ang_vel,
"ocean_current_x": currents[0],
"ocean_current_y": currents[1],
"desired_speed": env.speed,
"obstacles": obstacles
}
}
return json.dumps(out_dict)
def get_distances(self, waypoint, boat_x, boat_y):
x_targ, y_targ = waypoint[0], waypoint[1]
x_curr, y_curr = boat_x, boat_y
delta_x = LatLon.dist(LatLon(y_targ, x_curr), LatLon(y_targ, x_targ))
if x_targ < x_curr:
delta_x *= -1
delta_y = LatLon.dist(LatLon(y_curr, x_targ), LatLon(y_targ, x_targ))
if y_targ < y_curr:
delta_y *= -1
return delta_x, delta_y
def get_distances(self, waypoint, boat_x, boat_y):
"""
Given a LatLon waypoint, a lon boat_x, and a lat boat_y, outputs
the delta x and delta y needed to get to the waypoint (signed and in meters)
"""
x_targ, y_targ = waypoint[0], waypoint[1]
x_curr, y_curr = boat_x, boat_y
delta_x = LatLon.dist(LatLon(y_targ, x_curr), LatLon(y_targ, x_targ))
if x_targ < x_curr:
delta_x *= -1
delta_y = LatLon.dist(LatLon(y_curr, x_targ), LatLon(y_targ, x_targ))
if y_targ < y_curr:
delta_y *= -1
return delta_x, delta_y
def compute_objective(self,
input,
currPos,
targPos,
theta_i,
ang_vel,
v_i,
v_cx,
v_cy,
t=1):
a = input[0]
alpha = input[1]
theta_final = theta_i + ang_vel * t + .5 * alpha * (t**2)
x_curr, y_curr = currPos.lon, currPos.lat
x_targ, y_targ = targPos.lon, targPos.lat
delta_x = LatLon.dist(LatLon(y_targ, x_curr), LatLon(y_targ, x_targ))
if x_targ > x_curr:
delta_x *= -1
delta_y = LatLon.dist(LatLon(y_curr, x_targ), LatLon(y_targ, x_targ))
if y_targ > y_curr:
delta_y *= -1
delta_x_tot = -(0.5 * a * t**2 + v_i * t) * np.sin(
np.deg2rad(theta_final)) + v_cx * t
delta_y_tot = -(0.5 * a * t**2 + v_i * t) * np.cos(
np.deg2rad(theta_final)) + v_cy * t
final_ang_vel = ang_vel + alpha * t
final_speed = v_i + a * t
return (delta_x + delta_x_tot)**2 + (
delta_y + delta_y_tot)**2 + self.ang_vel_penalty * np.abs(
final_ang_vel) + self.vel_penalty * np.abs(final_speed)
def format_state(state, env):
boat_x, boat_y, boat_speed, boat_angle, boat_ang_vel, obstacles = state
currents = env.compute_ocean_current(LatLon(boat_y, boat_x))
return boat_x, boat_y, boat_speed, env.speed, boat_angle, boat_ang_vel, currents[0], currents[1], obstacles
import pygame
import sys
import numpy as np
from boat_simulation.latlon import LatLon
from boat_simulation.simulation_sprites import *
SCREEN_WIDTH = 800
SCREEN_HEIGHT = 600
SCREEN_WIDTH_M = 20 # meters
SCREEN_HEIGHT_M = (SCREEN_WIDTH_M / SCREEN_WIDTH) * SCREEN_HEIGHT # meters
# change in latitude is change in y, change in longitude is change in x
TOP_LEFT_LATLON = LatLon(7.399640, 134.457619)
BOT_RIGHT_LATLON = TOP_LEFT_LATLON.add_dist(SCREEN_WIDTH_M, SCREEN_HEIGHT_M)
PIXELS_PER_METER = SCREEN_WIDTH / SCREEN_WIDTH_M
BOAT_WIDTH = 0.5 # meters
BOAT_HEIGHT = 1 # meters
BOAT_MASS = 5 # kg
VEL_SCALE = 1 / 60
ANGLE_SCALE = 1 / 60
def latlon_to_xy(pos):
left_point = LatLon(pos.lat, TOP_LEFT_LATLON.lon)
top_point = LatLon(TOP_LEFT_LATLON.lat, pos.lon)
x = PIXELS_PER_METER * LatLon.dist(pos, left_point)
