def choose_trajectory(self, t):
''' Select the best trajectory avaliable to the robot at the current pose, according to the aquisition function.
Input:
t (int > 0): the current planning iteration (value of a point can change with algortihm progress)
Output:
either None or the (best path, best path value, all paths, all values, the max_locs for some functions)
'''
value = {}
param = None
max_locs = max_vals = None
if self.f_rew == 'mes' or self.f_rew == 'maxs-mes':
self.max_val, self.max_locs, self.target = aqlib.sample_max_vals(self.GP, t=t, visualize=True,
f_rew=self.f_rew,
obstacles=self.obstacle_world)
elif self.f_rew == 'naive' or self.f_rew == 'naive_value':
self.max_val, self.max_locs, self.target = aqlib.sample_max_vals(self.GP, t=t,
obstacles=self.obstacle_world,
visualize=True, f_rew=self.f_rew,
nK=int(self.sample_num))
param = ((self.max_val, self.max_locs, self.target), self.sample_radius)
pred_loc, pred_val = self.predict_max(t=t)
paths, true_paths = self.path_generator.get_path_set(self.loc)
for path, points in paths.items():
# set params
if self.f_rew == 'mes' or self.f_rew == 'maxs-mes':
param = (self.max_val, self.max_locs, self.target)
elif self.f_rew == 'exp_improve':
if len(self.maxes) == 0:
param = [self.current_max]
else:
param = self.maxes
# get costs
cost = 100.0
if self.use_cost == True:
cost = float(self.path_generator.path_cost(true_paths[path]))
if cost == 0.0:
cost = 100.0
# set the points over which to determine reward
if self.path_option == 'fully_reachable_goal' and self.goal_only == True:
poi = [(points[-1][0], points[-1][1])]
elif self.path_option == 'fully_reachable_step' and self.goal_only == True:
poi = [(self.goals[path][0], self.goals[path][1])]
else:
poi = points
if self.use_cost == False:
value[path] = self.aquisition_function(time=t, xvals=poi, robot_model=self.GP, param=param)
else:
reward = self.aquisition_function(time=t, xvals=poi, robot_model=self.GP, param=param)
value[path] = reward / cost
try:
best_key = np.random.choice([key for key in value.keys() if value[key] == max(value.values())])
return paths[best_key], true_paths[best_key], value[best_key], paths, value, self.max_locs
except:
return None
def star_max_dist(GP, true_loc, true_val):
# If no observations have been collected, return default value
if GP.xvals is None: #TODO: remember to change this
print "Skipping star analysis prediction!"
return 0.0, 0.0, 0.0, 0.0
max_vals, max_locs, func = aqlib.sample_max_vals(GP, t = 0, nK = 20)
max_vals = np.array(max_vals).reshape((-1, 1))
max_locs = np.array(max_locs).reshape((-1, 2))
np.savetxt('./sl_sampled_maxes.csv', np.vstack((max_locs.T, max_vals.T)))
SAVE_FLAG = True
true_loc = np.array(true_loc).reshape((-1, 2))
true_val = np.array(true_val).reshape((-1, 1))
# Compute average distance from stars to true loc
dist_loc = distance.cdist(max_locs, true_loc, 'euclidean')
dist_val = distance.cdist(max_vals, true_val, 'euclidean')
NBINS = 50
RANGE = np.array([(ranges[0], ranges[1]), (ranges[2], ranges[3])])
# Create the star heatmap
if SAVE_FLAG:
plt.figure(figsize=(8,8))
# plt.hist2d(max_locs[:, 0], max_locs[:, 1], bins = NBINS, normed = True, range = RANGE, cmap = 'magma', norm=mcolors.LogNorm())
plt.hist2d(max_locs[:, 0], max_locs[:, 1], bins = NBINS, normed = True, range = RANGE, cmap = 'viridis')
plt.colorbar()
plt.savefig('./star_heatmap_test.png')
# plt.show()
plt.close()
# Compute the histrogram entropy of the star distribution
ALPHA = 0.99
hist, xbins, ybins = np.histogram2d(max_locs[:, 0], max_locs[:, 1], bins = NBINS, normed = True, range = RANGE)
uniform = np.ones(hist.shape) / np.sum(np.ones(hist.shape))
histnorm = ALPHA * hist + (1. - ALPHA) * uniform
histnorm = histnorm / np.sum(histnorm)
entropy_x = -np.sum(histnorm[histnorm > 0.0] * np.log(histnorm[histnorm > 0.0]))
# Uniform santiy check
# uniform = np.ones(hist.shape) / np.sum(np.ones(hist.shape))
# unifrom_entropy = -np.sum(uniform[uniform > 0.0] * np.log(uniform[uniform > 0.0]))
# print "Entropy of a uniform distribution:", unifrom_entropy
hist_z, xbins_z = np.histogram(max_vals, bins = NBINS, density = True)
uniform = np.ones(hist_z.shape) / np.sum(np.ones(hist_z.shape))
hist_z = hist_z / np.sum(hist_z)
entropy_z = -np.mean(np.log(hist_z[hist_z > 0.0]))
return np.mean(dist_loc), np.mean(dist_val), entropy_x, entropy_z
print "Star entropy error in x, z:", h_x, h_z
''' Compute proportions of data witihin delta-epsilon regions '''
samp_dist_loc = distance.cdist(robot.GP.xvals, true_loc, 'euclidean')
samp_dist_val = distance.cdist(robot.GP.zvals, true_val, 'euclidean')
pdb.set_trace()
print samp_dist_loc[samp_dist_loc < 10.0]
print samp_dist_val[samp_dist_val < 0.5]
prop_x = float(len(samp_dist_loc[samp_dist_loc < 10.])) / float(len(samp_dist_loc))
prop_z = float(len(samp_dist_val[samp_dist_val < 0.5])) / float(len(samp_dist_val))
print "Proportion in x, z:", prop_x, prop_z
max_vals, max_locs, func = aqlib.sample_max_vals(robot.GP, t = 0, nK = 20, obstacles = ow)
max_vals= np.array(max_vals).reshape((-1, 1))
max_locs = np.array(max_locs).reshape((-1, 2))
dist_loc = distance.cdist(max_locs, true_loc, 'euclidean')
dist_val = distance.cdist(max_vals, true_val, 'euclidean')
np.savetxt('./sampled_maxes.csv', np.vstack((max_locs.T, max_vals.T)))
np.savetxt('./true_maxes.csv', np.vstack((true_loc.T, true_val.T)))
print "Distance mean location:", np.mean(dist_loc), "\t Value:", np.mean(dist_val)
loc_kernel = sp.stats.gaussian_kde(max_locs.T)
loc_kernel.set_bandwidth(bw_method=LEN)
def star_max_dist(xvals,
zvals,
true_loc,
true_val,
PATH,
ranges=[0.0, 10.0, 0.0, 10.0],
LEN=1.0,
VAR=100.0,
NOISE=0.5):
# If no observations have been collected, return default value
if xvals is None: #TODO: remember to change this
print "Skipping star analysis prediction!"
return 0.0, 0.0, 0.0, 0.0
GP = gplib.GPModel(ranges=ranges,
lengthscale=LEN,
variance=VAR,
noise=NOISE)
GP.add_data(xvals, zvals)
# If files already exist, simply read in. Othrewise, sample maxima
# and create files.
try:
sampled_maxes = np.loadtxt(os.path.join(PATH,
'sampled_maxes_dist.csv')).T
max_locs = sampled_maxes[:, 0:2].reshape((-1, 2))
max_vals = sampled_maxes[:, 2].reshape((-1, 1))
SAVE_FLAG = False
except:
max_vals, max_locs, func = aqlib.sample_max_vals(GP, t=0, nK=20)
max_vals = np.array(max_vals).reshape((-1, 1))
max_locs = np.array(max_locs).reshape((-1, 2))
np.savetxt(os.path.join(PATH, 'sampled_maxes_dist.csv'),
np.vstack((max_locs.T, max_vals.T)))
SAVE_FLAG = True
true_loc = np.array(true_loc).reshape((-1, 2))
true_val = np.array(true_val).reshape((-1, 1))
# Compute average distance from stars to true loc
dist_loc = distance.cdist(max_locs, true_loc, 'euclidean')
dist_val = distance.cdist(max_vals, true_val, 'euclidean')
NBINS = 50
RANGE = np.array([(ranges[0], ranges[1]), (ranges[2], ranges[3])])
# Create the star heatmap
if SAVE_FLAG:
plt.figure(figsize=(8, 8))
# plt.hist2d(max_locs[:, 0], max_locs[:, 1], bins = NBINS, normed = True, range = RANGE, cmap = 'magma', norm=mcolors.LogNorm())
plt.hist2d(max_locs[:, 0],
max_locs[:, 1],
bins=NBINS,
normed=True,
range=RANGE,
cmap='viridis')
plt.colorbar()
plt.savefig(os.path.join(PATH, 'star_heatmap.png'))
# plt.show()
plt.close()
# Compute the histrogram entropy of the star distribution
ALPHA = 0.99
hist, xbins, ybins = np.histogram2d(max_locs[:, 0],
max_locs[:, 1],
bins=NBINS,
normed=True,
range=RANGE)
uniform = np.ones(hist.shape) / np.sum(np.ones(hist.shape))
histnorm = ALPHA * hist + (1. - ALPHA) * uniform
histnorm = histnorm / np.sum(histnorm)
entropy_x = -np.sum(
histnorm[histnorm > 0.0] * np.log(histnorm[histnorm > 0.0]))
plt.imshow(hist)
plt.show()
# Uniform santiy check
# uniform = np.ones(hist.shape) / np.sum(np.ones(hist.shape))
# unifrom_entropy = -np.sum(uniform[uniform > 0.0] * np.log(uniform[uniform > 0.0]))
# print "Entropy of a uniform distribution:", unifrom_entropy
hist_z, xbins_z = np.histogram(max_vals, bins=NBINS, density=True)
uniform = np.ones(hist_z.shape) / np.sum(np.ones(hist_z.shape))
hist_z = hist_z / np.sum(hist_z)
entropy_z = -np.mean(np.log(hist_z[hist_z > 0.0]))
# Save statistics
if SAVE_FLAG:
np.savetxt(
os.path.join(PATH, 'star_stats.csv'),
np.array(
[np.mean(dist_loc),
np.mean(dist_val), entropy_x, entropy_z]))
np.savetxt(os.path.join(PATH, 'star_loc_variance.csv'),
np.array(dist_val))
np.savetxt(os.path.join(PATH, 'star_val_variance.csv'),
np.array(dist_loc))
return np.mean(dist_loc), np.mean(dist_val), entropy_x, entropy_z