def __init__(self, input_limit, sample_number, frontier_size, horizon_length, total_time, sample_step, extent, obstacle_world=obs.FreeWorld()): ''' Sample number: Number of action samples will generate. ''' self.limit = input_limit self.fs = frontier_size self.hl = horizon_length self.ss = sample_step self.extent = extent self.num_action = sample_number self.time = total_time #Time horizon for single local trajectory self.goals = [] self.samples = {} self.cp = (0, 0, 0) #Current pose of the vehicle self.obstacle_world = obstacle_world
def __init__(self, frontier_size, horizon_length, turning_radius, sample_step, extent, obstacle_world=obs.FreeWorld()): ''' Initialize a path generator Input: frontier_size (int) the number of points on the frontier we should consider for navigation horizon_length (float) distance between the vehicle and the horizon to consider turning_radius (float) the feasible turning radius for the vehicle sample_step (float) the unit length along the path from which to draw a sample extent (list of floats) the world boundaries ''' # the parameters for the dubin trajectory self.fs = frontier_size self.hl = horizon_length self.tr = turning_radius self.ss = sample_step self.extent = extent # Global variables self.goals = [] #The frontier coordinates self.samples = {} #The sample points which form the paths self.cp = (0, 0, 0) #The current pose of the vehicle # Determining whether to consider obstacles self.obstacle_world = obstacle_world
def __init__(self, extent, discretization, sample_step, turning_radius, step_size, obstacle_world=obs.FreeWorld()): self.ranges = extent self.discretization = discretization self.sample_step = sample_step self.turning_radius = turning_radius self.step_size = step_size 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.obstacle_world = obstacle_world
def __init__(self, ranges, NUM_PTS, variance, lengthscale, noise=0.0001, visualize=False, seed=None, dim=2, model=None, MIN_COLOR=-25.0, MAX_COLOR=25.0, obstacle_world=obslib.FreeWorld(), time_duration=None): ''' Initialize a random Gaussian environment using the input kernel, assuming zero mean function. Input: ranges (tuple of floats): a tuple representing the max/min of 2D rectangular domain i.e. (-10, 10, -50, 50) NUM_PTS (int): the number of points in each dimension to sample for initialization, resulting in a sample grid of size NUM_PTS x NUM_PTS variance (float): the variance parameter of the kernel lengthscale (float): the lengthscale parameter of the kernel noise (float): the sensor noise parameter of the kernel visualize (boolean): flag to plot the surface of the environment seed (int): an integer seed for the random draws. If set to \'None\', no seed is used MIN_COLOR (float) used for plottig the range for the visualization MAX_COLOR (float) used for plotting the range for the visualization ''' # Save the parmeters of GP model self.variance = variance self.lengthscale = lengthscale self.dim = dim self.noise = noise self.obstacle_world = obstacle_world self.time_duration = time_duration logger.info('Environment seed: {}'.format(seed)) # Expect ranges to be a 4-tuple consisting of x1min, x1max, x2min, and x2max self.x1min = float(ranges[0]) self.x1max = float(ranges[1]) self.x2min = float(ranges[2]) self.x2max = float(ranges[3]) if model is not None: self.GP = model # Plot the surface mesh and scatter plot representation of the samples points if visualize == True: # Generate a set of observations from robot model with which to make contour plots # x1vals = np.linspace(ranges[0], ranges[1], 100) # x2vals = np.linspace(ranges[2], ranges[3], 100) x1vals = np.arange(ranges[0], ranges[1], (ranges[1] - ranges[0]) / NUM_PTS) x2vals = np.arange(ranges[2], ranges[3], (ranges[3] - ranges[2]) / NUM_PTS) print(self.GP.xvals) x1, x2 = np.meshgrid(x1vals, x2vals) # dimension: NUM_PTS x NUM_PTS print(x1.shape, x2.shape) print(x1) print(x2) data = np.vstack([x1.ravel(), x2.ravel()]).T observations, var = self.GP.predict_value(data, include_noise=False) maxind = np.argmax(observations) self.max_val = observations[maxind, :] self.max_loc = data[maxind, :] fig2, ax2 = plt.subplots(figsize=(8, 6)) ax2.set_xlim(ranges[0:2]) ax2.set_ylim(ranges[2:]) ax2.set_title('Countour Plot of the True World Model') plot = ax2.contourf(x1, x2, observations.reshape(x1.shape), 25, cmap='viridis') if MAX_COLOR is not None and MIN_COLOR is not None: MAX_COLOR = np.percentile(observations, 99) MIN_COLOR = np.percentile(observations, 1) else: plot = ax2.contourf(x1, x2, observations.reshape(x1.shape), 25, cmap='viridis') # scatter = ax2.scatter(self.GP.xvals[:, 0], self.GP.xvals[:, 1], c = self.GP.zvals.ravel(), s = 4.0, cmap = 'viridis') print("Maxima at:", data[maxind, 0], data[maxind, 1]) ax2.scatter(data[maxind, 0], data[maxind, 1], color='k', marker='*', s=500) print("Maxima at:", self.GP.xvals[maxind, 0], self.GP.xvals[maxind, 1]) ax2.scatter(self.GP.xvals[maxind, 0], self.GP.xvals[maxind, 1], color='k', marker='*', s=500) if not os.path.exists('./figures'): os.makedirs('./figures') fig2.savefig('./figures/world_model_countour.png') plt.show() plt.close() else: #初始地图建立 # Generate a set of discrete grid points, uniformly spread across the environment x1 = np.linspace(self.x1min, self.x1max, NUM_PTS) x2 = np.linspace(self.x2min, self.x2max, NUM_PTS) x1vals, x2vals = np.meshgrid(x1, x2, sparse=False, indexing='xy') #生成数据,400 data = np.vstack([x1vals.ravel(), x2vals.ravel()]).T # 做一个缓冲区,主要是考虑到机器人大小,因此不能贴边采样 bb = ((ranges[1] - ranges[0]) * 0.05, (ranges[3] - ranges[2]) * 0.05) ranges = (ranges[0] + bb[0], ranges[1] - bb[0], ranges[2] + bb[1], ranges[3] - bb[1]) # A dictionary to hold the GP model for each time stamp #建立GP模型self字典 self.models = {} # If we have a static model if self.time_duration is None: self.time_duration = 1 for T in range(self.time_duration): print("Generating environment for time", T) logger.warning("Generating environemnt for time %d", T) # Initialize maxima arbitrarily to violate boundary constraints #(0,0) maxima = [self.x1min, self.x2min] #主循环,继续生成随机环境,直到全局极大值位于边界约束内 # Continue to generate random environments until the global maximia # lives within the boundary constraints while maxima[0] < ranges[0] or maxima[0] > ranges[1] or \ maxima[1] < ranges[2] or maxima[1] > ranges[3] or \ self.obstacle_world.in_obstacle(maxima, buff=0.0): print( "Current environment in violation of boundary constraint. Regenerating!" ) logger.warning( "Current environment in violation of boundary constraint. Regenerating!" ) #初始化GP模型环境 self.GP = GPModel(ranges=ranges, lengthscale=lengthscale, variance=variance, dimension=self.dim) # Initialize points at time T if self.dim == 2: data = np.vstack([x1vals.ravel(), x2vals.ravel()]).T elif self.dim == 3: data = np.vstack([ x1vals.ravel(), x2vals.ravel(), T * np.ones(len(x1vals.ravel())) ]).T # Take an initial sample in the GP prior, conditioned on no other data if T == 0: xsamples = np.reshape(np.array(data[0, :]), (1, dim)) # dimension: 1 x dim #根据初始值更新均值和方差 mean, var = self.GP.predict_value(xsamples, include_noise=False) if seed is not None: np.random.seed(seed) seed += 1 #生成正态分布随机数 zsamples = np.random.normal(loc=0, scale=np.sqrt(var)) zsamples = np.reshape(zsamples, (1, 1)) # dimension: 1 x 1 # Add initial sample data point to the GP model self.GP.add_data(xsamples, zsamples) np.random.seed(seed) #余下数据生成后验采样 observations = self.GP.posterior_samples(data[1:, :], full_cov=True, size=1) observations = np.reshape(observations, (399, 1)) self.GP.add_data(data[1:, :], observations) else: np.random.seed(seed) observations = self.models[T - 1].posterior_samples( data, full_cov=True, size=1) self.GP.add_data(data, observations) #更新maxima,及下一个采样点 maxima = self.GP.xvals[np.argmax(self.GP.zvals), :] # Plot the surface mesh and scatter plot representation of the samples points if visualize == True: # the 3D surface fig = plt.figure(figsize=(8, 6)) ax = fig.add_subplot(111, projection='3d') ax.set_title('Surface of the Simulated Environment') surf = ax.plot_surface(x1vals, x2vals, self.GP.zvals.reshape( x1vals.shape), cmap=cm.coolwarm, linewidth=1) # the contour map fig2 = plt.figure(figsize=(8, 6)) ax2 = fig2.add_subplot(111) ax2.set_title( 'Countour Plot of the Simulated Environment') plot = ax2.contourf(x1vals, x2vals, self.GP.zvals.reshape( x1vals.shape), 25, cmap='viridis') #最大的z的列表,应该为最大方差不确定度 maxind = np.argmax(self.GP.zvals) ax2.scatter(self.GP.xvals[maxind, 0], self.GP.xvals[maxind, 1], color='k', marker='*', s=500) fig2.colorbar(plot, ax=ax2) # If available, plot the obstacles in the world if len(self.obstacle_world.get_obstacles()) != 0: for o in self.obstacle_world.get_obstacles(): x, y = o.exterior.xy ax2.plot(x, y, 'r', linewidth=3) plt.show() plt.close() plt.close('all') # World with satisfactory maxima generated #self.models[T] = copy.deepcopy(self.GP) !!!!! #将结果存储 maxind = np.argmax(self.GP.zvals) self.max_val = self.GP.zvals[maxind, :] self.max_loc = self.GP.xvals[maxind, :] print("Environment initialized with bounds X1: (", self.x1min, ",", self.x1max, ") X2:(", self.x2min, ",", self.x2max, ")") logger.info( "Environment initialized with bounds X1: ({}, {}) X2: ({}, {})" .format(self.x1min, self.x1max, self.x2min, self.x2max))
def sample_max_vals(robot_model, t, nK=3, nFeatures=200, visualize=False, obstacles=obslib.FreeWorld(), f_rew='mes'): ''' The mutual information between a potential set of samples and the local maxima''' # If the robot has not samples yet, return a constant value if robot_model.xvals is None: return None, None, None d = robot_model.xvals.shape[ 1] # The dimension of the points (should be 2D) ''' Sample Maximum values i.e. return sampled max values for the posterior GP, conditioned on current observations. Construct random freatures and optimize functions drawn from posterior GP.''' samples = np.zeros((nK, 1)) locs = np.zeros((nK, d)) funcs = [] delete_locs = [] for i in range(nK): print "Starting global optimization", i, "of", nK logger.info("Starting global optimization {} of {}".format(i, nK)) # Draw the weights for the random features # TODO: make sure this formula is correct if robot_model.dimension == 2: W = np.random.normal(loc=0.0, scale=np.sqrt(1. / (robot_model.lengthscale)), size=(nFeatures, d)) elif robot_model.dimension == 3: W = np.random.normal(loc=0.0, scale=np.sqrt(1. / (robot_model.lengthscale[0])), size=(nFeatures, d)) b = 2 * np.pi * np.random.uniform( low=0.0, high=1.0, size=(nFeatures, 1)) # Compute the features for xx Z = np.sqrt(2 * robot_model.variance / nFeatures) * np.cos(np.dot(W, robot_model.xvals.T) + b) # Draw the coefficient theta noise = np.random.normal(loc=0.0, scale=1.0, size=(nFeatures, 1)) # TODO: Figure this code out if robot_model.xvals.shape[0] < nFeatures: # We adopt the formula $theta \sim \N(Z(Z'Z + \sigma^2 I)^{-1} y, I-Z(Z'Z + \sigma^2 I)Z')$. try: Sigma = np.dot( Z.T, Z) + robot_model.noise * np.eye(robot_model.xvals.shape[0]) mu = np.dot(np.dot(Z, np.linalg.inv(Sigma)), robot_model.zvals) [D, U] = np.linalg.eig(Sigma) U = np.real(U) D = np.real(np.reshape(D, (D.shape[0], 1))) R = np.reciprocal( (np.sqrt(D) * (np.sqrt(D) + np.sqrt(robot_model.noise)))) theta = noise - np.dot( Z, np.dot(U, R * (np.dot(U.T, np.dot(Z.T, noise))))) + mu except: # If Sigma is not positive definite, ignore this simulation print "[ERROR]: Sigma is not positive definite, ignoring simulation", i logger.warning( "[ERROR]: Sigma is not positive definite, ignoring simulation {}" .format(i)) delete_locs.append(i) continue else: # $theta \sim \N((ZZ'/\sigma^2 + I)^{-1} Z y / \sigma^2, (ZZ'/\sigma^2 + I)^{-1})$. try: Sigma = np.dot(Z, Z.T) / robot_model.noise + np.eye(nFeatures) Sigma = np.linalg.inv(Sigma) mu = np.dot(np.dot(Sigma, Z), robot_model.zvals) / robot_model.noise theta = mu + np.dot(np.linalg.cholesky(Sigma), noise) except: # If Sigma is not positive definite, ignore this simulation print "[ERROR]: Sigma is not positive definite, ignoring simulation", i logger.warning( "[ERROR]: Sigma is not positive definite, ignoring simulation {}" .format(i)) delete_locs.append(i) continue # theta = np.random.multivariate_normal(mean = np.reshape(mu, (nFeatures,)), cov = Sigma, size = (nFeatures, 1)) # Obtain a function samples from posterior GP # def target(x): # pdb.set_trace() # return np.dot(theta.T * np.sqrt(2.0 * robot_model.variance / nFeatures), np.cos(np.dot(W, x.T) + b)).T # target = lambda x: np.dot(theta.T * np.sqrt(2.0 * robot_model.variance / nFeatures), np.cos(np.dot(W, x.T) + b)).T # def target(x, W=W, theta=theta): # W = copy.deepcopy(W) # theta = copy.deepcopy(theta) # return np.dot(theta.T * np.sqrt(2.0 * robot_model.variance / nFeatures), np.cos(np.dot(W, x.T) + b)).T # target = lambda x: np.dot(theta.T * np.sqrt(2.0 * robot_model.variance / nFeatures), np.cos(np.dot(W, x.T) + b)).T target = partial(general_target, robot_model=robot_model, nFeatures=nFeatures, theta=theta, W=W, b=b) target_vector_n = lambda x: -target(x.reshape(1, d)) # Can only take a 1D input # def target_gradient(x): # return np.dot(theta.T * -np.sqrt(2.0 * robot_model.variance / nFeatures), np.sin(np.dot(W, x.reshape((2,1))) + b) * W) target_gradient = lambda x: np.dot( theta.T * -np.sqrt(2.0 * robot_model.variance / nFeatures), np.sin(np.dot(W, x.reshape((d, 1))) + b) * W) target_vector_gradient_n = lambda x: -np.asarray( target_gradient(x).reshape(d, )) # Optimize the function status = False count = 0 # Retry optimization up to 5 times; if hasn't converged, give up on this simulated world while status == False and count < 5: maxima, max_val, max_inv_hess, status = global_maximization( target, target_vector_n, target_gradient, target_vector_gradient_n, robot_model.ranges, robot_model.xvals, visualize, 't' + str(t) + '.nK' + str(i), obstacles, f_rew=f_rew, time=t) count += 1 if status == False: delete_locs.append(i) continue samples[i] = np.array(max_val).reshape((1, 1)) funcs.append(copy.deepcopy(target)) print "Max Value in Optimization \t \t", samples[i] logger.info("Max Value in Optimization \t {}".format(samples[i])) locs[i, :] = maxima.reshape((1, d)) # if max_val < np.max(robot_model.zvals) + 5.0 * np.sqrt(robot_model.noise) or \ # maxima[0] == robot_model.ranges[0] or maxima[0] == robot_model.ranges[1] or \ # maxima[1] == robot_model.ranges[2] or maxima[1] == robot_model.ranges[3]: ''' if max_val < np.max(robot_model.zvals) + 5.0 * np.sqrt(robot_model.noise): samples[i] = np.max(robot_model.zvals) + 5.0 * np.sqrt(robot_model.noise) print "Max observed is bigger than max in opt:", samples[i] logger.info("Max observed is bigger than max in opt: {}".format(samples[i])) locs[i, :] = robot_model.xvals[np.argmax(robot_model.zvals)] ''' print "Deleting values at:", delete_locs samples = np.delete(samples, delete_locs, axis=0) locs = np.delete(locs, delete_locs, axis=0) # If all global optimizations fail, just return the max value seen so far if len(delete_locs) == nK: samples[0] = np.max( robot_model.zvals) + 5.0 * np.sqrt(robot_model.noise) locs[0, :] = robot_model.xvals[np.argmax(robot_model.zvals)] print "Returning:", samples.shape, locs.shape return samples, locs, funcs
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 elif f_rew == 'naive': self.aquisition_function = aqlib.naive self.sample_num = 3 self.sample_radius = 1.5 elif f_rew == 'naive_value': self.aquisition_function = aqlib.naive_value self.sample_num = 3 self.sample_radius = 3.0 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) # 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
def __init__(self, ranges, NUM_PTS, variance, lengthscale, noise=0.0001, visualize=True, seed=None, dim=2, model=None, MIN_COLOR=-25.0, MAX_COLOR=25.0, obstacle_world=obslib.FreeWorld()): ''' Initialize a random Gaussian environment using the input kernel, assuming zero mean function. Input: ranges (tuple of floats): a tuple representing the max/min of 2D rectangular domain i.e. (-10, 10, -50, 50) NUM_PTS (int): the number of points in each dimension to sample for initialization, resulting in a sample grid of size NUM_PTS x NUM_PTS variance (float): the variance parameter of the kernel lengthscale (float): the lengthscale parameter of the kernel noise (float): the sensor noise parameter of the kernel visualize (boolean): flag to plot the surface of the environment seed (int): an integer seed for the random draws. If set to \'None\', no seed is used MIN_COLOR (float) used for plottig the range for the visualization MAX_COLOR (float) used for plotting the range for the visualization ''' # Save the parmeters of GP model self.variance = variance self.lengthscale = lengthscale self.dim = dim self.noise = noise self.obstacle_world = obstacle_world logger.info('Environment seed: {}'.format(seed)) # Expect ranges to be a 4-tuple consisting of x1min, x1max, x2min, and x2max self.x1min = float(ranges[0]) self.x1max = float(ranges[1]) self.x2min = float(ranges[2]) self.x2max = float(ranges[3]) if model is not None: self.GP = model # Plot the surface mesh and scatter plot representation of the samples points if visualize == True: # Generate a set of observations from robot model with which to make contour plots x1vals = np.linspace(ranges[0], ranges[1], 40) x2vals = np.linspace(ranges[2], ranges[3], 40) x1, x2 = np.meshgrid( x1vals, x2vals, sparse=False, indexing='xy') # dimension: NUM_PTS x NUM_PTS data = np.vstack([x1.ravel(), x2.ravel()]).T observations, var = self.GP.predict_value(data, include_noise=False) fig2, ax2 = plt.subplots(figsize=(8, 6)) ax2.set_xlim(ranges[0:2]) ax2.set_ylim(ranges[2:]) ax2.set_title('Countour Plot of the True World Model') plot = ax2.contourf(x1, x2, observations.reshape(x1.shape), cmap='viridis', vmin=MIN_COLOR, vmax=MAX_COLOR, levels=np.linspace(MIN_COLOR, MAX_COLOR, 15)) scatter = ax2.scatter(self.GP.xvals[:, 0], self.GP.xvals[:, 1], c=self.GP.zvals.ravel(), s=4.0, cmap='viridis') maxind = np.argmax(self.GP.zvals) ax2.scatter(self.GP.xvals[maxind, 0], self.GP.xvals[maxind, 1], color='k', marker='*', s=500) fig2.colorbar(plot, ax=ax2) fig2.savefig('./figures/world_model_countour.png') #plt.show() plt.close() else: # Generate a set of discrete grid points, uniformly spread across the environment x1 = np.linspace(self.x1min, self.x1max, NUM_PTS) x2 = np.linspace(self.x2min, self.x2max, NUM_PTS) # dimension: NUM_PTS x NUM_PTS x1vals, x2vals = np.meshgrid(x1, x2, sparse=False, indexing='xy') # dimension: NUM_PTS*NUM_PTS x 2 data = np.vstack([x1vals.ravel(), x2vals.ravel()]).T bb = ((ranges[1] - ranges[0]) * 0.05, (ranges[3] - ranges[2]) * 0.05) ranges = (ranges[0] + bb[0], ranges[1] - bb[0], ranges[2] + bb[1], ranges[3] - bb[1]) # Initialize maxima arbitrarily to violate boundary constraints maxima = [self.x1min, self.x2min] # Continue to generate random environments until the global maximia # lives within the boundary constraints while maxima[0] < ranges[0] or maxima[0] > ranges[1] or \ maxima[1] < ranges[2] or maxima[1] > ranges[3] or \ self.obstacle_world.in_obstacle(maxima, buff = 0.0): print "Current environment in violation of boundary constraint. Regenerating!" logger.warning( "Current environment in violation of boundary constraint. Regenerating!" ) # Intialize a GP model of the environment self.GP = OnlineGPModel(ranges=ranges, lengthscale=lengthscale, variance=variance) data = np.vstack([x1vals.ravel(), x2vals.ravel()]).T # Take an initial sample in the GP prior, conditioned on no other data xsamples = np.reshape(np.array(data[0, :]), (1, dim)) # dimension: 1 x 2 mean, var = self.GP.predict_value(xsamples, include_noise=False) if seed is not None: np.random.seed(seed) seed += 1 zsamples = np.random.normal(loc=0, scale=np.sqrt(var)) zsamples = np.reshape(zsamples, (1, 1)) # dimension: 1 x 1 # Add initial sample data point to the GP model self.GP.add_data(xsamples, zsamples) np.random.seed(seed) observations = self.GP.posterior_samples(data[1:, :], full_cov=True, size=1) self.GP.add_data(data[1:, :], observations) ''' # Iterate through the rest of the grid sequentially and sample a z values, # conditioned on previous samples for index, point in enumerate(data[1:, :]): # Get a new sample point xs = np.reshape(np.array(point), (1, dim)) # Compute the predicted mean and variance mean, var = self.GP.predict_value(xs) # Sample a new observation, given the mean and variance if seed is not None: np.random.seed(seed) seed += 1 zs = np.random.normal(loc = mean, scale = np.sqrt(var)) # Add new sample point to the GP model zsamples = np.vstack([zsamples, np.reshape(zs, (1, 1))]) xsamples = np.vstack([xsamples, np.reshape(xs, (1, dim))]) self.GP.add_data(np.reshape(xs, (1, dim)), np.reshape(zs, (1, 1))) ''' maxima = self.GP.xvals[np.argmax(self.GP.zvals), :] # Plot the surface mesh and scatter plot representation of the samples points if visualize == True: # the 3D surface fig = plt.figure(figsize=(8, 6)) ax = fig.add_subplot(111, projection='3d') ax.set_title('Surface of the Simulated Environment') surf = ax.plot_surface(x1vals, x2vals, self.GP.zvals.reshape(x1vals.shape), cmap=cm.coolwarm, linewidth=1) if not os.path.exists('./figures'): os.makedirs('./figures') fig.savefig('./figures/world_model_surface.png') # the contour map fig2 = plt.figure(figsize=(8, 6)) ax2 = fig2.add_subplot(111) ax2.set_title('Countour Plot of the Simulated Environment') plot = ax2.contourf(x1vals, x2vals, self.GP.zvals.reshape(x1vals.shape), cmap='viridis', vmin=MIN_COLOR, vmax=MAX_COLOR, levels=np.linspace( MIN_COLOR, MAX_COLOR, 15)) scatter = ax2.scatter(data[:, 0], data[:, 1], c=self.GP.zvals.ravel(), s=4.0, cmap='viridis') maxind = np.argmax(self.GP.zvals) ax2.scatter(self.GP.xvals[maxind, 0], self.GP.xvals[maxind, 1], color='k', marker='*', s=500) fig2.colorbar(plot, ax=ax2) # If available, plot the obstacles in the world if len(self.obstacle_world.get_obstacles()) != 0: for o in self.obstacle_world.get_obstacles(): x, y = o.exterior.xy ax2.plot(x, y, 'r', linewidth=3) fig2.savefig('./figures/world_model_countour.png') #plt.show() plt.close() print "Environment initialized with bounds X1: (", self.x1min, ",", self.x1max, ") X2:(", self.x2min, ",", self.x2max, ")" logger.info( "Environment initialized with bounds X1: ({}, {}) X2: ({}, {})" .format(self.x1min, self.x1max, self.x2min, self.x2max))
ftemp.append(c) break else: ftemp.append(c) else: pass true_path[i] = ftemp return sampling_path, true_path if __name__ == '__main__': # bw = obs.BlockWorld( [0., 10., 0., 10.], num_blocks=1, dim_blocks=(2.,2.), centers=[(6.1,5)]) # bw = obs.BugTrap([0., 10., 0., 10.], (5,5), 3, channel_size = 0.5, width = 3., orientation='left') # bw = obs.ChannelWorld([0., 10., 0., 10.], (6,5), 3, 0.4) bw = obs.FreeWorld() # extent, discretization, sample_step, turning_radius, step_size,obstacle_world=obs.FreeWorld() gen = Reachable_Frontier_Generator([0., 10., 0., 10.], (20, 20), 0.5, 0.1, 1.5, bw) # gen = Dubins_Path_Generator(15., 1.5, 0.05, 0.5, [0., 10., 0., 10.], bw) plt.figure() trajectory = [] samples = [] coord = (5.2, 5.2, 0) for m in range(1): paths, true_paths = gen.get_path_set(coord) print len(paths) action = np.random.choice(paths.keys())
ranges = (0.0, 0.0, 10.0, 10.0) # MAX_COLOR = None # MIN_COLOR = None # Parameters for plotting based on the seed world information # Set up paths for logging the data from the simulation run if not os.path.exists('./figures/' + str(REWARD_FUNCTION)): os.makedirs('./figures/' + str(REWARD_FUNCTION)) logging.basicConfig(filename = './figures/'+ REWARD_FUNCTION + '/robot.log', level = logging.INFO) logger = logging.getLogger('robot') # Create a random enviroment sampled from a GP with an RBF kernel and specified hyperparameters, mean function 0 # The enviorment will be constrained by a set of uniformly distributed sample points of size NUM_PTS x NUM_PTS # Create obstacle world ow = obslib.FreeWorld() # 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' xseed, zseed = baglib.read_bagfile(seed_bag, 20) # PLUMES trials seed_bag = '/home/genevieve/mit-whoi/barbados/rosbag_16Jan_slicklizard/slicklizard_2019-01-17-03-01-44.bag'
# Parameters for plotting based on the seed world information MIN_COLOR = -25. MAX_COLOR = 25. # 设置记录数据的路径 if not os.path.exists('./figures/' + str(REWARD_FUNCTION)): os.makedirs('./figures/' + str(REWARD_FUNCTION)) logging.basicConfig(filename = './figures/'+ REWARD_FUNCTION + '/robot.log', level = logging.INFO) logger = logging.getLogger('robot') #设置world的范围 ranges = (0., 10., 0., 10.) #加载无障碍物环境 ow = obslib.FreeWorld() #a world without obstacles #创建机器人能够识别的环境 #没有观测值的预设先验的环境更新 world = envlib.Environment(ranges = ranges, NUM_PTS = 20, variance = 100.0, lengthscale = 1.0, visualize = False, seed = SEED, MIN_COLOR=MIN_COLOR, MAX_COLOR=MAX_COLOR, obstacle_world = ow, noise=10.0) #将更新好后的地图和奖励函数类型 # Create the evaluation class used to quantify the simulation metrics