def batch_predict(SA): sa = np.mean(SA, 0) idx = kdt.query(sa.reshape(1, -1), k=K, return_distance=False) X_nn = Xtrain[idx, :].reshape(K, state_action_dim) Y_nn = Ytrain[idx, :].reshape(K, state_dim) if useDiffusionMaps: X_nn, Y_nn = reduction(sa, X_nn, Y_nn) m = np.zeros((SA.shape[0], state_dim)) s = np.zeros((SA.shape[0], state_dim)) for i in range(state_dim): if i == 0: gp_est = GaussianProcess(X_nn[:, :4], Y_nn[:, i], optimize=False, theta=None) theta = gp_est.cov.theta else: gp_est = GaussianProcess(X_nn[:, :4], Y_nn[:, i], optimize=False, theta=theta) mm, ss = gp_est.batch_predict(SA[:, :4]) m[:, i] = mm s[:, i] = np.diag(ss) return m, s
def predict(sa): idx = kdt.query(sa.T, k=K, return_distance=False) X_nn = Xtrain[idx, :].reshape(K, state_action_dim) Y_nn = Ytrain[idx, :].reshape(K, state_dim) if useDiffusionMaps: X_nn, Y_nn = reduction(sa, X_nn, Y_nn) m = np.zeros(state_dim) s = np.zeros(state_dim) for i in range(state_dim): if i == 0: gp_est = GaussianProcess(X_nn[:, :4], Y_nn[:, i], optimize=True, theta=None) theta = gp_est.cov.theta else: gp_est = GaussianProcess(X_nn[:, :4], Y_nn[:, i], optimize=False, theta=theta) m[i], s[i] = gp_est.predict(sa[:4].reshape(1, -1)[0]) return m, s
def one_predict(sa, X, Y, kdt, K=100): state_dim = 4 idx = kdt.query(sa.reshape(1, -1), k=K, return_distance=False) X_nn = X[idx, :].reshape(K, state_dim + 2) Y_nn = Y[idx, :].reshape(K, state_dim) ds_next = np.zeros((state_dim, )) std_next = np.zeros((state_dim, )) try: for i in range(state_dim): if i == 0: gp_est = GaussianProcess(X_nn[:, :state_dim], Y_nn[:, i], optimize=True, theta=None) Theta = gp_est.cov.theta else: gp_est = GaussianProcess(X_nn[:, :state_dim], Y_nn[:, i], optimize=False, theta=Theta) mm, vv = gp_est.predict(sa[:state_dim]) ds_next[i] = mm std_next[i] = np.sqrt(np.diag(vv)) except: print "pass" return np.array([-1, -1, -1, -1]), np.array([-1, -1, -1, -1]) # SKlearn # for i in range(state_dim): # gp_est = GaussianProcessRegressor(n_restarts_optimizer=9) # gp_est.fit(X_nn[:,:state_dim], Y_nn[:,i]) # mm, vv = gp_est.predict(sa[:state_dim].reshape(1,-1), return_std=True) # ds_next[i] = mm # std_next[i] = vv#np.sqrt(np.diag(vv)) # GPy # for i in range(state_dim): # kernel = GPy.kern.RBF(input_dim=state_dim, variance=1., lengthscale=1.) # gp_est = GPy.models.GPRegression(X_nn[:,:state_dim], Y_nn[:,i].reshape(-1,1), kernel) # gp_est.optimize(messages=False) # # m.optimize_restarts(num_restarts = 10) # mm, vv = gp_est.predict(sa[:state_dim].reshape(1,state_dim)) # ds_next[i] = mm # std_next[i] = np.sqrt(np.diag(vv)) s_next = sa[:4] + ds_next return s_next, std_next, X_nn
def test_cpu_gpu(self): C = torch.tensor([1.0]) l = torch.rand(5) X = torch.randn(10, 5) y = torch.randn(10, 1) Y = torch.randn(10, 5) X1 = torch.randn(2, 5) matern = Matern52(C, l) d2matern = Deriv2Matern52(C, l) gp1 = GaussianProcess(ZeroScalarMean(), matern, 1e-2) gp2 = GaussianProcess(ZeroVectorMean(), d2matern, 1e-2) self.assertTrue(check_device(gp1, X, y, X1)) self.assertTrue(check_device(gp2, X, Y, X1))
def batch_predict(self, SA): sa = np.mean(SA, 0) # Theta, K = self.get_theta(sa) # Get hyper-parameters for this query point K = 1 idx = self.kdt.query(sa.reshape(1, -1), k=K, return_distance=False) X_nn = self.Xtrain[idx, :].reshape(K, self.state_action_dim) Y_nn = self.Ytrain[idx, :].reshape(K, self.state_dim) if useDiffusionMaps: X_nn, Y_nn = self.reduction(sa, X_nn, Y_nn) dS_next = np.zeros((SA.shape[0], self.state_dim)) std_next = np.zeros((SA.shape[0], self.state_dim)) for i in range(self.state_dim): gp_est = GaussianProcess(X_nn[:, :self.state_action_dim], Y_nn[:, i], optimize=True, theta=None, algorithm='Matlab') mm, vv = gp_est.batch_predict(SA[:, :self.state_action_dim]) dS_next[:, i] = mm std_next[:, i] = np.sqrt(np.diag(vv)) S_next = SA[:, :self. state_dim] + dS_next #np.random.normal(dS_next, std_next) return S_next
def example_2d(): kern = RBF(dim=2, l=0.3, noise=0.01) cov = np.zeros((2, 2)) X = [np.array(e) for e in [[0, 0.0], [0.3, 0.7], [0.6, 0.2], [1.0, 1.0]]] Y = [2, 3, -2, 1] gp = GaussianProcess(X, Y, kern) return gp
def experiment_kiss(dataset=PUMADYN32NM, m=100, proj_d=2, cov='covSEard', standardize=True, proj=None): assert proj in {None, 'norm', 'orth'} train_x, train_y, test_x, test_y = load_dataset(dataset) if standardize: scaler = StandardScaler() train_x = scaler.fit_transform(train_x) test_x = scaler.transform(test_x) n, d = train_x.shape print 'Train KISS with {} data points and {} dimension'.format(n, d) # get GP functionality gp = GaussianProcess() # subtract mean train_y -= np.mean(train_y) test_y -= np.mean(train_y) # projection matrix P = np.random.normal(size=(proj_d, d)) # initialization hyp_lik = float(0.5 * np.log(np.var(train_y) / 4)) if cov == 'covSEard': init_x = np.dot(train_x, P.T) init_ell = np.log((np.max(init_x, axis=0) - np.min(init_x, axis=0)) / 2) hyp_cov = np.append(init_ell, [0.5 * np.log(np.var(train_y))]) else: hyp_cov = np.asarray([np.log(5), 0.5 * np.log(np.var(train_y))]) hyp_old = {'mean': [], 'lik': hyp_lik, 'cov': hyp_cov, 'proj': P} opt = {'cg_maxit': 500, 'cg_tol': 1e-5} if proj is not None: opt['proj'] = proj hyp = gp.train_kiss(train_x, train_y.reshape(-1, 1), k=m, hyp=hyp_old, opt=opt, n_iter=100) test_mean, test_var = gp.predict_kiss(train_x, train_y.reshape(-1, 1), k=m, xstar=test_x, opt=opt, hyp=hyp, cov=cov) print 'KISS error:' print_error(test_y, test_mean)
def one_predict(self, sa): Theta, _ = self.get_theta( sa) # Get hyper-parameters for this query point K = self.K idx = self.kdt.query(sa.reshape(1, -1), k=K, return_distance=False) X_nn = self.Xtrain[idx, :].reshape(K, self.state_action_dim) Y_nn = self.Ytrain[idx, :].reshape(K, self.state_dim) if useDiffusionMaps: X_nn, Y_nn = self.reduction(sa, X_nn, Y_nn) ds_next = np.zeros((self.state_dim, )) std_next = np.zeros((self.state_dim, )) for i in range(self.state_dim): gp_est = GaussianProcess(X_nn[:, :self.state_action_dim], Y_nn[:, i], optimize=False, theta=Theta[i], algorithm='Matlab') mm, vv = gp_est.predict(sa[:self.state_action_dim]) ds_next[i] = mm std_next[i] = np.sqrt(vv) s_next = sa[:self. state_dim] + ds_next #np.random.normal(ds_next, std_next) if self.state_dim == 5: s_next[4] += 1.0 if s_next[4] < 0.0 else 0.0 s_next[4] -= 1.0 if s_next[4] > 1.0 else 0.0 return s_next
def make_gp(): gp = GaussianProcess( lambda x, y, params: partial(periodic, params=params) (x, y) + params["ratio"] * partial(rbf, params=params)(x, y), sigma=0.03, ) return gp
def example_1d(): kern = Matern23(1, l=0.2, noise=0.01) cov = np.zeros((1, 1)) X = [(np.array(e), cov) for e in [[0.0], [0.3], [1.0]]] Y = [5.0, 7.0, 6.0] gp = GaussianProcess(X, Y, kern) gp.show(N_grid=200, bmin=-5, bmax=5) plt.show()
def train(self, x, y, doOptmization = False, params_min = None, params_max = None): self.X.append(x) self.Y.append(y) self.gp = GaussianProcess(self.X, self.Y, self.kernel_init if len(self.X) == 1 else self.gp.kernel) if doOptmization: self.gp.optimize(params_min = params_min, params_max = params_max)
def example_3d(): kern = RBF(dim=3, l=0.3, noise=0.01) cov = np.zeros((3, 3)) X = [(np.array(e, dtype=float), cov) for e in [[0, 0, 0], [0.3, 0.7, -0.2], [0.6, 0.2, -0.8], [0.3, 1.0, 1.0]]] Y = [2, 3, -2, 1] gp = GaussianProcess(X, Y, kern) return gp
def test_basic(self): """ 1. GP forward output is a mvNormal 2. GP predict output is a set of normal distributions 3. sample shape """ C = torch.tensor([3.0]) l = torch.rand(3) X1 = torch.randn(10, 3) X2 = torch.randn(2, 3) y1 = torch.randn(10, 1) y1prime = torch.randn(10, 3) scalar_kernels = [Matern52(C, l), RBF(C, l)] for k in scalar_kernels: gp = GaussianProcess(ZeroScalarMean(), k, 0.1) self.assertTrue(basic_test(gp, X1, y1, X2)) vector_kernels = [Deriv2Matern52(C, l), Deriv2RBF(C, l)] for k in vector_kernels: gp = GaussianProcess(ZeroVectorMean(), k, 0.1) self.assertTrue(basic_test(gp, X1, y1prime, X2))
def one_predict(sa, X, Y, kdt, K=100): state_dim = 4 idx = kdt.query(sa.reshape(1, -1), k=K, return_distance=False) X_nn = X[idx, :].reshape(K, state_dim + 2) Y_nn = Y[idx, :].reshape(K, state_dim) ds_next = np.zeros((state_dim, )) std_next = np.zeros((state_dim, )) try: for i in range(state_dim): if i == 0: gp_est = GaussianProcess(X_nn[:, :state_dim], Y_nn[:, i], optimize=True, theta=None) Theta = gp_est.cov.theta else: gp_est = GaussianProcess(X_nn[:, :state_dim], Y_nn[:, i], optimize=False, theta=Theta) mm, vv = gp_est.predict(sa[:state_dim]) ds_next[i] = mm std_next[i] = np.sqrt(np.diag(vv)) except: print "pass" return np.array([-1, -1, -1, -1]), np.array([-1, -1, -1, -1]) # for i in range(state_dim): # gp_est = GaussianProcessRegressor(n_restarts_optimizer=9) # gp_est.fit(X_nn[:,:state_dim], Y_nn[:,i]) # mm, vv = gp_est.predict(sa[:state_dim].reshape(1,-1), return_std=True) # ds_next[i] = mm # std_next[i] = vv#np.sqrt(np.diag(vv)) s_next = sa[:4] + ds_next return s_next, std_next
def batch_predict_iterative(self, SA): S_next = [] while SA.shape[0]: sa = np.copy(SA[np.random.randint(SA.shape[0]), :]) Theta, K = self.get_theta( sa) # Get hyper-parameters for this query point D, idx = self.kdt.query(sa.reshape(1, -1), k=K, return_distance=True) r = np.max(D) * 1.1 X_nn = self.Xtrain[idx, :].reshape(K, self.state_action_dim) Y_nn = self.Ytrain[idx, :].reshape(K, self.state_dim) neigh = NearestNeighbors(radius=r) neigh.fit(SA) idx_local = neigh.radius_neighbors(sa.reshape(1, -1), return_distance=False)[0] SA_local = np.copy(SA[idx_local, :]) SA = np.delete(SA, idx_local, axis=0) if useDiffusionMaps: X_nn, Y_nn = self.reduction(sa, X_nn, Y_nn) dS_next = np.zeros((SA_local.shape[0], self.state_dim)) std_next = np.zeros((SA_local.shape[0], self.state_dim)) for i in range(self.state_dim): gp_est = GaussianProcess(X_nn[:, :self.state_action_dim], Y_nn[:, i], optimize=False, theta=Theta[i], algorithm='Matlab') mm, vv = gp_est.batch_predict( SA_local[:, :self.state_action_dim]) dS_next[:, i] = mm std_next[:, i] = np.sqrt(np.diag(vv)) S_next_local = SA_local[:, :self.state_dim] + np.random.normal( dS_next, std_next) for s in S_next_local: S_next.append(s) return np.array(S_next)
def test_statistics(self): """ 1. sample from GP and calculate mean and covariance """ C = torch.tensor([3.0]) l = torch.rand(3) X1 = torch.randn(10, 3) kernels = [Matern52(C, l), RBF(C, l)] for k in kernels: gp = GaussianProcess(ZeroScalarMean(), k, 0.0) margin = gp(X1) n_sample = 100000 samples = margin.sample([n_sample]) statistic_mean = torch.mean(samples, dim=0) logging.debug("mean:{}".format(statistic_mean)) self.assertTrue(torch.allclose(statistic_mean, torch.zeros(10), atol=0.1)) statistic_cov = samples.T @ samples / (n_sample-1) K = k(X1) logging.debug("Cov:{}".format(statistic_cov)) self.assertTrue(torch.allclose(K, statistic_cov, atol=0.1))
def __init__( self, model, model_weight, model_bias, model_type, savedir, use_gp=True, sigma=1, r_loc=0.5, r_year=1.5, sigma_e=0.32, sigma_b=0.01, device=torch.device('cuda:0' if torch.cuda.is_available() else 'cpu')): self.savedir = savedir / model_type self.savedir.mkdir(parents=True, exist_ok=True) print(f'Using {device.type}') if device.type != 'cpu': model = model.cuda() self.model = model self.model_type = model_type self.model_weight = model_weight self.model_bias = model_bias self.device = device # for reproducability torch.manual_seed(42) torch.cuda.manual_seed_all(42) self.gp = None if use_gp: self.gp = GaussianProcess(sigma, r_loc, r_year, sigma_e, sigma_b)
def rbf_gp(): """Returns a GP instance with an rbf kernel""" return GaussianProcess(kernel='rbf')
N = 20 # Generate noisy data x_data = np.random.uniform(0.4, 4, N).reshape( -1, 1) # Generate N data points between 0.4 and 4 y_data = np.array([func(i, 0.2) for i in x_data]) # Add Gaussian noise with std. of 0.2 # Compute real function x_real = np.linspace(0, 6, 100).reshape(-1, 1) y_real = np.array([func(i, 0) for i in x_real]) # Initiate GP with training data gp_est = GaussianProcess(x_data, y_data.reshape((-1, )), optimize=True, theta=None, algorithm='Girard') # Compute prediction for new data x_new = np.linspace(0, 6, 100).reshape(-1, 1) means = np.empty(100) variances = np.empty(100) for i in range(100): means[i], variances[i] = gp_est.predict(x_new[i]) # Plot plt.plot(x_data, y_data, '+k', label='data') plt.plot(x_real, y_real, '--k', label='true function') msl = (means.reshape(1, -1)[0] - np.sqrt(variances)) #.reshape(-1,1) msu = (means.reshape(1, -1)[0] + np.sqrt(variances)) #.reshape(-1,1)[0]
return x * np.sin(x) + np.random.normal(0, v) # return 3*x+4+np.random.normal(0, 0.01) ax1 = plt.subplot2grid((3, 5), (0, 2), colspan=3, rowspan=2) x_data = np.random.uniform(0, 4, 20).reshape(-1, 1) y_data = np.array([func(i, 0.2) for i in x_data]) # plt.plot(x_data, y_data, '+k') x_real = np.linspace(0, 6, 100).reshape(-1, 1) y_real = np.array([func(i, 0) for i in x_real]) plt.plot(x_real, y_real, '--k') gp_est = GaussianProcess(x_data, y_data.reshape((-1, )), optimize=True, theta=None) gpup_est = UncertaintyPropagation(x_data, y_data.reshape((-1, )), optimize=True, theta=None, method=3) x_n = np.array([3.0]) # The mean of a normal distribution var_n = np.diag([ 0.2**2 ]) # The covariance matrix (must be diagonal because of lazy programming) m, s = gpup_est.predict(x_n, var_n) # m, s = gp_est.predict(x_n) print m, s plt.errorbar(x_n, m, yerr=np.sqrt(s), ecolor='y')
def main(): # Initialize the system. sys = System(f, g, DT, X0 - REF, T0) m1s = np.array([0]) k1s = np.array([SIGNAL_SIGMA]) m2s = np.array([0]) k2s = np.array([SIGNAL_SIGMA]) start_time = time.time() elapsed_time = np.array([0]) gp1 = GaussianProcess(SEKernel, signal_sigma=SIGNAL_SIGMA) gp2 = GaussianProcess(SEKernel, signal_sigma=SIGNAL_SIGMA) # Simulate the system. while sys.t < TF: x = sys.x # Predict the output. input_observations = [[x[0], x[1], sys.ref[1]]] m1, k1 = gp1.predict(input_observations) m2, k2 = gp2.predict(input_observations) u = np.dot(-K, sys.y) sys.step(u) # Observe the actual output. input_observations = [[x[0], x[1], u[1]]] gp1.observe(input_observations, [[sys.y[0]]]) gp2.observe(input_observations, [[sys.y[1]]]) # Record results. m1s = np.append(m1s, m1) k1s = np.append(k1s, np.sqrt(k1)) m2s = np.append(m2s, m2) k2s = np.append(k2s, np.sqrt(k2)) elapsed_time = np.append(elapsed_time, time.time() - start_time) print_sim_time(sys.t, DT) print() # Add operating point back to get back to normal coordinates. ys = sys.ys + np.tile(REF, (sys.ys.shape[0], 1)) m1s = m1s + np.ones(m1s.shape[0]) * REF[0] ts = sys.ts # Plot the results. plt.figure(1) plt.subplot(211) plt.plot([T0, TF], [REF[0], REF[0]], label='Reference') plt.plot(ts, ys[:, 0], label='Actual') plt.plot(ts, m1s, label='Predicted') plot_sigma_bounds(ts, m1s, k1s, 3, (0.9, ) * 3) plot_sigma_bounds(ts, m1s, k1s, 2, (0.8, ) * 3) plot_sigma_bounds(ts, m1s, k1s, 1, (0.7, ) * 3) plt.title('Inverted Pendulum') plt.ylabel('Angle (rad)') plt.legend() plt.grid() plt.subplot(212) plt.plot([T0, TF], [REF[1], REF[1]], label='Reference') plt.plot(ts, ys[:, 1], label='Actual') plt.plot(ts, m2s, label='Predicted') plot_sigma_bounds(ts, m2s, k2s, 3, (0.9, ) * 3) plot_sigma_bounds(ts, m2s, k2s, 2, (0.8, ) * 3) plot_sigma_bounds(ts, m2s, k2s, 1, (0.7, ) * 3) plt.xlabel('Time (s)') plt.ylabel('Angular velocity (rad/s)') plt.legend() plt.grid() plt.show() # Plot real time vs. simulation time, so we can see how the required # computation increases as more data is observed over the course of the # simulation. plt.figure(2) plt.plot(ts, elapsed_time) plt.xlabel('Simulation time (s)') plt.ylabel('Real time (s)') plt.title('Real time vs. Simulation time') plt.grid() plt.show() # Plot angle uncertainty over a portion of the state-space, with input u # fixed at 0. print('Plotting sample of state-space...') fig = plt.figure(3) ax = fig.add_subplot(111, projection='3d') # Sample the GP across the state-space for u = 0. samples = np.mgrid[-2:2:0.1, -2:2:0.1, 0:1].reshape(3, -1).T m, cov = gp1.predict(samples) # Generate plotting values. X1, X2 = np.mgrid[-2:2:0.1, -2:2:0.1] Z = np.sqrt(np.diag(cov)).T.reshape(40, 40) ax.plot_surface(X1, X2, Z, cmap=cm.coolwarm, linewidth=0) plt.xlabel('Angle') plt.ylabel('Angular velocity') plt.show()
def experiment_distill(dataset=PUMADYN32NM, use_kmeans=True, m=100, reduce_from='fitc', cov='covSEard', width=20, standardize=True, load_trained=False): train_x, train_y, test_x, test_y = load_dataset(dataset) if standardize: scaler = StandardScaler() train_x = scaler.fit_transform(train_x) test_x = scaler.transform(test_x) n, d = train_x.shape print 'Distilling with {} data points and {} dimension'.format(n, d) # get GP functionality gp = GaussianProcess() # subtract mean train_y -= np.mean(train_y) test_y -= np.mean(train_y) # initialization hyp_lik = float(0.5 * np.log(np.var(train_y) / 4)) if cov == 'covSEard': init_ell = np.log((np.max(train_x, axis=0) - np.min(train_x, axis=0)) / 2) hyp_cov = np.append(init_ell, [0.5 * np.log(np.var(train_y))]) else: hyp_cov = np.asarray([np.log(2)] * 2) hyp_old = {'mean': [], 'lik': hyp_lik, 'cov': hyp_cov} # train the kernel to reduce from # load hyp if trained already xu = np.random.choice(n, m, replace=False) xu = train_x[xu] hyp_fname = '../model/{}_hyp_{}.pkl'.format(dataset, reduce_from) if load_trained and os.path.exists(hyp_fname): print 'Loady hyperparams from {}'.format(hyp_fname) f = open(hyp_fname, 'rb') hyp = pkl.load(f) f.close() test_mean, test_var = gp.predict_exact(train_x, train_y.reshape(-1, 1), xstar=test_x, hyp=hyp, cov=cov) else: print 'Training the given kernel with {}'.format(reduce_from) if reduce_from == 'fitc': if dataset in {PUMADYN32NM, KIN40K}: hyp = load_trained_hyp(dataset) else: hyp = gp.train_fitc(train_x, train_y.reshape(-1, 1), xu, hyp_old, cov=cov) test_mean, test_var = gp.predict_fitc(train_x, train_y.reshape(-1, 1), xu=xu, xstar=test_x, hyp=hyp, cov=cov) f = open(hyp_fname, 'wb') pkl.dump(hyp, f, -1) f.close() elif reduce_from == 'exact': if dataset in {PUMADYN32NM, KIN40K}: hyp = load_trained_hyp(dataset) else: hyp = gp.train_exact(train_x, train_y.reshape(-1, 1), hyp_old, cov=cov, n_iter=100) f = open(hyp_fname, 'wb') pkl.dump(hyp, f, -1) f.close() else: raise ValueError(reduce_from) print '{} Error:'.format(reduce_from.capitalize()) print_error(test_y, test_mean) hyp_cov = hyp['cov'].flatten() sigmasq = np.exp(2 * hyp['lik']) kernel = SEard() if cov == 'covSEard' else SEiso() # distill the kernel distill = Distillation(X=train_x, y=train_y, U=xu, kernel=kernel, hyp=hyp_cov, num_iters=0, eta=1e-3, sigmasq=sigmasq, width=width, use_kmeans=use_kmeans, optimizer='adagrad') distill.grad_descent() distill.precompute(use_true_K=False) # plt.pcolor(np.abs(distill.diff_to_K()[:2000, :2000])) # plt.colorbar() # plt.show() mm, vv = [], [] for xstar in test_x: xstar = xstar.reshape(1, d) mstar, vstar = distill.predict(xstar, width=width) vv.append(vstar) mm.append(mstar) mm = np.asarray(mm).flatten() print 'Distill Error:' print_error(test_y, mm) print 'Mean error to true K:' print_error(test_mean, mm) return smse(test_mean, mm), smse(test_y, test_mean), smse(test_y, mm)
def experiment1D(): gp = GaussianProcess() # setup data n = 500 m = 50 f = lambda x: np.sin(x) * np.exp(-x**2 / 50) X = np.random.uniform(-10, 10, size=n) X = np.sort(X) y = f(X) + np.random.normal(0, 1, size=n) y -= np.mean(y) x_min, x_max = np.min(X), np.max(X) U = np.linspace(x_min, x_max, m).reshape(-1, 1) X = X.reshape(-1, 1) hyp_cov = np.asarray([np.log(1), np.log(2)]) hyp_lik = float(np.log(1)) hyp_old = {'mean': [], 'lik': hyp_lik, 'cov': hyp_cov} hyp = gp.train_exact(X, y.reshape(-1, 1), hyp_old) hyp_cov = hyp['cov'][0] sigmasq = np.exp(2 * hyp['lik']) kernel = SEiso() distill = Distillation(X=X, y=y, U=U, kernel=kernel, hyp=hyp_cov, num_iters=10, eta=5e-4, sigmasq=sigmasq, width=3, use_kmeans=True, optimizer='sgd') distill.grad_descent() distill.precompute(use_true_K=False) xx = np.linspace(x_min, x_max, 2 * n) mm_true, vv_true = gp.predict_exact(X, y.reshape(-1, 1), xx.reshape(-1, 1), hyp=hyp) mm = [] vv = [] opt = {'cg_maxit': 500, 'cg_tol': 1e-5} k = n / 2 hyp = gp.train_kiss(X, y.reshape(-1, 1), k, hyp=hyp_old, opt=opt) mm_kiss, vv_kiss = gp.predict_kiss(X, y.reshape(-1, 1), xx.reshape(-1, 1), k, hyp=hyp, opt=opt) for xstar in xx: xstar = np.asarray([xstar]) mstar, vstar = distill.predict(xstar, width=3) vv.append(vstar) mm.append(mstar) mm = np.asarray(mm).flatten() vv = np.asarray(vv).flatten() mm_kiss = np.asarray(mm_kiss).flatten() vv_kiss = np.asarray(vv_kiss).flatten() plt.fill_between(xx, mm - np.sqrt(vv) * 2, mm + np.sqrt(vv) * 2, color='gray', alpha=.5) plt.plot(xx, mm_true, color='y', lw=3, label='exact mean') plt.plot(xx, mm, color='r', lw=3, label='distill mean', ls='dotted') plt.plot(xx, mm_kiss, color='g', lw=3, label='kiss mean', ls=':') plt.plot(xx, f(xx), lw=3, label='true value', ls='dashed') plt.scatter(X, y, color='m', label='train data', marker='+') plt.xlim([x_min, x_max]) plt.legend() plt.show() plt.plot(xx, vv_kiss, color='g', lw=3, label='kiss var', ls=':') plt.plot(xx, vv_true, color='y', lw=3, label='exact var') plt.plot(xx, vv, color='r', lw=3, label='distill var', ls='dotted') plt.xlim([x_min, x_max]) plt.legend() plt.show()
["sarcos_inv"]) # Inputs (7 joint positions, 7 joint velocities, 7 joint accelerations) Xtrain = train[:100, :21].T # Outputs (7 joint torques) Ytrain = normalize(train[:100, 21:23])[0] # Load test set test = np.matrix( scipy.io.loadmat("/home/victor/Documents/MTL-GP/_data/sarcos_inv_test.mat") ["sarcos_inv_test"]) Xtest = test[:100, :21].T Ytest = normalize(test[:100, 21:23])[0] # my_GP = GaussianProcess(SMKernel(Xtrain.shape[0], 3), Xtrain) # my_GP = GaussianProcess(DotKernel(), Xtrain) my_GP = GaussianProcess(SEKernel(), Xtrain) my_GP.add_task(Ytrain[:, 0]) my_GP.add_task(Ytrain[:, 1]) # Ytrain = my_GP.gpr_normalize() x_star = Xtest[:, 0:100] # my_GP.gpr_make_prediction(my_GP.cov_function.INITIAL_GUESS, [0, 1], x_star) print my_GP.gpr_optimize([0, 1], x_star) # print my_GP.mean.T # print Ytest.T print np.mean(abs((my_GP.mean[:, 0] - Ytest[:, 0]))) print np.mean(abs((my_GP.mean[:, 1] - Ytest[:, 1]))) print my_GP.mlog_ML ''' for i in range(10): x_star = Xtest[:, i]
def precompute_hyperp(self, K=100, K_manifold=-1, sigma=-1, dim=-1, simORreal='sim', discreteORcont='discrete'): print( '[data_load] One time pre-computation of GP hyper-parameters data - THIS WILL TAKE A WHILE...!!!!' ) if K_manifold > 0: if self.dr == 'diff': from dr_diffusionmaps import DiffusionMap DR = DiffusionMap(sigma=sigma, embedding_dim=dim) elif self.dr == 'spec': from spectralEmbed import spectralEmbed DR = spectralEmbed(embedding_dim=dim) def reduction(sa, X, Y, K_manifold): inx = DR.ReducedClosestSetIndices(sa, X, k_manifold=K_manifold) return X[inx, :][0], Y[inx, :][0] from gp import GaussianProcess import pickle if extend_previous_opt and os.path.exists( self.path + self.prefix + 'opt_data_' + discreteORcont + self.postfix + '_k' + str(K) + '.obj'): with open( self.path + self.prefix + 'opt_data_' + discreteORcont + self.postfix + '_k' + str(K) + '.obj', 'rb') as f: SA_opt, theta_opt, K_opt, _ = pickle.load(f) print( '[data_load] Loaded precious optimized data with size %d...' % SA_opt.shape[0]) SA_opt = list(SA_opt) theta_opt = list(theta_opt) K_opt = list(K_opt) else: SA_opt = [] theta_opt = [] K_opt = [] # [theta_opt.append([]) for _ in range(self.state_dim)] # List for each dimension N = 4650 for i in range(N): print( '[data_load] Computing hyper-parameters for data point %d out of %d.' % (i, N)) sa = self.Xtrain[np.random.randint(self.Xtrain.shape[0]), :] best = {'k': 0, 'loglike': 1e9, 'theta': 0} for jj in range(1): Kcandidate = [ K ] #range(2, 253, 50) if jj == 0 else range(max(best['k'] - 50, 2), best['k'] + 50, 10) for k in Kcandidate: print('[data_load] Running with k = %d (i=%d)...' % (k, i)) try: idx = self.kdt.query(sa.reshape(1, -1), k=k, return_distance=False) X_nn = self.Xtrain[idx, :].reshape( k, self.state_action_dim) Y_nn = self.Ytrain[idx, :].reshape(k, self.state_dim) except: continue if K_manifold > 0: X_nn, Y_nn = reduction(sa, X_nn, Y_nn, K_manifold) fail = False Theta = [] LogLikeAvg = 0 for d in range(self.state_dim): try: gp_est = GaussianProcess( X_nn[:, :self.state_action_dim], Y_nn[:, d], optimize=True, theta=None) # Optimize to get hyper-parameters except: print('[data_load] Singular!') fail = True break Theta.append(gp_est.cov.theta) LogLikeAvg += gp_est.cov.neg_log_marginal_likelihood_value if fail: continue LogLikeAvg /= self.state_dim if LogLikeAvg < best['loglike']: best['loglike'] = LogLikeAvg best['k'] = k best['theta'] = Theta print "Best: k = ", best['k'], ", log-likelihood = ", best[ 'loglike'] SA_opt.append(sa) theta_opt.append(best['theta']) K_opt.append(best['k']) print('[data_load] %d optimization points exist.' % len(SA_opt)) if not (i % 50): self.SA_opt = np.array(SA_opt) self.theta_opt = np.array(theta_opt) self.K_opt = np.array(K_opt) self.opt_kdt = KDTree(SA_opt, leaf_size=20, metric='euclidean') with open( self.path + self.prefix + 'opt_data_' + discreteORcont + self.postfix + '_k' + str(K) + '.obj', 'wb') as f: pickle.dump([ self.SA_opt, self.theta_opt, self.K_opt, self.opt_kdt ], f) print('[data_load] Saved hyper-parameters data.')
import numpy as np import tf from controller import Controller from geometry_msgs.msg import PoseStamped from dynamixel_workbench_msgs.srv import SetPID from object_detection.srv import ObjectDetection from geometry_msgs.msg import Pose, Point, Quaternion from gp import GaussianProcess # Initialize controller ctrl = Controller() # Initialize GP gp = GaussianProcess() gp.fit_GP() MIN_JOINT_MOTION_FOR_HAND_OVER = 0.1 # define state Idle class Idle(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['gotToolInput']) def execute(self, userdata): ctrl.open_gripper() # ctrl.set_camera_angles(ctrl.HOME_POS_CAMERA_01) ctrl.set_arm_joint_angles(ctrl.HOME_POS_MANIPULATOR_00) rospy.loginfo('Executing state IDLE')
import math import numpy as np from gp import GaussianProcess, DotKernel from gp import SMKernel N_ROWS = 5 N_COLS = 100 X = np.matrix([(0.1*(1+i))**(1+j) for j in range(N_ROWS) for i in range(N_COLS)]).reshape((N_ROWS,N_COLS)) y1 = np.matrix([(0.1*(1+i)) for i in range(N_COLS)]).T y2 = np.matrix([np.sin(0.1*(1+i)) for i in range(N_COLS)]).T my_GP = GaussianProcess(SMKernel(N_ROWS, 1), X) my_GP = GaussianProcess(DotKernel(), X) my_GP.add_task(y1) my_GP.add_task(y2) x_star = np.matrix([3**(1+j) for j in range(N_ROWS)]).T my_GP.gpr_make_prediction(my_GP.cov_function.INITIAL_GUESS, 0, x_star) print my_GP.gpr_optimize(0, x_star) print my_GP.mean, my_GP.variance print my_GP.mlog_ML my_GP.gpr_make_prediction(my_GP.cov_function.INITIAL_GUESS, 0, x_star) print my_GP.mean, my_GP.variance
import scipy.io import numpy as np from gp import GaussianProcess, SMKernel, DotKernel, SEKernel from gp import LogisticFunction from numpy.random import normal as dnorm means, sigma, N = [-6,0,2], 0.8, 30 X = np.matrix( np.concatenate( [dnorm(i, sigma, N) for i in means] ) ) # my_GP = GaussianProcess(DotKernel(), X, LogisticFunction()) my_GP = GaussianProcess(SEKernel(), X, LogisticFunction()) # my_GP = GaussianProcess(SMKernel(X.shape[0], 3), X, LogisticFunction()) y = np.matrix( [-1]*N + [1]*N + [-1]*N ).T my_GP.add_task(y) y2 = [] for i in range(3*N): if X[0,i] > 0: y2.append(1) else: y2.append(-1) y2 = np.matrix(y2).T my_GP.add_task(y2)