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 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_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 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 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 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 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 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 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 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 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 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 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 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 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 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)) 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 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 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 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
def compute_voronoi(self, obstacles, boat_latlon, waypoint_latlon): obstacles_xy = [] for obs in obstacles: obs_latlon = LatLon(obs[2], obs[1]) obstacles_xy.append(list(latlon_to_xy(obs_latlon))) boat_xy = list(latlon_to_xy(boat_latlon)) waypoint_xy = list(latlon_to_xy(waypoint_latlon)) voronoi_points = obstacles_xy # obstacle LatLon objects voronoi_points.append( boat_xy) # boat is always at second-to-last index voronoi_points.append( waypoint_xy) # curr waypoint is always at last index if len(voronoi_points) < 4: return VoronoiGraph([], []) vor = Voronoi(voronoi_points) center = vor.points.mean(axis=0) points = vor.vertices.tolist() edges = defaultdict(dict) for i in range(len(vor.ridge_vertices)): edge = vor.ridge_vertices[i] if edge[0] >= 0 and edge[1] >= 0: start = edge[0] end = edge[1] else: # we need to approximate the unknown vertex if edge[1] < 0: # always make edge[0] the unknown vertex edge[0], edge[1] = edge[1], edge[0] between_pts = vor.ridge_points[i] p1 = between_pts[0] p2 = between_pts[1] # taken from lines 66-72 of https://gist.github.com/pv/8036995 t = vor.points[p2] - vor.points[p1] # tangent t /= np.linalg.norm(t) n = np.array([-t[1], t[0]]) # normal midpoint = vor.points[[p1, p2]].mean(axis=0) direction = np.sign(np.dot(midpoint - center, n)) * n endpt = vor.vertices[edge[1]] + direction * 1000 endpt = endpt.tolist() points.append(endpt) start = edge[1] end = len(points) - 1 d = self.dist(points[start], points[end]) edges[start][end] = d edges[end][start] = d points.append(boat_xy) points.append(waypoint_xy) # if the boat and waypoint regions border each other, add a connecting edge for edge in vor.ridge_points: p1 = edge[0] p2 = edge[1] if min(p1, p2) == len(vor.points) - 2 and max( p1, p2) == len(vor.points) - 1: d = self.dist(vor.points[p1], vor.points[p2]) edges[len(points) - 2][len(points) - 1] = d edges[len(points) - 1][len(points) - 2] = d # add edges from boat to vertices in its region for i in vor.regions[vor.point_region[len(voronoi_points) - 2]]: if i >= 0: d = self.dist(points[len(points) - 2], points[i]) edges[len(points) - 2][i] = d edges[i][len(points) - 2] = d # add edges from waypoint to vertices in its region for i in vor.regions[vor.point_region[len(voronoi_points) - 1]]: if i >= 0: d = self.dist(points[len(points) - 1], points[i]) edges[len(points) - 1][i] = d edges[i][len(points) - 1] = d return VoronoiGraph(points, edges)
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))
def new_control(self, theta_i, ang_vel, x_targ, x_curr, y_targ, y_curr, v_i, v_cx, v_cy, use_accumulator=True): 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) if self.last_alpha is None: alpha_init = self.compute_angular_accel(ang_vel, theta_i, angle) accel_init = self.compute_accel(dist, v_i, theta_i, angle) else: alpha_init = self.last_alpha accel_init = self.last_accel bounds = Bounds([-self.a_max, -self.max_alpha_mag], [self.a_max, self.max_alpha_mag]) t = 1 delta = 0 penalty = self.i_constant * self.running_error if use_accumulator and self.last_dist is not None: delta = np.abs(dist) - np.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) 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, t), method='trust-constr', bounds=bounds, options={'maxiter': 3}) x = solved.x print( f"dist: {round(dist, 5)}, curr_vel: {round(v_i, 5)}, accel: {round(x[0], 5)}, alpha: {round(x[1], 5)}, t: {round(t, 5)}, delta: {round(delta, 5)}" ) # print(self.compute_objective_integrated([accel_init, alpha_init], # *(theta_i, ang_vel, x_targ, x_curr, y_targ, y_curr, v_i, v_cx, v_cy, t))) 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)}, t: {round(t, 5)}, delta: {round(delta, 5)}") return solved # (accel, alpha)
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)