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