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)