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 get_comm_readings(self, env, comm_disabled, verbose=False): """ Get comm sensor readings. Inputs: - position of the agent - position of all other agents - range of the 4 comm sensors Output: - list, reading of the 4 comm sensors """ if comm_disabled: self.comm_readings = [0, 0, 0, 0] else: self.comm_readings = [] comm = self.comm_sensors received = [ [0], [0], [0], [0] ] for agent in env.agents: # first, check if an agent is within range if agent.name == self.name: pass else: if verbose: print('current agent:', agent.name) d = get_distance(self.loc, agent.loc) if d <= 100: # if this is true, then it's within range # get the signal signal = agent.comm_output if verbose: print('perceived signal:', signal) # determine which comm sensor receives the signal diff = norm_ang(find_ang(self.loc, agent.loc) - self.ang) if diff >= comm[0][0] or diff < comm[0][1]: received[0].append(signal) if verbose: print('received by front sensor') elif diff >= comm[1][0] and diff < comm[1][1]: received[1].append(signal) if verbose: print('received by left sensor') elif diff >= comm[2][0] and diff < comm[2][1]: received[2].append(signal) if verbose: print('received by rear sensor') elif diff >= comm[3][0] and diff < comm[3][1]: received[3].append(signal) if verbose: print('received by right sensor') else: if verbose: print('out of detectable range') self.comm_readings = [max(received[0]), max(received[1]), max(received[2]), max(received[3])]
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