def update_loc(self, env, wheel_dist=5.2, iteration_time=0.1, verbose=False): """ Given speed of the wheel, get delta ang and displacement of agent. input: - self: network output for both wheels, each is float between 0 - 1 output: - d: displacement of the agent - ang: change in ang """ w = wheel_dist # distance between the two wheels u = 1 / iteration_time # number of iterations per second max_speed = 8 noise_l = rd.uniform(0.9, 1.1) # noise rate for left motor noise_r = rd.uniform(0.9, 1.1) # noise rate for right motor v_l = self.left_output * max_speed * noise_l v_r = self.right_output * max_speed * noise_r # change in ang delta_rad = (v_r - v_l) / (w * u) delta_ang = math.degrees(delta_rad) # change in displace in the direction of the new ang d = (v_r + v_l) / (2 * u) if verbose: print('left: velocity = {}, noise = {}'.format(v_l, noise_l)) print('right: velocity = {}, noise = {}'.format(v_r, noise_r)) print('delta_rad:', delta_rad) print('delta_ang = {}, displacement = {}'.format(delta_ang, d)) print('') raw_loc = find_loc(self.loc, self.ang, d) loc = [] # make sure loc doesn't go beyond constraints of the environment if raw_loc[0] < self.r: loc.append(self.r) elif raw_loc[0] > (env.width - self.r): loc.append(env.width - self.r) else: loc.append(raw_loc[0]) if raw_loc[1] < self.r: loc.append(self.r) elif raw_loc[1] > (env.height - self.r): loc.append(env.height - self.r) else: loc.append(raw_loc[1]) self.ang = norm_ang(self.ang + delta_ang) self.loc = loc[0], loc[1]
def get_ir_readings(self, env, verbose=False): """Get readings for all IR sensors.""" self.ir_readings = [] # iterate through all ir sensors for i in range(len(self.ir_placement)): # first get loc and ang for the ir sensor placement_ang = norm_ang(self.ir_placement[i]+self.ang) # self.r-0.3 is default distance for IR sensor from center of body ir_loc = find_loc(self.loc, placement_ang, self.r-0.3) ir_ang = norm_ang(self.ir_ang[i]+self.ang) if verbose: print('\ncurrent sensor:', i) print('sensor position:', ir_loc, ir_ang) reading = self.ir_read(ir_loc, ir_ang, env, verbose) # update reading in the agent self.ir_readings.append(reading)
def ir_read(self, ir_loc, ir_ang, env, verbose=True, ir_range=5): """ Get the reading for an IR sensor. Inputs: - ir_loc: ir location, xy - ir_ang: ir angle - ir_range: default is 5 cm - env: environment; get width, height and agent attributes from it Output: - reading: float, sensor reading intensity, between 0 - 1 """ reading = 0 # check wall distance_wall = [] range_max = find_loc(ir_loc, ir_ang, ir_range) # 1st quadrant if ir_ang >= 0 and ir_ang < 90: # if this is true, then wall is detected if range_max[0] > env.width or range_max[1] > env.height: # if hitting wall via x axis if range_max[0] > env.width: if verbose: print('wall detected: 1st quadrant, x') side_a = env.width - ir_loc[0] ang_a = 90 - ir_ang d = side_a * math.sin(math.radians(90)) / \ math.sin(math.radians(ang_a)) # print('x diff:', side_a) # print('ang:', ang_a, 'distance:', d) distance_wall.append(d) # if hitting wall via y axis if range_max[1] > env.height: if verbose: print('wall detected: 1st quadrant, y') side_a = env.height - ir_loc[1] ang_a = ir_ang d = side_a * math.sin(math.radians(90)) / \ math.sin(math.radians(ang_a)) # print('y diff:', side_a) # print('ang:', ang_a, 'distance:', d) distance_wall.append(d) # 2nd quadrant elif ir_ang >= 90 and ir_ang < 180: # if this is true, then wall is detected if range_max[0] < 0 or range_max[1] > env.height: # if hitting wall via x axis if range_max[0] < 0: if verbose: print('wall detected: 2nd quadrant, x') side_a = ir_loc[0] ang_a = ir_ang - 90 d = side_a * math.sin(math.radians(90)) / \ math.sin(math.radians(ang_a)) distance_wall.append(d) # if hitting wall via y axis if range_max[1] > env.height: if verbose: print('wall detected: 2nd quadrant, y') side_a = env.height - ir_loc[1] ang_a = 180 - ir_ang d = side_a * math.sin(math.radians(90)) / \ math.sin(math.radians(ang_a)) distance_wall.append(d) # 3rd quadrant elif ir_ang >= 180 and ir_ang < 270: # if this is true, then wall is detected if range_max[0] < 0 or range_max[1] < 0: # if hitting wall via x axis if range_max[0] < 0: if verbose: print('wall detected: 3rd quadrant, x') side_a = ir_loc[0] ang_a = 270 - ir_ang d = side_a * math.sin(math.radians(90)) / \ math.sin(math.radians(ang_a)) distance_wall.append(d) # if hitting wall via y axis if range_max[1] < 0: if verbose: print('wall detected: 3rd quadrant, y') side_a = ir_loc[1] ang_a = ir_ang - 180 d = side_a * math.sin(math.radians(90)) / \ math.sin(math.radians(ang_a)) distance_wall.append(d) # 4th quadrant elif ir_ang >= 270 and ir_ang < 360: # if this is true, then wall is detected if range_max[0] > env.width or range_max[1] < 0: # if hitting wall via x axis if range_max[0] > env.width: if verbose: print('wall detected: 4th quadrant, x') side_a = env.width - ir_loc[0] ang_a = ir_ang - 270 d = side_a * math.sin(math.radians(90)) / \ math.sin(math.radians(ang_a)) distance_wall.append(d) # if hitting wall via x axis if range_max[1] < 0: if verbose: print('wall detected: 4th quadrant, y') side_a = ir_loc[1] ang_a = 360 - ir_ang d = side_a * math.sin(math.radians(90)) / \ math.sin(math.radians(ang_a)) distance_wall.append(d) # detect agents distance_agents = [] # iterate over each agent in the environment for agent in env.agents: if agent.name == self.name: pass else: if verbose: print('\ncurrent agent:', agent.name) overlap = False detect = False # Step 1. Detection range overlap with agent? distance = get_distance(ir_loc, agent.loc) if distance < ir_range + agent.r: overlap = True if verbose: print('agent within detectable range') else: if verbose: print('agent outside of detectable range') # Step 2. Check if agent at detectable angle if overlap: ir_agent_ang = find_ang(ir_loc, agent.loc) diff = abs(ir_ang-ir_agent_ang) # if the angle is greater than 180, take the complementary if diff > 180: diff = 360 - diff if diff >= 90: # pointed away, would not be able to detect the agent if verbose: print('no detected; pointed away') else: side = get_distance(ir_loc, agent.loc) # print('side:', side) closest = side * math.sin(math.radians(diff)) / \ math.sin(math.radians(90)) # print(closest, agent_r) if closest < agent.r: detect = True else: if verbose: print('not detected; not close enough') if detect: if verbose: print('agent detected') possible_distance = [] side_c = side side_a = agent.r ang_a = diff sin_c = side_c * (math.sin(math.radians(ang_a)) / agent.r) ang_c = math.degrees(math.asin(sin_c)) for c in [ang_c, 180-ang_c]: ang_b = 180 - (180-c) - ang_a side_b = math.sin(math.radians(ang_b)) * \ (side_a / math.sin(math.radians(ang_a))) # Side_b should not be smaller than 0 # If it is, IR overlapped w/ agent # This means an error in the agent collision adjustment # Ideally, debug this. # Temporarily: treat side_b as 0; lead to max reading if side_b < 0: if verbose: print("ERROR: IR overlap w/ agent") side_b = 0 # side_b must be within ir_range; # otherwise it's not really detected if side_b < ir_range: possible_distance.append(side_b) if possible_distance: final = min(possible_distance) if verbose: print('all:', possible_distance) print('final:', final) print('') distance_agents.append(final) else: if verbose: print('False alarm, nothing detected') # review detection results distance = distance_wall + distance_agents if distance: reading = 1 - min(distance) / ir_range if verbose: print('\nCurrent sensor recap:') print('distance_wall:', distance_wall) print('distance_agents:', distance_agents) print('final_reading:', reading) print('\n') return reading
def get_patches(self, verbose=False): """Get a list of patches for plotting via other functions.""" patches = [] if verbose: # body, color is cyan patches.append(Circle(self.loc, self.r, color='cyan')) # ground sensor patches.append(Circle(self.loc, 0.4, color='gray')) # comm unit patches.append(Circle(self.loc, 0.2, color='green')) # IR sensors for i in range(len(self.ir_ang)): # width and height of the rectangular IR sensor representation width = 0.2 height = 0.5 placement_ang = norm_ang(self.ir_placement[i] + self.ang) detect_ang = norm_ang(self.ir_ang[i] + self.ang) loc = find_loc(self.loc, placement_ang, self.r - 0.3) patches.append( Rectangle((loc[0], loc[1]), width / 2, height / 2, angle=detect_ang, color='black')) patches.append( Rectangle((loc[0], loc[1]), height / 2, width / 2, angle=detect_ang + 90, color='black')) patches.append( Rectangle((loc[0], loc[1]), width / 2, height / 2, angle=detect_ang + 180, color='black')) patches.append( Rectangle((loc[0], loc[1]), height / 2, width / 2, angle=detect_ang + 270, color='black')) # easier but uglier to use Ellipses; abandoned # ax.add_patch(Ellipse(loc, 0.2, 0.7, # angle=detect_ang, color='black')) patches.append( FancyArrow(loc[0], loc[1], find_dx(loc[0], detect_ang, 0.5), find_dy(loc[1], detect_ang, 0.5), color='black', length_includes_head=False, head_width=0.15)) # comm sensors comm_sensors = self.comm_sensors # use different, random color to distinguish between the 4 sensors for comm_range in comm_sensors: patches.append( Wedge(self.loc, 0.8, norm_ang(comm_range[0] + self.ang), norm_ang(comm_range[1] + self.ang), 0.3, color=(rd.uniform(0, 1), rd.uniform(0, 1), rd.uniform(0, 1)))) # wheels left_ang = norm_ang(self.ang + 90) right_ang = norm_ang(self.ang - 90) left_center = find_loc(self.loc, left_ang, self.r - 0.4) right_center = find_loc(self.loc, right_ang, self.r - 0.4) # wheels are represented with a ellipse # width=0.3, length=1.5 patches.append( Ellipse(left_center, 0.3, 1.5, left_ang, color='red')) patches.append( Ellipse(right_center, 0.3, 1.5, right_ang, color='red')) # add mid line patches.append( FancyArrow(self.loc[0], self.loc[1], find_dx(self.loc[0], self.ang, self.r), find_dy(self.loc[1], self.ang, self.r), color='black', length_includes_head=True, head_width=0.2)) else: # body; color is as assigned patches.append(Circle(self.loc, self.r, color=self.color)) # add a longer mid line for better display in the environment patches.append( FancyArrow(self.loc[0], self.loc[1], find_dx(self.loc[0], self.ang, 15), find_dy(self.loc[1], self.ang, 15), color='black', length_includes_head=True, head_width=6)) return patches