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