def turn(x, y, yaw, theta, phi, theta_s, phi_s, flow, noise=0.):
sky.theta_s, sky.phi_s = theta_s, phi_s
Y, P, A = sky(theta, phi + yaw, noise=noise)
# Y, P, A = get_sky_cues(theta, phi + yaw, theta_s, phi_s, noise=noise)
r_tb1 = encode(theta, phi, Y, P, A)[::-1]
_, yaw = decode_sph(r_tb1)
# yaw = phi_s - yaw
# motor = net(r_tb1, flow)
net(yaw, flow)
yaw = (yaw + np.pi) % (2 * np.pi) - np.pi
v = np.array([np.sin(yaw), np.cos(yaw)]) * route.dx
def create_ephem_paths():
# sensor design
n = 60
omega = 56
theta, phi, fit = angles_distribution(n, float(omega))
theta_t, phi_t = 0., 0.
# ant-world
noise = 0.0
ttau = .06
dx = 1e-02 # meters
dt = 2. / 60. # min
delta = timedelta(minutes=dt)
routes = load_routes()
flow = dx * np.ones(2) / np.sqrt(2)
max_theta = 0.
def encode(theta, phi, Y, P, A, theta_t=0., phi_t=0., d_phi=0., nb_tcl=8, sigma=np.deg2rad(13),
shift=np.deg2rad(40)):
alpha = (phi + np.pi / 2) % (2 * np.pi) - np.pi
phi_tcl = np.linspace(0., 2 * np.pi, nb_tcl, endpoint=False) # TB1 preference angles
phi_tcl = (phi_tcl + d_phi) % (2 * np.pi)
# Input (POL) layer -- Photo-receptors
s_1 = Y * (np.square(np.sin(A - alpha)) + np.square(np.cos(A - alpha)) * np.square(1. - P))
s_2 = Y * (np.square(np.cos(A - alpha)) + np.square(np.sin(A - alpha)) * np.square(1. - P))
r_1, r_2 = np.sqrt(s_1), np.sqrt(s_2)
r_pol = (r_1 - r_2) / (r_1 + r_2 + eps)
# Tilting (CL1) layer
d_cl1 = (np.sin(shift - theta) * np.cos(theta_t) +
np.cos(shift - theta) * np.sin(theta_t) *
np.cos(phi - phi_t))
gate = np.power(np.exp(-np.square(d_cl1) / (2. * np.square(sigma))), 1)
w = -float(nb_tcl) / (2. * float(n)) * np.sin(phi_tcl[np.newaxis] - alpha[:, np.newaxis]) * gate[:, np.newaxis]
r_tcl = r_pol.dot(w)
R = r_tcl.dot(np.exp(-np.arange(nb_tcl) * (0. + 1.j) * 2. * np.pi / float(nb_tcl)))
res = np.clip(3.5 * (np.absolute(R) - .53), 0, 2) # certainty of prediction
ele_pred = 26 * (1 - 2 * np.arcsin(1 - res) / np.pi) + 15
d_phi += np.deg2rad(9 + np.exp(.1 * (54 - ele_pred))) / (60. / float(dt))
return r_tcl, d_phi
stats = {
"max_alt": [],
"noise": [],
"opath": [],
"ipath": [],
"d_x": [],
"d_c": [],
"tau": []
}
avg_time = timedelta(0.)
terrain = z_terrain.copy()
for enable_ephemeris in [False, True]:
if enable_ephemeris:
print "Foraging with the time compensation mechanism."
else:
print "Foraging without the time compensation mechanism."
# stats
d_x = [] # logarithmic distance
d_c = []
tau = [] # tortuosity
ri = 0
print "Routes: ",
for route in routes[::2]:
net = CX(noise=0., pontin=False)
net.update = True
# sun position
cur = datetime(2018, 6, 21, 10, 0, 0)
seville_obs.date = cur
sun.compute(seville_obs)
theta_s = np.array([np.pi / 2 - sun.alt])
phi_s = np.array([(sun.az + np.pi) % (2 * np.pi) - np.pi])
sun_azi = []
sun_ele = []
time = []
# outward route
route.condition = Hybrid(tau_x=dx)
oroute = route.reverse()
x, y, yaw = [(x0, y0, yaw0) for x0, y0, _, yaw0 in oroute][0]
opath = [[x, y, yaw]]
v = np.zeros(2)
tb1 = []
d_phi = 0.
for _, _, _, yaw in oroute:
theta_n, phi_n = tilt(theta_t, phi_t, theta, phi + yaw)
sun_ele.append(theta_s[0])
sun_azi.append(phi_s[0])
time.append(cur)
sky.theta_s, sky.phi_s = theta_s, phi_s
Y, P, A = sky(theta_n, phi_n, noise=noise)
if enable_ephemeris:
r_tb1, d_phi = encode(theta, phi, Y, P, A, d_phi=d_phi)
else:
r_tb1, d_phi = encode(theta, phi, Y, P, A, d_phi=0.)
yaw0 = yaw
_, yaw = np.pi - decode_sph(r_tb1) + phi_s
net(yaw, flow)
yaw = (yaw + np.pi) % (2 * np.pi) - np.pi
v = np.array([np.sin(yaw), np.cos(yaw)]) * route.dx
opath.append([opath[-1][0] + v[0], opath[-1][1] + v[1], yaw])
tb1.append(net.tb1)
cur += delta
seville_obs.date = cur
sun.compute(seville_obs)
theta_s = np.array([np.pi / 2 - sun.alt])
phi_s = np.array([(sun.az + np.pi) % (2 * np.pi) - np.pi])
opath = np.array(opath)
yaw -= phi_s
# inward route
ipath = [[opath[-1][0], opath[-1][1], opath[-1][2]]]
L = 0. # straight distance to the nest
C = 0. # distance towards the nest that the agent has covered
SL = 0.
TC = 0.
tb1 = []
tau.append([])
d_x.append([])
d_c.append([])
while C < 15:
theta_n, phi_n = tilt(theta_t, phi_t, theta, phi + yaw)
sun_ele.append(theta_s[0])
sun_azi.append(phi_s[0])
time.append(cur)
sky.theta_s, sky.phi_s = theta_s, phi_s
Y, P, A = sky(theta_n, phi_n, noise=noise)
if enable_ephemeris:
r_tb1, d_phi = encode(theta, phi, Y, P, A, d_phi=d_phi)
else:
r_tb1, d_phi = encode(theta, phi, Y, P, A, d_phi=0.)
_, yaw = np.pi - decode_sph(r_tb1) + phi_s
motor = net(yaw, flow)
yaw = (ipath[-1][2] + motor + np.pi) % (2 * np.pi) - np.pi
v = np.array([np.sin(yaw), np.cos(yaw)]) * route.dx
ipath.append([ipath[-1][0] + v[0], ipath[-1][1] + v[1], yaw])
tb1.append(net.tb1)
L = np.sqrt(np.square(opath[0][0] - ipath[-1][0]) + np.square(opath[0][1] - ipath[-1][1]))
C += route.dx
d_x[-1].append(L)
d_c[-1].append(C)
tau[-1].append(L / C)
if C <= route.dx:
SL = L
if TC == 0. and len(d_x[-1]) > 50 and d_x[-1][-1] > d_x[-1][-2]:
TC = C
cur += delta
seville_obs.date = cur
sun.compute(seville_obs)
theta_s = np.array([np.pi / 2 - sun.alt])
phi_s = np.array([(sun.az + np.pi) % (2 * np.pi) - np.pi])
ipath = np.array(ipath)
d_x[-1] = np.array(d_x[-1]) / SL * 100
d_c[-1] = np.array(d_c[-1]) / TC * 100
tau[-1] = np.array(tau[-1])
ri += 1
avg_time += cur - datetime(2018, 6, 21, 10, 0, 0)
stats["max_alt"].append(0.)
stats["noise"].append(noise)
stats["opath"].append(opath)
stats["ipath"].append(ipath)
stats["d_x"].append(d_x[-1])
stats["d_c"].append(d_c[-1])
stats["tau"].append(tau[-1])
print ".",
print ""
print "average time:", avg_time / ri # 1:16:40
np.savez_compressed("data/pi-stats-ephem.npz", **stats)
def create_paths(noise_type="uniform"):
global seville_obs, sun, dx
# sensor design
n = 60
omega = 56
theta, phi, fit = angles_distribution(n, float(omega))
theta_t, phi_t = 0., 0.
# sun position
seville_obs.date = datetime(2018, 6, 21, 9, 0, 0)
sun.compute(seville_obs)
theta_s = np.array([np.pi / 2 - sun.alt])
phi_s = np.array([(sun.az + np.pi) % (2 * np.pi) - np.pi])
# ant-world
noise = 0.0
ttau = .06
dx = 1e-02
routes = load_routes()
flow = dx * np.ones(2) / np.sqrt(2)
max_theta = 0.
stats = {
"max_alt": [],
"noise": [],
"opath": [],
"ipath": [],
"d_x": [],
"d_c": [],
"tau": []
}
for max_altitude in [.0, .1, .2, .3, .4, .5]:
for ni, noise in enumerate([0.0, 0.2, 0.4, 0.6, 0.8, .97]):
# stats
d_x = [] # logarithmic distance
d_c = []
tau = [] # tortuosity
ri = 0
for route in routes[::2]:
dx = route.dx
net = CX(noise=0., pontin=False)
net.update = True
# outward route
route.condition = Hybrid(tau_x=dx)
oroute = route.reverse()
x, y, yaw = [(x0, y0, yaw0) for x0, y0, _, yaw0 in oroute][0]
opath = [[x, y, yaw]]
v = np.zeros(2)
tb1 = []
for _, _, _, yaw in oroute:
theta_t, phi_t = get_3d_direction(opath[-1][0], opath[-1][1], yaw, tau=ttau)
max_theta = max_theta if max_theta > np.absolute(theta_t) else np.absolute(theta_t)
theta_n, phi_n = tilt(theta_t, phi_t, theta, phi + yaw)
sky.theta_s, sky.phi_s = theta_s, phi_s
Y, P, A = sky(theta_n, phi_n, noise=get_noise(theta_n, phi_n, noise, mode=noise_type))
r_tb1 = encode(theta, phi, Y, P, A)
yaw0 = yaw
_, yaw = np.pi - decode_sph(r_tb1) + phi_s
net(yaw, flow)
yaw = (yaw + np.pi) % (2 * np.pi) - np.pi
v = np.array([np.sin(yaw), np.cos(yaw)]) * route.dx
opath.append([opath[-1][0] + v[0], opath[-1][1] + v[1], yaw])
tb1.append(net.tb1)
opath = np.array(opath)
yaw -= phi_s
# inward route
ipath = [[opath[-1][0], opath[-1][1], opath[-1][2]]]
L = 0. # straight distance to the nest
C = 0. # distance towards the nest that the agent has covered
SL = 0.
TC = 0.
tb1 = []
tau.append([])
d_x.append([])
d_c.append([])
while C < 15:
theta_t, phi_t = get_3d_direction(ipath[-1][0], ipath[-1][1], yaw, tau=ttau)
theta_n, phi_n = tilt(theta_t, phi_t, theta, phi + yaw)
sky.theta_s, sky.phi_s = theta_s, phi_s
Y, P, A = sky(theta_n, phi_n, noise=noise)
r_tb1 = encode(theta, phi, Y, P, A)
_, yaw = np.pi - decode_sph(r_tb1) + phi_s
motor = net(yaw, flow)
yaw = (ipath[-1][2] + motor + np.pi) % (2 * np.pi) - np.pi
v = np.array([np.sin(yaw), np.cos(yaw)]) * route.dx
ipath.append([ipath[-1][0] + v[0], ipath[-1][1] + v[1], yaw])
tb1.append(net.tb1)
L = np.sqrt(np.square(opath[0][0] - ipath[-1][0]) + np.square(opath[0][1] - ipath[-1][1]))
C += route.dx
d_x[-1].append(L)
d_c[-1].append(C)
tau[-1].append(L / C)
if C <= route.dx:
SL = L
if TC == 0. and len(d_x[-1]) > 50 and d_x[-1][-1] > d_x[-1][-2]:
TC = C
ipath = np.array(ipath)
d_x[-1] = np.array(d_x[-1]) / SL * 100
d_c[-1] = np.array(d_c[-1]) / TC * 100
tau[-1] = np.array(tau[-1])
ri += 1
stats["max_alt"].append(max_altitude)
stats["noise"].append(noise)
stats["opath"].append(opath)
stats["ipath"].append(ipath)
stats["d_x"].append(d_x[-1])
stats["d_c"].append(d_c[-1])
stats["tau"].append(tau[-1])
np.savez_compressed("../data/pi-stats-%s.npz" % noise_type, **stats)
theta_t, phi_t = get_3d_direction(opath[-1][0],
opath[-1][1],
yaw,
tau=ttau)
max_theta = max_theta if max_theta > np.absolute(
theta_t) else np.absolute(theta_t)
theta_n, phi_n = tilt(theta_t, phi_t, theta, phi + yaw)
sky.theta_s, sky.phi_s = theta_s, phi_s
eta = get_noise(theta, phi, noise, noise_type)
Y, P, A = sky(theta_n, phi_n, noise=eta)
r_tb1 = encode(theta, phi, Y, P, A)
yaw0 = yaw
_, yaw = np.pi - decode_sph(r_tb1) + phi_s
# plt.plot(yaw0 % (2*np.pi), yaw % (2*np.pi), 'k.')
# motor = net(r_tb1, flow)
net(yaw, flow)
yaw = (yaw + np.pi) % (2 * np.pi) - np.pi
v = np.array([np.sin(yaw), np.cos(yaw)]) * route.dx
opath.append([opath[-1][0] + v[0], opath[-1][1] + v[1], yaw])
tb1.append(net.tb1)
# plt.xlabel("org")
# plt.ylabel("com")
# plt.xlim([0, 2*np.pi])
# plt.ylim([0, 2*np.pi])
# plt.show()
opath = np.array(opath)
def decode_sun(x):
lat, lon = decode_sph(x)
return lon, lat
def visualise_compnet(sensor, sL=None, interactive=True, cmap="coolwarm", vmin=0, vmax=1, title=None):
"""
:param sensor:
:type sensor: CompassSensor
:return:
"""
if interactive and not plt.isinteractive():
plt.ion()
if isinstance(cmap, basestring):
cmap = get_cmap(cmap)
xyz = sph2vec(np.pi/2 - sensor.theta_local, np.pi + sensor.phi_local, sensor.R_c)
xyz[0] *= -1
lat, lon = hp.Rotator(rot=(
np.rad2deg(-sensor.yaw), np.rad2deg(-sensor.pitch), np.rad2deg(-sensor.roll)
))(sensor.sky.lat, np.pi-sensor.sky.lon)
xyz_sun = sph2vec(np.pi/2 - lat, np.pi + lon, sensor.R_c)
xyz_sun[0] *= -1
fig = plt.figure("Compass-CompModel" if title is None or interactive else title, figsize=(12, 7))
fig.clear()
ax_t = plt.subplot2grid((1, 12), (0, 0), colspan=10)
outline = Ellipse(xy=np.zeros(2),
width=2 * sensor.R_c,
height=2 * sensor.R_c)
sensor_outline = Ellipse(xy=np.zeros(2),
width=2 * sensor.alpha + 2 * sensor.R_l,
height=2 * sensor.alpha + 2 * sensor.R_l)
ax_t.add_patch(outline)
outline.set_clip_box(ax_t.bbox)
outline.set_alpha(.2)
outline.set_facecolor("grey")
stheta, sphi = sensor.theta_local, np.pi + sensor.phi_local
sL = np.clip(sL if sL is not None else sensor.L, 0., 1.)
for k, ((x, y, z), th, ph, L) in enumerate(zip(xyz.T, stheta, sphi, sL)):
for j, tl2 in enumerate(sensor.tl2):
line = ConnectionPatch([x, y], [sensor.R_c + 5, j * sensor.R_c / 8. - sensor.R_c + 1],
"data", "data", lw=.5, color=cmap(np.asscalar(L) * sensor.w_tl2[k, j] + .5))
ax_t.add_patch(line)
ax_t.add_patch(sensor_outline)
sensor_outline.set_clip_box(ax_t.bbox)
sensor_outline.set_alpha(.5)
sensor_outline.set_facecolor("grey")
for k, ((x, y, z), th, ph, L) in enumerate(zip(xyz.T, stheta, sphi, sL)):
lens = Ellipse(xy=[x, y], width=1.5 * sensor.r_l, height=1.5 * np.cos(th) * sensor.r_l,
angle=np.rad2deg(ph))
ax_t.add_patch(lens)
lens.set_clip_box(ax_t.bbox)
lens.set_facecolor(cmap(np.asscalar(L)))
scale = 5.
for j, tl2 in enumerate(sensor.tl2):
x = sensor.R_c + 5
y = j * sensor.R_c / 8. - sensor.R_c + 1
for k, cl1 in enumerate(sensor.cl1):
line = ConnectionPatch([x, y], [sensor.R_c + 10, k * sensor.R_c / 8. - sensor.R_c + 1],
"data", "data", lw=.5, color=cmap(scale * tl2 * sensor.w_cl1[j, k] + .5))
ax_t.add_patch(line)
neuron = Ellipse(xy=[x, y], width=.1 * sensor.R_c, height=.1 * sensor.R_c)
ax_t.add_artist(neuron)
neuron.set_clip_box(ax_t.bbox)
neuron.set_facecolor(cmap(scale * tl2 + .5))
for j, cl1 in enumerate(sensor.cl1):
x = sensor.R_c + 10
y = j * sensor.R_c / 8. - sensor.R_c + 1
for k, tb1 in enumerate(sensor.tb1):
line = ConnectionPatch([x, y], [sensor.R_c + 15, k * sensor.R_c / 8. - sensor.R_c / 2. + 1],
"data", "data", lw=.5, color=cmap(scale * cl1 * sensor.w_tb1[j, k] + .5))
ax_t.add_patch(line)
neuron = Ellipse(xy=[x, y], width=.1 * sensor.R_c, height=.1 * sensor.R_c)
ax_t.add_artist(neuron)
neuron.set_clip_box(ax_t.bbox)
neuron.set_facecolor(cmap(scale * cl1 + .5))
for j, tb1 in enumerate(sensor.tb1):
x = sensor.R_c + 15
y = j * sensor.R_c / 8. - sensor.R_c / 2 + 1
neuron = Ellipse(xy=[x, y], width=.1 * sensor.R_c, height=.1 * sensor.R_c)
ax_t.add_artist(neuron)
neuron.set_clip_box(ax_t.bbox)
neuron.set_facecolor(cmap(scale * tb1 + .5))
ax_t.text(0, sensor.R_c - sensor.r_l, "0", fontsize=15, verticalalignment='center', horizontalalignment='center')
ax_t.text(-sensor.R_c + sensor.r_l, 0, "90", fontsize=15, verticalalignment='center', horizontalalignment='center')
ax_t.text(0, -sensor.R_c + sensor.r_l, "180", fontsize=15, verticalalignment='center', horizontalalignment='center')
ax_t.text(sensor.R_c - sensor.r_l, 0, "-90", fontsize=15, verticalalignment='center', horizontalalignment='center')
norm = np.sqrt(np.square(xyz_sun[0]) + np.square(xyz_sun[1])) / (sensor.R_c - 3 * sensor.r_l)
ax_t.plot([-xyz_sun[0]/norm, xyz_sun[0]/norm], [-xyz_sun[1]/norm, xyz_sun[1]/norm], "k-")
ax_t.plot(xyz_sun[0], xyz_sun[1],
marker='o',
fillstyle='full',
markeredgecolor='black',
markerfacecolor='yellow',
markersize=15)
ax_t.text(0, 0, "z", fontsize=15, verticalalignment='center', horizontalalignment='center',
color='red', fontweight='bold')
ax_t.set_xlim(-sensor.R_c - 2, sensor.R_c + 17)
ax_t.set_ylim(-sensor.R_c - 2, sensor.R_c + 2)
ax_t.set_xticklabels([])
ax_t.set_yticklabels([])
plt.axis('off')
plt.subplot2grid((4, 12), (1, 10), colspan=2, rowspan=2)
theta, phi = decode_sph(sensor.tb1)
alpha = np.linspace(0, 2*np.pi, 100)
plt.plot(theta * np.sin(phi + alpha + np.pi/2), alpha)
plt.ylabel("phase (degrees)")
plt.ylim([0, 2*np.pi])
plt.xlim([theta+np.pi/360, -theta-np.pi/360])
plt.xticks([-theta, 0, theta], ["%d" % np.rad2deg(-theta), "0", "%d" % np.rad2deg(theta)])
plt.yticks([0, np.pi/2, np.pi, 3*np.pi/2, 2*np.pi], ["0", "90", "180", "270", "360"])
plt.tight_layout(pad=0.)
if interactive:
plt.draw()
plt.pause(.1)
else:
plt.show()