def update(self, control_signals): """ The function to update solver agent position within maze. After agent position updated it will be checked to find out if maze exit was reached afetr that. Arguments: control_signals: The control signals received from the control ANN Returns: The True if maze exit was found after update or maze exit was already found in previous simulation cycles. """ if self.exit_found: # Maze exit already found return True # Apply control signals self.apply_control_signals(control_signals) # get X and Y velocity components vx = math.cos(geometry.deg_to_rad( self.agent.heading)) * self.agent.speed vy = math.sin(geometry.deg_to_rad( self.agent.heading)) * self.agent.speed # Update current Agent's heading (we consider the simulation time step size equal to 1s # and the angular velocity as degrees per second) self.agent.heading += self.agent.angular_vel # Enforce angular velocity bounds by wrapping if self.agent.heading > 360: self.agent.heading -= 360 elif self.agent.heading < 0: self.agent.heading += 360 # find the next location of the agent new_loc = geometry.Point(x=self.agent.location.x + vx, y=self.agent.location.y + vy) if not self.test_wall_collision(new_loc): self.agent.location = new_loc # update agent's sensors self.update_rangefinder_sensors() self.update_radars() # check if agent reached exit point distance = self.agent_distance_to_exit() self.exit_found = (distance < self.exit_range) return self.exit_found
def update_rangefinder_sensors(self): """ The function to update the agent range finder sensors. """ for i, angle in enumerate(self.agent.range_finder_angles): rad = geometry.deg_to_rad(angle) # project a point from agent location outwards projection_point = geometry.Point( x=self.agent.location.x + math.cos(rad) * self.agent.range_finder_range, y=self.agent.location.y + math.sin(rad) * self.agent.range_finder_range) # rotate the projection point by the agent's heading angle to # align it with heading direction projection_point.rotate(self.agent.heading, self.agent.location) # create the line segment from the agent location to the projected point projection_line = geometry.Line(a=self.agent.location, b=projection_point) # set range to maximum detection range min_range = self.agent.range_finder_range # now test against maze walls to see if projection line hits any wall # and find the closest hit for wall in self.walls: found, intersection = wall.intersection(projection_line) if found: found_range = intersection.distance(self.agent.location) # we are interested in the closest hit if found_range < min_range: min_range = found_range # Update sensor value self.agent.range_finders[i] = min_range
def __init__(self, threshold=geometry.deg_to_rad(15)): super().__init__() self.threshold = threshold self.min_direction = None self.max_direction = None
def __init__(self, threshold=geometry.deg_to_rad(15)): self.points = [] self.cache = {} self.top = None self.bottom = None self.left = None self.right = None
def _point_groups_are_similar(group1, group2, bounds, threshold=geometry.deg_to_rad(15)): # measure the nearest distance between the two point groups distance1 = geometry.manhattan(group1.start, group2.start) distance2 = geometry.manhattan(group1.start, group2.stop) distance3 = geometry.manhattan(group1.stop, group2.start) distance4 = geometry.manhattan(group1.stop, group2.stop) # reject them if they are too far apart bad_distance = 50 too_far = all([ distance1 > bad_distance, distance2 > bad_distance, distance3 > bad_distance, distance4 > bad_distance, ]) if too_far: return False # fit a line to each point group xs1, ys1 = group1.xs, group1.ys m1, b1 = geometry.Line.compute_model(xs1, ys1) xs2, ys2 = group2.xs, group2.ys m2, b2 = geometry.Line.compute_model(xs2, ys2) xs = [ min(group1.start.x, group2.start.x), max(group1.stop.x, group2.stop.x), ] # reject them if the slopes and y-intercepts are too far apart for x in xs: y1 = m1 * x + b1 y2 = m2 * x + b2 dy = abs(y1 - y2) if dy >= 5: return False # fit a line to the merged point group m, b = geometry.Line.compute_model(xs1 + xs2, ys1 + ys2) # reject if the fitted line is too different from either of the # original point groups if m1 - threshold > m or m1 + threshold < m: return False if m2 - threshold > m or m2 + threshold < m: return False return True
def _get_glideslope_tang(landing_point): return METERS_PER_KILOMETER * tan( geometry.deg_to_rad(max(MIN_GLIDESLOPE_ANGLE, landing_point[3])))
from math import tan import utils import geometry METERS_PER_KILOMETER = 1000 ASC_TANG = tan(geometry.deg_to_rad(10.0)) * METERS_PER_KILOMETER DESC_TANG = tan(geometry.deg_to_rad(10.0)) * METERS_PER_KILOMETER TAKEOFF_ALTITUDE = 0.1 * ASC_TANG TAKEOFF_STRAIGHT_UNTIL_ALTITUDE = 1500 MIN_GLIDESLOPE_ANGLE = 3.3 MIN_IAF_LEVEL_DISTANCE = 3.0 IAF_DISTANCE = 25.0 FAF_DISTANCE = 10.0 FLARE_DISTANCE = 0.2 GLIDESLOPE_ALTITUDE_CORRECTION = 75 * tan( geometry.deg_to_rad(MIN_GLIDESLOPE_ANGLE)) def point_to_params(name, pt, alt=None, marker=None): """Returns point coordinates as a list for Kramax AutoPilot flight plan.""" if alt is None: alt = pt[2] params = [ ('name', name), ('Vertical', 'true'), ('lat', pt[0]),
def _get_glideslope_tang(landing_point): return METERS_PER_KILOMETER * tan( geometry.deg_to_rad(max(MIN_GLIDESLOPE_ANGLE, landing_point[3])) )
from math import tan import utils import geometry METERS_PER_KILOMETER = 1000 ASC_TANG = tan(geometry.deg_to_rad(10.0)) * METERS_PER_KILOMETER DESC_TANG = tan(geometry.deg_to_rad(10.0)) * METERS_PER_KILOMETER TAKEOFF_ALTITUDE = 0.1 * ASC_TANG TAKEOFF_STRAIGHT_UNTIL_ALTITUDE = 1500 MIN_GLIDESLOPE_ANGLE = 3.3 MIN_IAF_LEVEL_DISTANCE = 3.0 IAF_DISTANCE = 25.0 FAF_DISTANCE = 10.0 FLARE_DISTANCE = 0.2 GLIDESLOPE_ALTITUDE_CORRECTION = 75 * tan(geometry.deg_to_rad(MIN_GLIDESLOPE_ANGLE)) def point_to_params(name, pt, alt=None, marker=None): """Returns point coordinates as a list for Kramax AutoPilot flight plan.""" if alt is None: alt = pt[2] params = [ ('name', name), ('Vertical', 'true'), ('lat', pt[0]), ('lon', pt[1]),