def __init__(self,
compass=CompassSensor(nb_lenses=60,
fov=np.deg2rad(COMPASS_FOV)),
*args,
**kwargs):
"""
:param init_pos: the initial position
:type init_pos: np.ndarray
:param init_rot: the initial orientation
:type init_rot: np.ndarray
:param condition:
:type condition: Hybrid
:param live_sky: flag to update the sky with respect to the time
:type live_sky: bool
:param rgb: flag to set as input to the network all the channels (otherwise use only green)
:type rgb: bool
:param compass: the compass sensor
:type compass: CompassSensor
:param visualiser:
:type visualiser: Visualiser
:param name: a name for the agent
:type name: string
"""
if 'fov' in kwargs.keys() and kwargs['fov'] is None:
kwargs['fov'] = CXAgent.FOV
super(CXAgent, self).__init__(*args, **kwargs)
self._net = CX(noise=0., pontin=False)
self.compass = compass
self.bump_shift = 0.
self.log = CXLogger()
try:
self.compass.load_weights()
except Exception as e:
print("No parameters found for compass.")
print(e)
if 'name' in kwargs.keys() and kwargs['name'] is None:
self.name = "cx_agent_%02d" % self.id
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)
class CXAgent(Agent):
FOV = (-np.pi / 6, np.pi / 3)
COMPASS_FOV = 60
def __init__(self,
compass=CompassSensor(nb_lenses=60,
fov=np.deg2rad(COMPASS_FOV)),
*args,
**kwargs):
"""
:param init_pos: the initial position
:type init_pos: np.ndarray
:param init_rot: the initial orientation
:type init_rot: np.ndarray
:param condition:
:type condition: Hybrid
:param live_sky: flag to update the sky with respect to the time
:type live_sky: bool
:param rgb: flag to set as input to the network all the channels (otherwise use only green)
:type rgb: bool
:param compass: the compass sensor
:type compass: CompassSensor
:param visualiser:
:type visualiser: Visualiser
:param name: a name for the agent
:type name: string
"""
if 'fov' in kwargs.keys() and kwargs['fov'] is None:
kwargs['fov'] = CXAgent.FOV
super(CXAgent, self).__init__(*args, **kwargs)
self._net = CX(noise=0., pontin=False)
self.compass = compass
self.bump_shift = 0.
self.log = CXLogger()
try:
self.compass.load_weights()
except Exception as e:
print("No parameters found for compass.")
print(e)
if 'name' in kwargs.keys() and kwargs['name'] is None:
self.name = "cx_agent_%02d" % self.id
@property
def sky(self):
return self.world.sky
@sky.setter
def sky(self, value):
self.world.sky = value
def reset(self):
if super(CXAgent, self).reset():
# reset to the nest instead of the feeder
self.pos[:2] = self.nest.copy()
self.yaw = (self.homing_routes[-1].phi[-2] + np.pi) % (2 * np.pi)
self._net.update = True
return True
else:
return False
def start_learning_walk(self):
if self.world is None:
# TODO: warn about not setting the world
return None
elif len(self.homing_routes) == 0:
# TODO: warn about not setting the homing route
return None
print("Resetting...")
self.reset()
self.log.stage = "training"
# initialise visualisation
if self.visualiser is not None:
self.visualiser.reset()
# follow a reverse homing route
rt = self.homing_routes[-1].reverse() # type: Route
# add a copy of the current route to the world to visualise the path
self.log.add(self.pos[:3], self.yaw)
self.world.routes.append(
route_like(rt,
self.log.x,
self.log.y,
self.log.z,
self.log.phi,
self.condition,
agent_no=self.id - 1,
route_no=1))
counter = 0 # count the steps
phi_ = (np.array([np.pi - phi for _, _, _, phi in rt]) +
np.pi) % (2 * np.pi) - np.pi
# phi_ = np.roll(phi_, 1) # type: np.ndarray
for phi in phi_:
if not self.step(phi, counter):
break
counter += 1
self.log.update_outbound_end()
# remove the copy of the route from the world
rt = self.world.routes[-1]
self.world.routes.remove(self.world.routes[-1])
return rt # return the learned route
def start_homing(self, reset=True):
if self.world is None:
# TODO: warn about not setting the world
return None
if reset:
print("Resetting...")
super(CXAgent, self).reset()
self.log.stage = "homing"
# initialise the visualisation
if self.visualiser is not None:
self.visualiser.reset()
phi, _ = self.update_state(np.pi - self.yaw)
# add a copy of the current route to the world to visualise the path
self.world.routes.append(
route_like(self.world.routes[0],
self.log.x,
self.log.y,
self.log.z,
self.log.phi,
agent_no=self.id,
route_no=len(self.world.routes) + 1))
counter = 0
start_time = datetime.now()
while self.d_nest > 0.1:
if not self.step(phi, counter, start_time):
break
phi = np.pi - self.yaw
counter += 1
# remove the copy of the route from the world
self.world.routes.remove(self.world.routes[-1])
return Route(self.log.x,
self.log.y,
self.log.z,
self.log.phi,
condition=self.condition,
agent_no=self.id,
route_no=len(self.world.routes) + 1)
def step(self, phi, counter=0, start_time=None, use_flow=False):
# stop the loop when we close the visualisation window
if self.visualiser is not None and self.visualiser.is_quit():
return False
sun = self.read_sensor()
if isinstance(sun, np.ndarray) and sun.size == 8:
heading = decode_sun(sun)[0]
else:
# heading = self.compass.sky.lon-self.yaw
heading = sun
if use_flow:
self.world_snapshot()
self.log.snap.append(self.world.eye.L[:, 0].flatten())
# TODO: fix error in the optic-flow calculation
# flow = self.get_flow(self.log.snap[-1], self.log.snap[-2] if len(self.log.snap) > 1 else None)
v_trans = self.dx * np.array([np.sin(heading), np.cos(heading)])
flow = self._net.get_flow(heading, v_trans)
else:
flow = self.dx * np.ones(2) / np.sqrt(2)
# make a forward pass from the network
if isinstance(sun, np.ndarray) and sun.size == 8:
motor = self._net(sun,
flow,
tl2=self.compass.tl2,
cl1=self.compass.cl1)
else:
motor = self._net(sun, flow)
phi, v = self.update_state(phi, rotation=motor)
v_trans = self.transform_velocity(heading, v)
self.log.update_hist(tl2=self._net.tl2,
cl1=self._net.cl1,
tb1=self._net.tb1,
cpu4=self._net.cpu4_mem,
cpu1=self._net.cpu1,
tn1=self._net.tn1,
tn2=self._net.tn2,
motor0=motor,
flow0=flow,
v0=v,
v1=v_trans,
phi=phi,
sun=heading)
# update the route in the world
self.world.routes[-1] = route_like(self.world.routes[-1], self.log.x,
self.log.y, self.log.z,
self.log.phi)
# update view
img_func = None
if self.visualiser is not None and self.visualiser.mode == "top":
img_func = self.world.draw_top_view
elif self.visualiser is not None and self.visualiser.mode == "panorama":
img_func = self.world_snapshot
if self.visualiser is not None:
names = self.name.split('_')
names[0] = self.world.date.strftime(datestr)
names.append(counter)
names.append(self.d_feeder)
names.append(self.d_nest)
names.append(np.rad2deg(motor))
n = 4
if start_time is not None:
now = datetime.now()
now = now - start_time
names.append(now.seconds // 60)
names.append(now.seconds % 60)
n += 2
capt_format = "%s " * (
len(names) -
n) + "| C: % 2d D_f: % 2.2f D_n: % 2.2f MTR: % 3.1f | "
if start_time is not None:
capt_format += " | Elapsed time: %02d:%02d"
self.visualiser.update_main(img_func,
caption=capt_format % tuple(names))
d_max = 2 * np.sqrt(np.square(self.feeder - self.nest).sum())
if self.d_feeder > d_max and self.d_nest > d_max or counter > 20 / self.dx:
return False
return True
def get_flow(self, now, previous=None):
if previous is not None:
flow = -get_sph_flow(
n_val=np.array(now).flatten(),
o_val=np.array(previous).flatten(),
rdir=np.array([
self.world.eye.theta_global, self.world.eye.phi_global
]).T,
rsensor=np.array(
[[0, self._net.tn_prefs], [0, -self._net.tn_prefs]])).mean(
axis=1) # TODO: scale the value so that it fits better
flow = self.dx * flow / np.sqrt(np.square(flow).sum())
else:
flow = self.dx * np.ones(2)
# flow = self._net.get_flow(heading, v_trans)
return flow
def read_sensor(self, decode=False):
self.compass.rotate(yaw=self.yaw - self.compass.yaw,
# pitch=self.pitch - self.compass.pitch
)
self.compass.refresh()
if decode:
# sun = self.compass.facing_direction - self.world.sky.lon
sun = self.compass(self.world.sky, decode=decode).flatten()
sun = (sun[0] + np.pi) % (2 * np.pi) - np.pi
else:
sun = self.compass(self.world.sky, decode=decode).flatten()
# sun = (sun / np.absolute(sun).max() + 1.) / 2.
return sun
def transform_velocity(self, heading, velocity):
"""
:param heading:
:param velocity:
:type velocity: np.ndarray
:return:
"""
# R = np.array([[np.sin(heading), -np.cos(heading)],
# [np.cos(heading), np.sin(heading)]]).T
r = np.sqrt(np.square(velocity).sum())
return self.get_velocity(heading, r)
# stats
d_x = [] # logarithmic distance
d_c = []
tau = [] # tortuosity
ri = 0
print "Noise:", noise
sky.theta_s, sky.phi_s = np.pi / 6, np.pi
eta = get_noise(theta_sky, phi_sky, noise, noise_type)
Y, P, A = sky(theta_sky, phi_sky, noise=eta)
plt.figure("Sky-%d-%s" % (noise * 10, noise_type), figsize=(7.5, 2.5))
plot_sky(phi_sky, theta_sky, Y, P, A)
# for route in routes[::2]:
for route in [routes[0]]:
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 = []
# print np.rad2deg(phi_s)
# plt.figure("yaws")
for _, _, _, yaw in oroute:
if mode == "uneven":