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
