def _wait_for_upright_pole(self, verbose=False): if verbose: print("\tCentering the Pole:\t\t", end="") t_max = 15.0 upright = False pos_th = np.array([self.c_lim, 2. * np.pi / 180.]) vel_th = 0.1 * np.ones(2) th = np.hstack((pos_th, vel_th)) # Wait until the pole is upright: t0 = time.time() while (time.time() - t0) <= t_max: state = self._zero_sim_step() transformed_state = np.array(state, copy=True) transformed_state[1] -= np.sign(transformed_state[1]) * np.pi if np.all(np.abs(transformed_state) <= th): upright = True break if not upright: if verbose: print("\u274C") time.sleep(0.1) state_str = np.array2string( np.abs(state), suppress_small=True, precision=2, formatter={'float_kind': lambda x: "{0:+05.2f}".format(x)}) th_str = np.array2string( th, suppress_small=True, precision=2, formatter={'float_kind': lambda x: "{0:+05.2f}".format(x)}) raise TimeoutError( "The Pole is not upright, i.e., {0} > {1}".format( state_str, th_str)) elif verbose: print("\u2713") return
def DefaultOutput(step, progress, mag, nuggets, scales): sys.stdout.write( "\rProcessing %i iteration, progress = %.1f%% gradient magnitude = %2.6f, nuggets = %2.3f, scales = %s " % ( step, progress, mag, nuggets, np.array2string(scales, formatter={"float_kind": lambda x: "%02.3f" % x}), )) if progress == 100: sys.stdout.write("\n") sys.stdout.flush()
def optimize(self) -> None: """ This is used to optimize the hyperparams for the GP :return: None """ # start optimizing with current parameter set params = self._wrap_kernel_hyperparams() logging.debug("Length scales before: {}".format( np.array2string(np.exp(self.length_scales)))) logging.debug("Sigma_f before: {}".format( np.array2string(np.exp(self.sigma_f)))) logging.debug("Sigma_eps before: {}".format( np.array2string(np.exp(self.sigma_eps)))) try: logging.info("Optimization with L-BFGS-B started.") res = minimize(value_and_grad(self._optimize_hyperparams), params, jac=True, method='L-BFGS-B') except Exception: # use CG if numerical instabilities occur during optimization logging.info("Optimization with CG started.") res = minimize(value_and_grad(self._optimize_hyperparams), params, jac=True, method='CG') best_params = res.x self.length_scales, self.sigma_f, self.sigma_eps = self.unwrap_params( best_params) logging.debug("Length scales after: {}".format( np.array2string(np.exp(self.length_scales)))) logging.debug("Sigma_f after: {}".format( np.array2string(np.exp(self.sigma_f)))) logging.debug("Sigma_eps after: {}".format( np.array2string(np.exp(self.sigma_eps)))) # compute betas and K_inv which is required for later predictions self.compute_matrices()
data = DataReader(filename).get_data() cal = SystemCalibrator(*data) result = cal.optimize() print print result print # Perform projection for validation pixels = projectPoints(data[0], data[1], result.x[:7], result.x[7:]) diff = np.abs(pixels - data[1]) print "Pixel diffs (one axis)" print "----------------------" print "Min:", np.min(diff) print "Max:", np.max(diff) print dists = np.linalg.norm(diff, axis=0) print "Pixel distances (two axis)" print "--------------------------" print "Min:", np.min(dists) print "Max:", np.max(dists) print print "XYZ:", result.x[:3] print "RPY:", np.array(transf.euler_from_quaternion(result.x[3:7])) print K, P = cameraMats(result.x[-4:]) print "K:", np.array2string(K.flatten(), separator=", ") print "P:", np.array2string(P.flatten(), separator=", ") print "D:", np.array2string(result.x[7:11], separator=", ")
def EarlyStopping( input_, target, validation_runs, initial_scales, initial_nuggets, input_pipe, output_pipe, scales_rate=0.001, nuggets_rate=0.003, nsteps=1500, ): valid_par = input_[validation_runs.tolist()] valid_obs = target[validation_runs.tolist()] input_ = np.delete(input_, validation_runs, 0) target = np.delete(target, validation_runs, 0) input_pipe.Fit(input_) output_pipe.Fit(target) valid_par = input_pipe.Transform(valid_par) valid_obs = output_pipe.Transform(valid_obs) emulator = EmulatorMultiOutput(input_pipe.Transform(input_), output_pipe.Transform(target)) emulator.SetCovariance(squared_exponential) scales_list = [] nuggets_list = [] for index, emu in enumerate(emulator.emulator_list): gd = GradientDescentForEmulator(scales_rate, nuggets_rate) gd.SetFunc(emu.MarginalLikelihood) nuggets = initial_nuggets scales = initial_scales hist_scales = [] hist_nuggets = [] partial_likelihood = [] for i in range(nsteps): scales, nuggets, grad_scales, grad_nuggets = gd.StepDescent( scales, nuggets) emu.SetScales(scales) emu.SetNuggets(nuggets) emu.StartUp() hist_scales.append(scales) hist_nuggets.append(nuggets) val = 0 for par, exp in zip(valid_par, valid_obs): val += emu.LogLikelihood(par.reshape(1, -1), exp[index].reshape(1, -1)) partial_likelihood.append(val) sys.stdout.write( "\rProcessing %i iteration, likelihood = %f, nuggets = %f, scales = %s" % (i, val, nuggets, np.array2string(scales))) sys.stdout.flush() # plt.plot(partial_likelihood) # plt.show() # get index corresponds to maximum validation log likelihood i, value = max(enumerate(partial_likelihood), key=operator.itemgetter(1)) print("max", i) scales_list.append(hist_scales[i]) nuggets_list.append(hist_nuggets[i]) return np.array(scales_list), np.array(nuggets_list)