Beispiel #1
0
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
Beispiel #2
0
    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
Beispiel #3
0
    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
Beispiel #4
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
Beispiel #5
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