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 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 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 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 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 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 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 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_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 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 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 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 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_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 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 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 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 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 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 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, 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) 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 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 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)