def check_overlap(self, bounds, inner_walls, trees, xagents, crush_damage):
# keep within bounds
while bounds.contains(self.area) == False:
self.x -= self.dx
self.y -= self.dy
do = np.radians(np.random.randint(360))
self.o = geometry.force_angle(self.o + do)
self.define_body_area()
self.damage += crush_damage
# no damage from trees
for tx in trees:
if self.area.intersects(tx.area):
self.x -= self.dx
self.y -= self.dy
self.define_body_area()
break
# no damage from other agents
for xa in xagents:
if self.area.intersects(xa.area):
self.x -= self.dx
self.y -= self.dy
self.define_body_area()
break
# check for overlappings with objects
# other_world_objects = inner_walls+xagents
for iw in inner_walls:
if self.area.intersects(iw.area):
self.x -= self.dx
self.y -= self.dy
do = np.radians(np.random.randint(360))
self.o = geometry.force_angle(self.o + do)
self.define_body_area()
self.damage += crush_damage
def define_angles(self):
ir_angles = []
# front sensors
for i in range(1,4):
ir_angles.extend([geom.force_angle(self.do_front*i), geom.force_angle(-self.do_front*i)])
# rear sensors
ir_angles.extend([geom.force_angle(np.pi+self.do_rear), geom.force_angle(np.pi-self.do_rear)])
return ir_angles
def read_irs(self, x, y, o, r, objects):
# data
vis_sensors_domain = []
ir_reading = []
# for each sensor
for ir_angle in self.ir_angles:
ir_val = 0
min_dist = self.ray_length
# location and orientation according to agent
ir_o = geometry.force_angle(o + ir_angle)
ir_x = x + r * np.cos(ir_o)
ir_y = y + r * np.sin(ir_o)
# define visual domain
arc_start = ir_o + self.beam_angles[0]
arc_end = ir_o + self.beam_angles[1]
# to be sure the angle is counter-clockwise
if arc_start > arc_end:
arc_end += np.radians(360)
# get arc angle points and force angles
arc_points = np.linspace(arc_start, arc_end, self.s_points)
arc_angles = np.array(
[geometry.force_angle(oi) for oi in arc_points])
# get the arc coordinates and create polygon
arc_x = ir_x + self.ray_length * np.cos(arc_angles)
arc_y = ir_y + self.ray_length * np.sin(arc_angles)
vis_coords = [(ir_x, ir_y)]
[vis_coords.append((xi, yi)) for xi, yi in zip(arc_x, arc_y)]
vis_domain = Polygon(vis_coords)
vis_sensors_domain.append(vis_domain)
# check for intersections
for w in objects["walls"]:
wall = LineString([(w.xmin, w.ymin), (w.xmax, w.ymax)])
if vis_domain.intersects(wall):
dist = Point(ir_x,
ir_y).distance(vis_domain.intersection(wall))
if dist < min_dist:
min_dist = dist
round_objects = objects["trees"] + objects["agents"]
for obj in round_objects:
obj_loc = Point(obj.x, obj.y)
obj_space = obj_loc.buffer(obj.r)
if vis_domain.intersects(obj_space):
dist = Point(ir_x, ir_y).distance(
vis_domain.intersection(obj_space))
if dist < min_dist:
min_dist = dist
# get ir value
if min_dist < self.ray_length:
# IR reading for a given distance from empirical fitting data
# gaussian 3371*e^(-(d/8.5)^2) fits well [0, 3500]
k = -1 * ((dist / 8.5)**2)
ir_val = (3371 * np.exp(k)) / 3500
ir_reading.append(ir_val)
self.sensory_domain["vis"].append(vis_sensors_domain)
return ir_reading
def define_sensors_area(self, x, y, o, r):
self.vs_sensors = []
self.vs_xy = []
for n in range(self.vs_n):
# list of angles of sensors defined in genotype
vso = geometry.force_angle(o + self.vs_loc[n])
# sensor location
vsx = x + r * np.cos(vso)
vsy = y + r * np.sin(vso)
self.vs_xy.append([vsx, vsy])
# define arc (sensor angle +- sensor angle range)
arc_start = vso - self.vs_theta / 2
arc_end = vso + self.vs_theta / 2
# check for counter-clockwise
if arc_start > arc_end:
arc_end += np.radians(360)
# define sensor area
arc_points = np.linspace(arc_start, arc_end, self.s_points)
arc_angles = np.array(
[geometry.force_angle(oi) for oi in arc_points])
arc_xs = vsx + self.vs_range * np.cos(arc_angles)
arc_ys = vsy + self.vs_range * np.sin(arc_angles)
area_coords = [(vsx, vsy)]
[area_coords.append((xi, yi)) for xi, yi in zip(arc_xs, arc_ys)]
# define sensor polygon
sensor_domain = Polygon(area_coords)
self.vs_sensors.append(sensor_domain)
# olfactory sensor location
self.olf_sensors = []
self.olf_xy = []
# basically the same, but by separate is clearer
for n in range(self.olf_n):
# list of angles of sensors defined in genotype
olfo = geometry.force_angle(o + self.olf_loc[n])
# sensor location
olfx = x + r * np.cos(olfo)
olfy = y + r * np.sin(olfo)
self.olf_xy.append([olfx, olfy])
# define arc
arc_start = olfo - self.olf_theta / 2
arc_end = olfo + self.olf_theta / 2
if arc_start > arc_end:
arc_end += np.radians(360)
arc_points = np.linspace(arc_start, arc_end, self.s_points)
arc_angles = np.array(
[geometry.force_angle(oi) for oi in arc_points])
arc_xs = olfx + self.olf_range * np.cos(arc_angles)
arc_ys = olfy + self.olf_range * np.sin(arc_angles)
area_coords = [(olfx, olfy)]
[area_coords.append((xi, yi)) for xi, yi in zip(arc_xs, arc_ys)]
self.olf_sensors.append(Polygon(area_coords))
def __init__(self, s_points=10\
, ir_angle=60, ray_length=40, beam_spread=120\
, olf_angle=120, olf_range=20\
, aud_angle=90, aud_range=50):
# init
self.s_points = s_points
self.ir_angles = [
geometry.force_angle(np.radians(-ir_angle)),
geometry.force_angle(np.radians(ir_angle))
]
self.ray_length = ray_length
self.beam_angles = [
geometry.force_angle(np.radians(-beam_spread / 2)),
geometry.force_angle(np.radians(beam_spread / 2))
]
self.olf_angles = [
geometry.force_angle(np.radians((-olf_angle / 2))),
geometry.force_angle(np.radians((olf_angle / 2)))
]
self.olf_range = olf_range
self.aud_angles = [
geometry.force_angle(np.radians(-aud_angle)),
geometry.force_angle(np.radians(aud_angle))
]
self.aud_range = aud_range
# internal
self.sensory_domain = {}
self.sensory_domain["vis"] = []
self.sensory_domain["olf"] = []
self.sensory_domain["aud"] = []
def move_fx(self):
# noise as in Shibuya et al.
vl = (self.lm*self.speed)*self.dt * np.random.uniform(0.9,1.1)
vr = (self.rm*self.speed)*self.dt * np.random.uniform(0.9,1.1)
vel = (vr+vl)/2
# change
self.dx = vel*np.cos(self.o)
self.dy = vel*np.sin(self.o)
self.do = geom.force_angle((vr-vl)/self.wsep)
# new location
self.x += self.dx
self.y += self.dy
self.o = geom.force_angle(self.o+self.do)
self.define_body()
def update_sensors(self):
# update information from ir sensors
self.ir_sensors = []
for sensor in self.irs:
sx = self.x + sensor[0][0]
sy = self.y + sensor[0][1]
so = geometry.force_angle(self.orientation + sensor[1])
ll_ray = geometry.force_angle(so + sensor[2][0])
rr_ray = geometry.force_angle(so + sensor[2][-1])
# [[rel_x, rel_y], rel_angle, left_most_ray, right_most_ray]
self.ir_sensors.append([[sx, sy],
np.degrees(so),
np.degrees(ll_ray),
np.degrees(rr_ray)])
def feed(self, objects):
# "mouth" location
fx = self.x + self.r * np.cos(self.o)
fy = self.y + self.r * np.sin(self.o)
# feeding area: arc
arc_start = self.o - np.radians(self.feeding_angle / 2)
arc_end = self.o + np.radians(self.feeding_angle / 2)
# force counter-clockwise
if arc_start > arc_end:
arc_end += np.radians(360)
# get arc angle points and force angles
arc_points = np.linspace(arc_start, arc_end, self.genotype.s_points)
arc_angles = np.array([geometry.force_angle(oi) for oi in arc_points])
# get the arc coordinates and create polygon
arc_x = fx + self.olf_range * np.cos(arc_angles)
arc_y = fy + self.olf_range * np.sin(arc_angles)
feeding_coords = [(fx, fy)]
[feeding_coords.append((xi, yi)) for xi, yi in zip(arc_x, arc_y)]
feeding_area = Polygon(feeding_coords)
# check for trees to feed
feeding = False
trees = objects["trees"]
for tree in trees:
if feeding_area.intersects(tree):
feeding = True
feed_rate = self.feed_rate * (1 /
np.exp(dist / self.feed_range))
self.feeding_states.append([feeding_area, feeding])
def read_olf(self, x, y, o, r, objects):
# sensor location-orientation
olf_x = x + r * np.cos(o)
olf_y = y + r * np.sin(o)
olf_val = 0
# define sensor domain (polygon)
arc_start = o + self.olf_angles[0]
arc_end = o + self.olf_angles[1]
# to be sure the angle is counter-clockwise
if arc_start > arc_end:
arc_end += np.radians(360)
# get arc angle points and force angles
arc_points = np.linspace(arc_start, arc_end, self.s_points)
arc_angles = np.array([geometry.force_angle(oi) for oi in arc_points])
# get the arc coordinates and create polygon
arc_x = olf_x + self.olf_range * np.cos(arc_angles)
arc_y = olf_y + self.olf_range * np.sin(arc_angles)
olf_coords = [(olf_x, olf_y)]
[olf_coords.append((xi, yi)) for xi, yi in zip(arc_x, arc_y)]
olf_domain = Polygon(olf_coords)
self.sensory_domain["olf"].append(olf_domain)
# check for each tree
min_dist = self.olf_range
trees = objects["trees"]
for tx in trees:
tree_loc = Point(tx.x, tx.y)
tree_space = tree_loc.buffer(tx.r)
if olf_domain.intersects(tree_space):
dist = Point(olf_x, olf_y).distance(
olf_domain.intersection(tree_space))
if dist <= min_dist:
olf_val = (1 / np.exp(min_dist / self.olf_range))**2
return olf_val
def collision_fx(self):
# if they were to collide, they don't move (Quinn's thesis)
self.x -= self.dx
self.y -= self.dy
# but the angle changes in a fraction of the original spin
self.o = geom.force_angle(self.o-self.do+(self.do*np.random.uniform(0,1)))
self.define_body()
def __init__(self,time,network,alpha,dt=0.1,data_dt=1):
# basics
self.network = network
self.time = time
self.alpha = alpha
self.dt = dt
self.data_dt = data_dt
# parameters as in Shibuya et al.
self.agsR = 2.9
self.di = 1.25
self.cmax = 10
self.dmax = 40
self.dist = 0
self.max_dist = 0
self.cols = 0
self.cp = 1
self.ft = 0
# data for visualization
self.triangles=[]
self.data_gt=[]
self.data_st=[]
self.data_ft=[]
self.data_cp=[[0,1]]
# data for transfer entropy
self.states = []
# self.i_given_ij = Counter()
self.te_frame = 50
# allocate agents (Shibuya et al)
self.agents = []
y0 = np.sqrt((2*(self.di+self.agsR))**2 - (self.di+self.agsR)**2)
o0 = geom.force_angle(np.radians(300)+self.alpha)
self.agents.append(qagent.Agent(self.network,0,y0,o0))
x1 = -(self.agsR+self.di)
o1 = geom.force_angle(np.radians(60)+self.alpha)
self.agents.append(qagent.Agent(self.network,x1,0,o1))
x2 = self.agsR+self.di
o2 = geom.force_angle(np.radians(180)+self.alpha)
self.agents.append(qagent.Agent(self.network,x2,0,o2))
# initial allocations data
self.triangle = Polygon([[ag.x,ag.y] for ag in self.agents])
self.xy0 = self.triangle.centroid
self.ags_dist = [agent.body.distance(self.xy0) for agent in self.agents]
self.save_data()
self.run()
def ray_end(self, ir_x, ir_y, angle, rel_angle):
# end of ray given standard lenght and particular angle
# angle1: angle of sensor
# angle2: angle of ray relative to sensor mid
# anticlockwise is negative
ray_angle = geometry.force_angle(angle + rel_angle)
ray_x = ir_x + self.ray_length * np.cos(ray_angle)
ray_y = ir_y + self.ray_length * np.sin(ray_angle)
ray_end = np.array([ray_x, ray_y])
return ray_end
def move_fx(self):
# compute changes in location (force movement if 0)
lw = self.lm * self.max_speed + np.random.uniform(-0.1, 0.1)
rw = self.rm * self.max_speed + np.random.uniform(-0.1, 0.1)
vel = (lw + rw) / 2
# update dx, dy, do
self.dx = vel * np.cos(self.o)
self.dy = vel * np.sin(self.o)
do = np.radians((lw - rw) / self.wheels_sep)
# update x, y, o and body area
self.x += self.dx
self.y += self.dy
self.o = geometry.force_angle(self.o + do)
def define_irs(self,x,y,o):
# update position of active sensors
self.irs = []
for irx,ir_angle in zip(self.irx,self.ir_angles):
ir = None
if irx==True:
# IRs: shapely linear objects
oir = geom.force_angle(o+ir_angle)
sx = x + self.r*np.cos(oir)
sy = y + self.r*np.sin(oir)
rx = sx + self.srange*np.cos(oir)
ry = sy + self.srange*np.sin(oir)
ir = LineString([(sx,sy),(rx,ry)])
self.irs.append(ir)
def move(self, lw, rw, objects):
# compute dx, dy, do
lw = lw * self.max_speed
rw = rw * self.max_speed
vel = (lw + rw) / 2
dx = vel * np.cos(self.o)
dy = vel * np.sin(self.o)
do = np.radians((lw - rw) / self.wheels_sep)
# update
self.x += dx
self.y += dy
self.o = geometry.force_angle(self.o + do)
self.energy -= 1
self.bouncing_fx(dx, dy, objects)
def move(self):
# new x,y and orientation
ls, rs = self.robot_speed()
vel = (ls + rs) / 2
dx = vel * np.cos(self.orientation)
dy = vel * np.sin(self.orientation)
do = np.radians((ls - rs) / self.wheel_sep)
# update, but keep x and y within limits
self.x += dx
if self.x > world.xmax:
self.x = world.xmax - self.radius * 2
if self.x < 0:
self.x = self.radius * 2
self.y += dy
if self.y > world.ymax:
self.y = world.ymax - self.radius * 2
if self.y < 0:
self.y = self.radius * 2
# update
self.position = np.array([self.x, self.y])
self.orientation += do
self.orientation = geometry.force_angle(self.orientation)
# what to do after collisions?
if self.wall_collision() == True:
#print("collided...")
self.notes = "collision"
self.energy -= 10
# face opposite direction
self.orientation = geometry.force_angle(self.orientation + np.pi)
# update sensors parameters
# irs[i]: [ir_rel_pos, ir_rel_angles[n], ir_rel_rays]
for i in range(len(self.irs)):
rel_x = self.radius * np.cos(self.orientation + self.irs[i][1])
rel_y = self.radius * np.sin(self.orientation + self.irs[i][1])
self.irs[i][0] = [rel_x, rel_y]
def define_feeding_area(self):
# define feeding area
fx = self.x + self.r * np.cos(self.o)
fy = self.y + self.r * np.sin(self.o)
arc_start = self.o - np.radians(self.f_theta / 2)
arc_end = self.o + np.radians(self.f_theta / 2)
# force counter-clockwise
if arc_start > arc_end:
arc_end += np.radians(360)
# get arc angle points
arc_points = np.linspace(arc_start, arc_end, 100)
arc_angles = np.array([geometry.force_angle(oi) for oi in arc_points])
# get arc coordinates and create polygon
arc_x = fx + self.f_range * np.cos(arc_angles)
arc_y = fy + self.f_range * np.sin(arc_angles)
f_coords = [(fx, fy)]
[f_coords.append((xi, yi)) for xi, yi in zip(arc_x, arc_y)]
self.f_area = Polygon(f_coords)
def read_aud(self, x, y, o, r, objects):
aud_vals = []
for aud_angle in self.aud_angles:
ao = geometry.force_angle(o + aud_angle)
ax = x + r * np.cos(ao)
ay = y + r * np.sin(ao)
min_dist = self.aud_range
aud_val = 0
# agents = [ag for ag in objects if type(ag)==agent.Agent]
agents = objects["agents"]
for ag in agents:
dist = np.linalg.norm(
np.array([ag.x, ag.y]) - np.array([ax, ay]))
if dist <= min_dist:
# the com output from agent weighted by the distance
aud_val = ag.com * (1 /
np.exp(min_dist / self.aud_range))**2
aud_vals.append(aud_val)
return aud_vals
def allocate_irs(self):
# sensor only on the top half of the body uniformily distributed
if self.n_irs == 2:
ir_rel_angles = [np.radians(315), np.radians(45)]
else:
angle_sep = 180 / (self.n_irs + 1)
ir_rel_angles = [
geometry.force_angle(np.radians(-90 + angle_sep * i))
for i in range(1, self.n_irs + 1)
]
for n in range(self.n_irs):
# relative position for each sensor
ir_rel_x = self.radius * np.cos(ir_rel_angles[n])
ir_rel_y = self.radius * np.sin(ir_rel_angles[n])
ir_rel_pos = np.array([ir_rel_x, ir_rel_y])
# define ray relative angles for beams
rays_sep = self.ray_spread / (self.n_rays - 1)
# negative for counterclockwise
ir_rel_rays = [(-self.ray_spread / 2) + rays_sep * n
for n in range(self.n_rays)]
# everything relative to location/orientation
self.irs.append([ir_rel_pos, ir_rel_angles[n], ir_rel_rays])
def animate(i):
# time
time.set_text("time: " + str(i))
# for each agent in the list of animated objects
for enum, agent_obj in enumerate(agent_objs):
# location data
o = np.degrees(agents[enum].positions[i][2])
x = agents[enum].positions[i][0]
y = agents[enum].positions[i][1]
# agent body (circle)
agent_obj.center = (x, y)
agent_obj.set_color("blue")
# agent orientation (line from center)
ox = x + agents[enum].r * np.cos(np.radians(o))
oy = y + agents[enum].r * np.sin(np.radians(o))
o_lines[enum].set_data([x, ox], [y, oy])
# olf domain (wedge)
olf_x = x + agents[enum].r * np.cos(np.radians(o))
olf_y = y + agents[enum].r * np.sin(np.radians(o))
olf_o1 = geometry.force_angle_degrees(
np.degrees(agents[enum].sensors.olf_angles[0]) + o)
olf_o2 = geometry.force_angle_degrees(
np.degrees(agents[enum].sensors.olf_angles[1]) + o)
olf_domain[enum].center = (olf_x, olf_y)
olf_domain[enum].theta1 = olf_o1
olf_domain[enum].theta2 = olf_o2
olf_domain[enum]._recompute_path()
# olf signals
olf_val = agents[enum].states[i][2]
print("time: {}, location: {},{}, agent: {}, olf_val: {}".format(
i, round(x, 2), round(y, 2), enum, olf_val))
if olf_val > 0:
# olf_domain[enum].set_visible(True)
olf_domain[enum].set_color("purple")
else:
# olf_domain[enum].set_visible(False)
olf_domain[enum].set_color("grey")
# aud domain (circles)
for n in range(len(aud_domain[enum])):
#aud_o = o + agents[enum].ps[5][n]
aud_o = o + agents[enum].sensors.aud_angles[n]
aud_x = x + agents[enum].r * np.cos(aud_o)
aud_y = y + agents[enum].r * np.sin(aud_o)
aud_domain[enum][n].center = (aud_x, aud_y)
# auditory signal
aud_val = agents[enum].states[i][3 + n]
# if aud_val > 0:
# aud_domain[enum][n].set_visible(True)
# else:
# aud_domain[enum][n].set_visible(False)
# vis domain (wedges)
for n in range(len(vis_domain[enum])):
# vis_or = agent_or + agent.sensor_or[n], ps[1]: ir_angles: [rad(-60), rad(60)]
vis_o = geometry.force_angle(agents[enum].positions[i][2] +
agents[enum].sensors.ir_angles[n])
# sensor location
vis_x = x + agents[enum].r * np.cos(vis_o)
vis_y = y + agents[enum].r * np.sin(vis_o)
# beam_spread, start and end angles of beam
vis_sp = agents[enum].genotype.beam_spread / 2
vis_o1 = geometry.force_angle_degrees(
np.degrees(vis_o) - vis_sp)
vis_o2 = geometry.force_angle_degrees(
np.degrees(vis_o) + vis_sp)
vis_domain[enum][n].center = (vis_x, vis_y)
vis_domain[enum][n].theta1 = vis_o1
vis_domain[enum][n].theta2 = vis_o2
vis_domain[enum][n]._recompute_path()
# visual signals
vis_val = agents[enum].states[i][n]
# print("time: {}, location: {},{}, agent: {}, sensor: {}, vis_val: {}".format(i,round(x,2),round(y,2),enum,n,vis_val))
if vis_val > 0:
# vis_domain[enum][n].set_visible(True)
vis_domain[enum][n].set_color("yellow")
else:
# vis_domain[enum][n].set_visible(False)
vis_domain[enum][n].set_color("grey")
# feeding (circle)
if agents[enum].feeding_states[i] == True:
feed.center = (x, y)
feed.set_visible(True)
else:
feed.set_visible(False)
# check energy values (circle)
energy = agents[enum].states[i][5]
if 0 < energy <= 200:
agent_obj.set_color("grey")
agent_obj.fill = False
if energy <= 0:
agent_obj.set_color("black")
agent_obj.fill = False
olf_domain[enum].set_visible(False)
aud_domain[enum][0].set_visible = False
aud_domain[enum][1].set_visible = False
vis_domain[enum][0].set_visible(False)
vis_domain[enum][1].set_visible(False)
all_aud = [aud for audx in aud_domain for aud in audx]
all_vis = [vis for visx in vis_domain for vis in visx]
return time, tuple(agent_objs) + tuple(o_lines) + tuple(
olf_domain) + tuple(all_aud) + tuple(all_vis) + tuple(feed_domain)
def animate(i):
# current robots locations
x = [robot_loc[0] for robot_loc in simlocations[i]]
y = [robot_loc[1] for robot_loc in simlocations[i]]
o = [
geometry.force_angle(np.radians(robot_or))
for robot_or in orientations[i]
]
# allocate robot
for enum, robot in enumerate(robots):
robot.center = (x[enum], y[enum])
if notes[i][enum] == "collision":
robot.set_color("red")
# current food sensors locations
for enum, fsensor in enumerate(fsensors):
fsensor.center = (x[enum], y[enum])
if fs_vals[i][0] != None:
fsensor.set_color("red")
else:
fsensor.set_color("blue")
# for enum, fs_arc in enumerate(fs_arcs):
# fs_arc.center = (x[enum], y[enum])
# fs_arc.angle = np.degrees(o[enum])
# fs_arc.theta1 = -fs_angle/2
# fs_arc.theta2 = fs_angle/2
# if fs_vals[i][0] != None:
# fs_arc.set_color("red")
# else:
# fs_arc.set_color("blue")
# past locations: more than one you can't see anything
if past == True and n_robots == 1:
xlocs.append(x)
ylocs.append(y)
if len(xlocs) < len(simlocations) - 1:
past_locations.set_data(xlocs, ylocs)
# sensors list, added proyection for main orientation
sx = [[x[n], x[n] + 10 * np.cos(o[n])] for n in range(len(x))]
sy = [[y[n], y[n] + 10 * np.sin(o[n])] for n in range(len(y))]
irs = [None]
# ir beams proyections
for robot_i in range(n_robots):
for ir_sensor in ir_data[i][robot_i]:
# data for each sensor for each robot
rs_x, rs_y = ir_sensor[0]
so = np.radians(ir_sensor[1])
sleft = np.radians(ir_sensor[2])
sright = np.radians(ir_sensor[3])
ir_val = ir_sensor[4]
# center ray of beam
re_x = rs_x + ir_range * np.cos(so)
re_y = rs_y + ir_range * np.sin(so)
# left most ray from left most beam
re_xl = rs_x + ir_range * np.cos(sleft)
re_yl = rs_y + ir_range * np.sin(sleft)
# right most ray from right most beam
re_xr = rs_x + ir_range * np.cos(sright)
re_yr = rs_y + ir_range * np.sin(sright)
# ir_rays
sx.append([rs_x, re_xl])
sy.append([rs_y, re_yl])
sx.append([rs_x, re_xr])
sy.append([rs_y, re_yr])
# ir value for sensor
irs.extend([ir_val, ir_val])
# plot ir_rays
for enum, ray in enumerate(rays):
ray.set_data(sx[enum], sy[enum])
if irs[enum]:
ray.set_color("orange")
else:
ray.set_color("black")
# import pdb; pdb.set_trace()
return (past_locations, ) + tuple(robots) + tuple(rays) + tuple(
fsensors) #+tuple(fs_arcs)
def read_ir(self):
# basically 3 options for left & right most rays of beam:
# a) don't sense anything: None
# b) sense the same wall: full_ir_val()
# c) sense different things: for all rays: hits/n_rays * irval(av_dist)
self.ir_reading = []
for ir_sensor in self.irs:
rel_pos = ir_sensor[0]
rel_angle = ir_sensor[1]
rel_rays = ir_sensor[2]
# angle of mid ray from sensor, clockwise from due north
ir_angle = geometry.force_angle(self.orientation + rel_angle)
# position of sensors
# sens_x = self.x + self.radius*np.cos(sensor_angle)
# sens_y = self.y + self.radius*np.sin(sensor_angle)
ir_x = self.x + rel_pos[0]
ir_y = self.y + rel_pos[1]
ir_pos = np.array([ir_x, ir_y])
# bounding rays relative to a central ir beam
sp_left = rel_rays[0]
sp_right = rel_rays[-1]
# position of ray start
# px = sens_x
# py = sens_y
if self.ray_hit(ir_pos, ir_angle,
sp_left) == False and self.ray_hit(
ir_pos, ir_angle, sp_right) == False:
# beam misses everything
self.ir_reading.append(None)
else:
# check right and left rays
left_ray_end = self.ray_end(ir_x, ir_y, ir_angle, sp_left)
right_ray_end = self.ray_end(ir_x, ir_y, ir_angle, sp_right)
l_hit, l_wall, l_dist = self.ray_hit_nearest(
ir_pos, left_ray_end)
r_hit, r_wall, r_dist = self.ray_hit_nearest(
ir_pos, right_ray_end)
# if both hit same wall as nearest object
if l_hit == True and r_hit == True and l_wall == r_wall:
val = self.full_ir_val(ir_pos, ir_angle, l_wall)
# if not, complicated...
# find wich rays do hit (proportion that do * fullval)
else:
# try to compute average distance
n_hits = 0
av_dist = 0
# loop through the all the beam's rays
for ray_angle in rel_rays:
i_ray_end = self.ray_end(ir_x, ir_y, ir_angle,
ray_angle)
ray_hit = self.ray_hit_nearest(ir_pos, i_ray_end)
if ray_hit:
n_hits += 1
av_dist += ray_hit[2]
# average value of min distance
av_dist /= n_hits
# proportional to n rays that hit (proportion of the beam)
val = (n_hits / self.n_rays) * self.ir_val(av_dist)
# ir noise: random gaussian loc=0, sigma=1 * ir_noise
ir_noise = np.random.normal() * self.ir_noise
val += ir_noise
# it can only be positive
val = 0 if val < 0 else val
self.ir_reading.append(int(val))
self.notes = "detection"
