def predict_max(xvals,
zvals,
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 or True: # TODO: remeber to change this!
if xvals is None or True: # TODO: remeber to change this!
print "Skipping maxima prediction!"
return np.array([0., 0.]), 0.
GP = gplib.GPModel(ranges=ranges,
lengthscale=LEN,
variance=VAR,
noise=NOISE)
GP.add_data(xvals, zvals)
''' First option, return the max value observed so far '''
#return self.GP.xvals[np.argmax(GP.zvals), :], np.max(GP.zvals)
''' Second option: generate a set of predictions from model and return max '''
# Generate a set of observations from robot model with which to predict mean
x1vals = np.linspace(ranges[0], ranges[1], 100)
x2vals = np.linspace(ranges[2], ranges[3], 100)
x1, x2 = np.meshgrid(x1vals, x2vals, sparse=False, indexing='xy')
data = np.vstack([x1.ravel(), x2.ravel()]).T
observations, var = GP.predict_value(data)
max_loc, max_val = data[np.argmax(observations), :], np.max(observations)
# fig2, ax2 = plt.subplots(figsize=(8, 8))
# plot = ax2.contourf(x1, x2, observations.reshape(x1.shape), 25, cmap = 'viridis')
# scatter = ax2.scatter(GP.xvals[:, 0], GP.xvals[:, 1], c='k', s = 20.0, cmap = 'viridis')
# scatter = ax2.scatter(data[:, 0], data[:, 1], c='b', s = 10.0, cmap = 'viridis')
# scatter = ax2.scatter(max_loc[0], max_loc[1], c='r', s = 20.0, cmap = 'viridis')
# plt.show()
return max_loc, max_val
def __init__(self,
sample_world,
start_loc=(0.0, 0.0, 0.0),
extent=(-10., 10., -10., 10.),
kernel_file=None,
kernel_dataset=None,
prior_dataset=None,
init_lengthscale=10.0,
init_variance=100.0,
noise=0.05,
path_generator='default',
frontier_size=6,
horizon_length=5,
turning_radius=1,
sample_step=0.5,
evaluation=None,
f_rew='mean',
create_animation=False,
learn_params=False,
nonmyopic=False,
computation_budget=10,
rollout_length=5,
discretization=(10, 10),
use_cost=False,
MIN_COLOR=-25.,
MAX_COLOR=25.,
goal_only=False,
obstacle_world=obslib.FreeWorld(),
tree_type='dpw'):
''' Initialize the robot class with a GP model, initial location, path sets, and prior dataset
Inputs:
sample_world (method) a function handle that takes a set of locations as input and returns a set of observations
start_loc (tuple of floats) the location of the robot initially in 2-D space e.g. (0.0, 0.0, 0.0)
extent (tuple of floats): a tuple representing the max/min of 2D rectangular domain i.e. (-10, 10, -50, 50)
kernel_file (string) a filename specifying the location of the stored kernel values
kernel_dataset (tuple of nparrays) a tuple (xvals, zvals), where xvals is a Npoint x 2 nparray of type float and zvals is a Npoint x 1 nparray of type float
prior_dataset (tuple of nparrays) a tuple (xvals, zvals), where xvals is a Npoint x 2 nparray of type float and zvals is a Npoint x 1 nparray of type float
init_lengthscale (float) lengthscale param of kernel
init_variance (float) variance param of kernel
noise (float) the sensor noise parameter of kernel
path_generator (string): one of default, dubins, or equal_dubins. Robot path parameterization.
frontier_size (int): the number of paths in the generated path set
horizon_length (float): the length of the paths generated by the robot
turning_radius (float): the turning radius (in units of distance) of the robot
sample_set (float): the step size (in units of distance) between sequential samples on a trajectory
evaluation (Evaluation object): an evaluation object for performance metric compuation
f_rew (string): the reward function. One of {hotspot_info, mean, info_gain, exp_info, mes}
create_animation (boolean): save the generate world model and trajectory to file at each timestep
'''
# Parameterization for the robot
self.ranges = extent
self.create_animation = create_animation
self.eval = evaluation
self.loc = start_loc
self.sample_world = sample_world
self.f_rew = f_rew
self.fs = frontier_size
self.discretization = discretization
self.tree_type = tree_type
self.maxes = []
self.current_max = -1000
self.current_max_loc = [0, 0]
self.max_locs = None
self.max_val = None
self.target = None
self.noise = noise
self.learn_params = learn_params
self.use_cost = use_cost
if f_rew == 'hotspot_info':
self.aquisition_function = aqlib.hotspot_info_UCB
elif f_rew == 'mean':
self.aquisition_function = aqlib.mean_UCB
elif f_rew == 'info_gain':
self.aquisition_function = aqlib.info_gain
elif f_rew == 'mes':
self.aquisition_function = aqlib.mves
elif f_rew == 'maxs-mes':
self.aquisition_function = aqlib.mves_maximal_set
elif f_rew == 'exp_improve':
self.aquisition_function = aqlib.exp_improvement
else:
raise ValueError(
'Only \'hotspot_info\' and \'mean\' and \'info_gain\' and \'mes\' and \'exp_improve\' reward fucntions supported.'
)
# Initialize the robot's GP model with the initial kernel parameters
# self.GP = gplib.OnlineGPModel(ranges = extent, lengthscale = init_lengthscale, variance = init_variance, noise = self.noise)
# self.GP = gplib.SpatialGPModel(ranges = extent, lengthscale = init_lengthscale, variance = init_variance, noise = self.noise)
# self.GP = gplib.SubsampledGPModel(ranges = extent, lengthscale = init_lengthscale, variance = init_variance, noise = self.noise)
self.GP = gplib.GPModel(ranges=extent,
lengthscale=init_lengthscale,
variance=init_variance,
noise=self.noise)
# If both a kernel training dataset and a prior dataset are provided, train the kernel using both
if kernel_dataset is not None and prior_dataset is not None:
data = np.vstack([prior_dataset[0], kernel_dataset[0]])
observations = np.vstack([prior_dataset[1], kernel_dataset[1]])
self.GP.train_kernel(data, observations, kernel_file)
# Train the kernel using the provided kernel dataset
elif kernel_dataset is not None:
self.GP.train_kernel(kernel_dataset[0], kernel_dataset[1],
kernel_file)
# If a kernel file is provided, load the kernel parameters
elif kernel_file is not None:
self.GP.load_kernel()
# No kernel information was provided, so the kernel will be initialized with provided values
else:
pass
# Incorporate the prior dataset into the model
if prior_dataset is not None:
self.GP.add_data(prior_dataset[0], prior_dataset[1])
# The path generation class for the robot
path_options = {
'default':
pathlib.Path_Generator(frontier_size, horizon_length,
turning_radius, sample_step, self.ranges,
obstacle_world),
'dubins':
pathlib.Dubins_Path_Generator(frontier_size, horizon_length,
turning_radius, sample_step,
self.ranges, obstacle_world),
'equal_dubins':
pathlib.Dubins_EqualPath_Generator(frontier_size, horizon_length,
turning_radius, sample_step,
self.ranges, obstacle_world),
'fully_reachable_goal':
pathlib.Reachable_Frontier_Generator(extent, discretization,
sample_step, turning_radius,
horizon_length,
obstacle_world),
'fully_reachable_step':
pathlib.Reachable_Step_Generator(extent, discretization,
sample_step, turning_radius,
horizon_length, obstacle_world)
}
self.path_generator = path_options[path_generator]
self.path_option = path_generator
self.nonmyopic = nonmyopic
self.comp_budget = computation_budget
self.roll_length = rollout_length
self.step_size = horizon_length
self.sample_step = sample_step
self.turning_radius = turning_radius
self.MIN_COLOR = MIN_COLOR
self.MAX_COLOR = MAX_COLOR
x1vals = np.linspace(extent[0], extent[1], discretization[0])
x2vals = np.linspace(extent[2], extent[3], discretization[1])
x1, x2 = np.meshgrid(x1vals, x2vals, sparse=False, indexing='xy')
self.goals = np.vstack([x1.ravel(), x2.ravel()]).T
self.goal_only = goal_only
self.obstacle_world = obstacle_world
xobs, zobs = baglib.read_bagfile(seed_bag, 1)
trunc_index = baglib.truncate_by_distance(xobs, dist_lim = 1000.0)
print "Lawnmower trunc:", trunc_index
xobs = xobs[0:trunc_index, :]
zobs = zobs[0:trunc_index, :]
xobs = np.vstack([xseed, xobs])
zobs = np.vstack([zseed, zobs])
LEN = 2.0122
VAR = 5.3373 / 10.0
NOISE = 0.19836 / 10.0
# Create the GP model
# gp_world = gplib.GPModel(ranges, lengthscale = 4.0543111858072445, variance = 0.3215773006606948, noise = 0.0862445597387173)
gp_world = gplib.GPModel(ranges, lengthscale = LEN, variance = VAR, noise = NOISE)
gp_world.add_data(xfull[::5], zfull[::5])
# VAR = 0.3215773006606948
# LEN = 4.0543111858072445
# NOISE = 0.0862445597387173
LEN = 2.0122
VAR = 5.3373 / 10.0
NOISE = 0.19836 / 10.0
else:
gp_world = None
# VAR = 50.0
# LEN = 5.0
# NOISE = 0.1
VAR = 100.0
# ow = obslib.ChannelWorld(ranges, (3.5, 7.), 3., 0.3)
# ow = obslib.BugTrap(ranges, (2.2, 3.0), 4.6, orientation = 'left', width = 5.0)
# ow = obslib.BlockWorld(ranges,12, dim_blocks=(1., 1.), centers=[(2.5, 2.5), (7.,4.), (5., 8.), (8.75, 6.), (3.5,6.), (6.,1.5), (1.75,5.), (6.2,6.), (8.,8.5), (4.2, 3.8), (8.75,2.5), (2.2,8.2)])
if RUN_REAL_EXP:
''' Bagging '''
xfull, zfull = baglib.read_fulldataset()
# Add subsampled data from a previous bagifle
seed_bag = '/home/genevieve/mit-whoi/barbados/rosbag_15Jan_slicklizard/slicklizard_2019-01-15-20-22-16.bag'
xobs, zobs = baglib.read_bagfile(seed_bag)
print xobs.shape
print zobs.shape
# Create the GP model
gp_world = gplib.GPModel(ranges,
lengthscale=4.0543111858072445,
variance=0.3215773006606948,
noise=0.0862445597387173)
gp_world.add_data(xfull[::5], zfull[::5])
VAR = 0.3215773006606948
LEN = 4.0543111858072445
NOISE = 0.0862445597387173
else:
gp_world = None
# VAR = 50.0
# LEN = 5.0
# NOISE = 0.1
VAR = 100.0
LEN = 1.0
NOISE = 1.0
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