def VanderPol_continuous_prior_mean_Michelangelo_deriv(x, u, prior_kwargs):
mu = prior_kwargs.get('mu')
x = reshape_pt1(x)
u = reshape_pt1(u)
deriv_x = reshape_dim1(-2 * mu * x[:, 0] * x[:, 1] - np.ones_like(x[:, 0]))
deriv_xdot = reshape_dim1(mu * (1 - x[:, 0]**2))
phi_deriv = reshape_pt1(np.concatenate((deriv_x, deriv_xdot), axis=1))
return phi_deriv
def mass_spring_mass_vdot(z, kwargs):
m1 = kwargs.get('m1')
k1 = kwargs.get('k1')
k2 = kwargs.get('k2')
z = reshape_pt1(z)
x1d3 = reshape_pt1(z[:, 3])
A = k1 / m1 + 3 * k2 / m1 * mass_spring_mass_v(z, kwargs)**2
vdot = reshape_pt1(x1d3 / A)
return vdot
def VanderPol_dynamics(t, x, u, t0, init_control, process_noise_var, kwargs):
mu = kwargs.get('mu')
x = reshape_pt1(x)
u = reshape_pt1(u(t, kwargs, t0, init_control))
A = reshape_pt1([[0, 1], [-1, 0]])
F = reshape_pt1([0, mu * (1 - x[:, 0]**2) * x[:, 1] - x[:, 0]])
xdot = reshape_pt1(np.dot(A, reshape_pt1_tonormal(x)) + F + u)
if process_noise_var != 0:
xdot += np.random.normal(0, np.sqrt(process_noise_var), xdot.shape)
return xdot
def duffing_continuous_prior_mean_Michelangelo_deriv(x, u, prior_kwargs):
alpha = prior_kwargs.get('alpha')
beta = prior_kwargs.get('beta')
delta = prior_kwargs.get('delta')
x = reshape_pt1(x)
deriv = reshape_dim1(-3 * beta * ((x[:, 0])**2) - alpha)
phi_deriv = reshape_pt1(
np.concatenate((deriv, reshape_dim1(-delta * np.ones_like(x[:, 0]))),
axis=1))
return phi_deriv
def mass_spring_mass_xtoz(x, kwargs):
m1 = kwargs.get('m1')
k1 = kwargs.get('k1')
k2 = kwargs.get('k2')
x = reshape_pt1(x)
z = reshape_pt1(x)
z[:, 2] = k1 / m1 * (x[:, 2] - x[:, 0]) + k2 / m1 * (x[:, 2] - x[:, 0])**3
z[:, 3] = k1 / m1 * (x[:, 3] - x[:, 1]) + \
3 * k2 / m1 * (x[:, 3] - x[:, 1]) * (x[:, 2] - x[:, 0]) ** 2
return reshape_pt1(z)
def wdc_arm_continuous_prior_mean_Michelangelo_u(x, u, prior_kwargs):
inertia = prior_kwargs.get('inertia')
m = prior_kwargs.get('m')
lG = prior_kwargs.get('lG')
g = prior_kwargs.get('g')
x = reshape_pt1(x)
u = reshape_pt1(u)
# phi = reshape_dim1(-m * g * lG / inertia * np.sin(x[:, 0]))
phi = reshape_dim1(np.zeros_like(x[:, 0]) + u[:, 1])
return phi
def derivative_function(X, U, y_observed, GP):
X = reshape_pt1(X)
u = lambda t, kwargs, t0, init_control: reshape_pt1(U)[t]
y = lambda t, kwargs: reshape_pt1(y_observed)[t]
Xdot = np.array([
observer(t, X[t], u, y, t0, init_control, GP, dyn_kwargs)
for t in range(len(X))
])
Xdot = Xdot.reshape(X.shape)
return Xdot.reshape(X.shape)
def duffing_observer_Delgado_GP(t, xhat, u, y, t0, init_control, GP, kwargs):
alpha = kwargs.get('alpha')
beta = kwargs.get('beta')
delta = kwargs.get('delta')
xhat = reshape_pt1(xhat)
y = reshape_pt1(y(t, kwargs))
u = reshape_pt1(u(t, kwargs, t0, init_control))
# My gains
l1 = delta - 5
l2 = alpha - delta ** 2 + 3 * beta * xhat[:, 0] ** 2
# # Delgado gains
# l1 = - 5
# l2 = - (alpha + 3 * xhat[:, 0] ** 2)
if GP:
if 'GP' in GP.__class__.__name__:
mean, var, lowconf, uppconf = GP.predict(reshape_pt1(xhat),
reshape_pt1(u))
if not kwargs.get('continuous_model'):
# In this case we have continuous observer dynamics, but GP is
# discrete # TODO better than Euler?
mean = (mean - xhat) / GP.prior_kwargs.get('dt')
GP.prior_kwargs['observer_gains'] = {'l1': l1, 'l2': l2}
else:
mean = GP(reshape_pt1(xhat), reshape_pt1(u),
kwargs.get('prior_kwargs'))
else:
mean = np.zeros_like(u)
LC = reshape_pt1([[l1 * (xhat[:, 0] - y), l2 * (xhat[:, 0] - y)]])
xhatdot = reshape_pt1(mean + LC)
return xhatdot
def plot_GP(dyn_GP, grid, verbose=False):
xdim = dyn_GP.X.shape[1]
udim = dyn_GP.U.shape[1]
for i in range(xdim + udim):
dataplot = np.linspace(np.min(grid[:, i]), np.max(grid[:, i]),
dyn_GP.nb_plotting_pts)
data = np.zeros((dyn_GP.nb_plotting_pts, xdim + udim))
data[:, i] = dataplot
x = data[:, :xdim]
u = data[:, xdim:]
predicted_mean, predicted_var, predicted_lowconf, \
predicted_uppconf = dyn_GP.predict(x, u)
if not dyn_GP.ground_truth_approx:
true_predicted_mean = predicted_mean.copy()
for idx, _ in enumerate(true_predicted_mean):
true_predicted_mean[idx] = reshape_pt1(
dyn_GP.true_dynamics(reshape_pt1(x[idx]),
reshape_pt1(u[idx])))
df = pd.DataFrame(true_predicted_mean)
df.to_csv(os.path.join(dyn_GP.results_folder,
'GP_plot_true' + str(i) + '.csv'),
header=False)
df = pd.DataFrame(predicted_mean)
df.to_csv(os.path.join(dyn_GP.results_folder,
'GP_plot_estim' + str(i) + '.csv'),
header=False)
for j in range(dyn_GP.Y.shape[1]):
# Plot function learned by GP
name = 'GP_plot' + str(i) + str(j) + '.pdf'
if not dyn_GP.ground_truth_approx:
plt.plot(data[:, i],
true_predicted_mean[:, j],
label='True function',
c='darkgreen')
plt.plot(data[:, i],
predicted_mean[:, j],
label='GP mean',
alpha=0.9)
plt.fill_between(data[:, i],
predicted_lowconf[:, j],
predicted_uppconf[:, j],
facecolor='blue',
alpha=0.2)
if not dyn_GP.ground_truth_approx:
plt.title('Visualization of GP posterior')
else:
plt.title('Visualization of GP posterior over training data')
plt.legend()
plt.xlabel('Input state ' + str(i))
plt.ylabel('GP prediction ' + str(j) + '(x)')
plt.savefig(os.path.join(dyn_GP.results_folder, name),
bbox_inches='tight')
if verbose:
plt.show()
plt.close('all')
def harmonic_oscillator_observer_GP(t, xhat, u, y, t0, init_control, GP,
kwargs):
k = kwargs.get('k')
m = kwargs.get('m')
xhat = reshape_pt1(xhat)
y = reshape_pt1(y(t, kwargs))
u = reshape_pt1(u(t, kwargs, t0, init_control))
# Gains
l1 = - 1
l2 = k / m - 1
if GP:
if 'GP' in GP.__class__.__name__:
mean, var, lowconf, uppconf = GP.predict(reshape_pt1(xhat),
reshape_pt1(u))
if not kwargs.get('continuous_model'):
# In this case we have continuous observer dynamics, but GP is
# discrete # TODO better than Euler?
mean = (mean - xhat) / GP.prior_kwargs.get('dt')
GP.prior_kwargs['observer_gains'] = {'l1': l1, 'l2': l2}
else:
mean = GP(reshape_pt1(xhat), reshape_pt1(u),
kwargs.get('prior_kwargs'))
else:
mean = np.zeros_like(u)
LC = reshape_pt1([[l1 * (xhat[:, 0] - y), l2 * (xhat[:, 0] - y)]])
xhatdot = reshape_pt1(mean + LC)
return xhatdot
def pendulum_continuous_prior_mean_Michelangelo_deriv(x, u, prior_kwargs):
k = prior_kwargs.get('k')
m = prior_kwargs.get('m')
g = prior_kwargs.get('g')
l = prior_kwargs.get('l')
x = reshape_pt1(x)
deriv = reshape_dim1(-g / l * np.cos(x[:, 0]))
phi_deriv = reshape_pt1(
np.concatenate((deriv, reshape_dim1(-k / m * np.ones_like(x[:, 0]))),
axis=1))
return phi_deriv
def harmonic_oscillator_dynamics(t, x, u, t0, init_control, process_noise_var,
kwargs):
k = kwargs.get('k')
m = kwargs.get('m')
x = reshape_pt1(x)
u = reshape_pt1(u(t, kwargs, t0, init_control))
A = reshape_pt1([[0, 1], [-k / m, 0]])
xdot = reshape_pt1(np.dot(A, reshape_pt1_tonormal(x)) + u)
if process_noise_var != 0:
xdot += np.random.normal(0, np.sqrt(process_noise_var), xdot.shape)
return xdot
def harmonic_oscillator_continuous_prior_mean(x, u, prior_kwargs):
k = prior_kwargs.get('k')
m = prior_kwargs.get('m')
x = reshape_pt1(x)
u = reshape_pt1(u)
A = reshape_pt1([[0, 1], [-k / m, 0]])
# Amult = np.array([np.dot(A, x[i, :]) for i in range(len(x))])
dot = lambda a: np.dot(A, a)
Amult = np.apply_along_axis(func1d=dot, axis=1, arr=x)
xdot = reshape_pt1(Amult + u)
return xdot
def mass_spring_mass_v(z, kwargs):
m1 = kwargs.get('m1')
k1 = kwargs.get('k1')
k2 = kwargs.get('k2')
z = reshape_pt1(z)
x1d2 = reshape_pt1(z[:, 2])
p = k1 / k2
q = -m1 / k2 * x1d2
D = np.power(p / 3, 3) + np.power(q / 2, 2)
A = np.cbrt(-q / 2 + np.sqrt(D))
B = np.cbrt(-q / 2 - np.sqrt(D))
v = reshape_pt1(A + B)
return v
def sin_controller_1D(t, kwargs, t0, init_control):
gamma = kwargs.get('gamma')
omega = kwargs.get('omega')
if np.isscalar(t):
if t == t0:
u = reshape_pt1(init_control)
else:
u = reshape_pt1([[gamma * np.cos(omega * t)]])
else:
u = reshape_dim1(gamma * np.cos(omega * t))
if t[0] == t0:
u[0] = reshape_pt1(init_control)
return u
def wdc_arm_continuous_justvelocity_prior_mean(x, u, prior_kwargs):
inertia = prior_kwargs.get('inertia')
m = prior_kwargs.get('m')
lG = prior_kwargs.get('lG')
g = prior_kwargs.get('g')
x = reshape_pt1(x)
u = reshape_pt1(u)
# F1 = reshape_dim1(np.zeros_like(x[:, 0]))
# F2 = reshape_dim1(-m * g * lG / inertia * np.sin(x[:, 0]))
# F = reshape_pt1(np.concatenate((F1, F2), axis=1))
F = reshape_pt1(np.zeros_like(u))
vdot = reshape_dim1(F[:, 1] + u[:, 1])
return vdot
def evaluate_val_kalman_rollouts(self,
observer,
observe_data,
discrete_observer,
no_GP_in_observer=False,
only_prior=False):
if len(self.val_rollout_list) == 0:
return 0
rollout_list, rollout_RMSE, rollout_SRMSE, rollout_log_AL, \
rollout_stand_log_AL = run_rollouts(
self, np.array(self.val_rollout_list),
folder=self.validation_folder, observer=observer,
observe_data=observe_data, discrete_observer=discrete_observer,
kalman=True, no_GP_in_observer=no_GP_in_observer,
only_prior=only_prior)
self.specs['val_kalman_rollout_RMSE'] = rollout_RMSE
self.specs['val_kalman_rollout_SRMSE'] = rollout_SRMSE
self.specs['val_kalman_rollout_log_AL'] = rollout_log_AL
self.specs['val_kalman_rollout_stand_log_AL'] = rollout_stand_log_AL
self.val_kalman_rollout_RMSE = \
np.concatenate((self.val_kalman_rollout_RMSE, reshape_pt1(
np.array([self.sample_idx, rollout_RMSE]))), axis=0)
self.val_kalman_rollout_SRMSE = \
np.concatenate((self.val_kalman_rollout_SRMSE, reshape_pt1(
np.array([self.sample_idx, rollout_SRMSE]))), axis=0)
self.val_kalman_rollout_log_AL = \
np.concatenate((self.val_kalman_rollout_log_AL, reshape_pt1(
np.array([self.sample_idx, rollout_log_AL]))), axis=0)
self.val_kalman_rollout_stand_log_AL = np.concatenate(
(self.val_kalman_rollout_stand_log_AL,
reshape_pt1(np.array([self.sample_idx, rollout_stand_log_AL]))),
axis=0)
self.variables['val_kalman_rollout_RMSE'] = \
self.val_kalman_rollout_RMSE
self.variables['val_kalman_rollout_SRMSE'] = \
self.val_kalman_rollout_SRMSE
self.variables['val_kalman_rollout_log_AL'] = \
self.val_kalman_rollout_log_AL
self.variables['val_kalman_rollout_stand_log_AL'] = \
self.val_kalman_rollout_stand_log_AL
plot_val_kalman_rollout_data(self, folder=self.validation_folder)
complete_val_rollout_list = np.concatenate(
(self.val_rollout_list, rollout_list), axis=1)
save_kalman_rollout_variables(
self.validation_folder,
len(complete_val_rollout_list),
complete_val_rollout_list,
step=self.step - 1,
ground_truth_approx=self.ground_truth_approx,
title='Val_rollouts')
def duffing_dynamics(t, x, u, t0, init_control, process_noise_var, kwargs):
alpha = kwargs.get('alpha')
beta = kwargs.get('beta')
delta = kwargs.get('delta')
x = reshape_pt1(x)
u = reshape_pt1(u(t, kwargs, t0, init_control))
A = reshape_pt1([[0, 1], [-alpha, -delta]])
F1 = reshape_dim1(np.zeros_like(x[:, 0]))
F2 = reshape_dim1(-beta * x[:, 0]**3)
F = reshape_pt1(np.concatenate((F1, F2), axis=1))
xdot = reshape_pt1(np.dot(A, reshape_pt1_tonormal(x)) + F + u)
if process_noise_var != 0:
xdot += np.random.normal(0, np.sqrt(process_noise_var), xdot.shape)
return xdot
def derivative_function(X, U, y_observed, GP):
X = reshape_pt1(X)
u = lambda t, kwargs, t0, init_control: reshape_pt1(U)[t]
y = lambda t, kwargs: reshape_pt1(y_observed)[t]
# time = np.arange(len(X))
# obs_1D = lambda t: config.observer(t, X[t], u, y, config.t0,
# config.init_control, GP, config)
# Xdot = np.apply_along_axis(func1d=obs_1D, axis=0, arr=time)
Xdot = np.array([
config.observer(t, X[t], u, y, config.t0, config.init_control,
GP, config) for t in range(len(X))
])
Xdot = Xdot.reshape(X.shape)
return Xdot.reshape(X.shape)
def true_dynamics(x, control):
m1 = config.get('m1')
m2 = config.get('m2')
k1 = config.get('k1')
k2 = config.get('k2')
z = reshape_pt1(x)
z3 = reshape_pt1(z[:, 2])
u = control
v = reshape_pt1_tonormal(mass_spring_mass_v(z, config))
vdot = reshape_pt1_tonormal(mass_spring_mass_vdot(z, config))
return reshape_pt1(k1 * (m1 * m2) * (u - (m1 + m2) * z3) +
(3 * k2) / (m1 * m2) *
(u - (m1 + m2) * z3) * v**2 +
(6 * k2) / m1 * v * vdot**2)
def sin_controller_02D(t, kwargs, t0, init_control):
gamma = kwargs.get('gamma')
omega = kwargs.get('omega')
if np.isscalar(t):
if t == t0:
u = reshape_pt1(init_control)
else:
u = reshape_pt1([[0, gamma * np.cos(omega * t)]])
else:
u = reshape_pt1(np.concatenate((reshape_dim1(
np.zeros(len(t))), reshape_dim1(gamma * np.cos(omega * t))),
axis=1))
if t[0] == t0:
u[0] = reshape_pt1(init_control)
return u
def wdc_arm_dynamics(t, x, u, t0, init_control, process_noise_var, kwargs):
inertia = kwargs.get('inertia')
m = kwargs.get('m')
lG = kwargs.get('lG')
g = kwargs.get('g')
x = reshape_pt1(x)
u = reshape_pt1(u(t, kwargs, t0, init_control))
A = reshape_pt1([[0, 1], [0, 0]])
F1 = reshape_dim1(np.zeros_like(x[:, 0]))
F2 = reshape_dim1(-m * g * lG / inertia * np.sin(x[:, 0]))
F = reshape_pt1(np.concatenate((F1, F2), axis=1))
xdot = reshape_pt1(np.dot(A, reshape_pt1_tonormal(x)) + F + u)
if process_noise_var != 0:
xdot += np.random.normal(0, np.sqrt(process_noise_var), xdot.shape)
return xdot
def duffing_continuous_prior_mean(x, u, prior_kwargs):
alpha = prior_kwargs.get('alpha')
beta = prior_kwargs.get('beta')
delta = prior_kwargs.get('delta')
x = reshape_pt1(x)
u = reshape_pt1(u)
A = reshape_pt1([[0, 1], [-alpha, -delta]])
# Amult = np.array([np.dot(A, x[i, :]) for i in range(len(x))])
dot = lambda a: np.dot(A, a)
Amult = np.apply_along_axis(func1d=dot, axis=1, arr=x)
F1 = reshape_dim1(np.zeros_like(x[:, 0]))
F2 = reshape_dim1(-beta * (x[:, 0])**3)
F = reshape_pt1(np.concatenate((F1, F2), axis=1))
xdot = reshape_pt1(Amult + F + u)
return xdot
def evaluate_test_closedloop_rollouts(self,
observer,
observe_data,
no_GP_in_observer=False):
if len(self.test_rollout_list) == 0:
return 0
rollout_list, rollout_RMSE, rollout_SRMSE, rollout_log_AL, \
rollout_stand_log_AL = run_rollouts(
self, np.array(self.test_rollout_list), folder=self.test_folder,
observer=observer, observe_data=observe_data, closedloop=True,
no_GP_in_observer=no_GP_in_observer)
self.specs['test_closedloop_rollout_RMSE'] = rollout_RMSE
self.specs['test_closedloop_rollout_SRMSE'] = rollout_SRMSE
self.specs['test_closedloop_rollout_log_AL'] = rollout_log_AL
self.specs['test_closedloop_rollout_stand_log_AL'] = \
rollout_stand_log_AL
self.test_closedloop_rollout_RMSE = \
np.concatenate((self.test_closedloop_rollout_RMSE, reshape_pt1(
np.array([self.sample_idx, rollout_RMSE]))), axis=0)
self.test_closedloop_rollout_SRMSE = \
np.concatenate((self.test_closedloop_rollout_SRMSE, reshape_pt1(
np.array([self.sample_idx, rollout_SRMSE]))), axis=0)
self.test_closedloop_rollout_log_AL = \
np.concatenate((self.test_closedloop_rollout_log_AL, reshape_pt1(
np.array([self.sample_idx, rollout_log_AL]))), axis=0)
self.test_closedloop_rollout_stand_log_AL = np.concatenate(
(self.test_closedloop_rollout_stand_log_AL,
reshape_pt1(np.array([self.sample_idx, rollout_stand_log_AL]))),
axis=0)
self.variables['test_closedloop_rollout_RMSE'] = \
self.test_closedloop_rollout_RMSE
self.variables['test_closedloop_rollout_SRMSE'] = \
self.test_closedloop_rollout_SRMSE
self.variables['test_closedloop_rollout_log_AL'] = \
self.test_closedloop_rollout_log_AL
self.variables['test_closedloop_rollout_stand_log_AL'] = \
self.test_closedloop_rollout_stand_log_AL
plot_test_closedloop_rollout_data(self, folder=self.test_folder)
complete_test_rollout_list = np.concatenate(
(self.test_rollout_list, rollout_list), axis=1)
save_closedloop_rollout_variables(
self.test_folder,
len(complete_test_rollout_list),
complete_test_rollout_list,
step=self.step - 1,
ground_truth_approx=self.ground_truth_approx,
title='Test_rollouts')
def harmonic_oscillator_continuous_prior_mean_Michelangelo_u(
x, u, prior_kwargs):
k = prior_kwargs.get('k')
m = prior_kwargs.get('m')
x = reshape_pt1(x)
phi = reshape_dim1(-k / m * x[:, 0] + u[:, 1])
return phi
def WDC_justvelocity_discrete_observer_highgain_GP(t, xhat, u, y, t0,
init_control, GP, kwargs):
xhatdot = WDC_justvelocity_observer_highgain_GP(t, xhat, u, y, t0,
init_control, GP, kwargs)
xnext = reshape_pt1(xhat + kwargs.get('dt_before_subsampling') * xhatdot)
# TODO better than Euler?
return xnext
def null_controller(t, kwargs, t0, init_control):
if np.isscalar(t):
t = np.array([t])
else:
t = reshape_dim1_tonormal(t)
u = reshape_pt1(np.zeros((len(t), init_control.shape[1])))
return u
def MSM_continuous_Michelangelo_prior_mean_u(x, u, prior_kwargs):
x = reshape_pt1(x)
u = reshape_pt1(u)
m1 = prior_kwargs.get('m1')
m2 = prior_kwargs.get('m2')
k1 = prior_kwargs.get('k1')
k2 = prior_kwargs.get('k2')
z = reshape_pt1(x)
z3 = reshape_pt1(z[:, 2])
v = reshape_pt1_tonormal(mass_spring_mass_v(z, prior_kwargs))
vdot = reshape_pt1_tonormal(mass_spring_mass_vdot(z, prior_kwargs))
# phi = reshape_pt1(
# k1 * (m1 * m2) * (u - (m1 + m2) * z3) + (3 * k2) / (m1 * m2) * (
# u - (m1 + m2) * z3) * v ** 2 + (
# 6 * k2) / m1 * v * vdot ** 2)
phi = np.zeros((x.shape[0], 1))
return phi
def wdc_arm_continuous_prior_mean(x, u, prior_kwargs):
inertia = prior_kwargs.get('inertia')
m = prior_kwargs.get('m')
lG = prior_kwargs.get('lG')
g = prior_kwargs.get('g')
x = reshape_pt1(x)
u = reshape_pt1(u)
A = reshape_pt1([[0, 1], [0, 0]])
# Amult = np.array([np.dot(A, x[i, :]) for i in range(len(x))])
dot = lambda a: np.dot(A, a)
Amult = np.apply_along_axis(func1d=dot, axis=1, arr=x)
# F1 = reshape_dim1(np.zeros_like(x[:, 0]))
# F2 = reshape_dim1(m * g * lG / inertia * np.sin(x[:, 0]))
# F = reshape_pt1(np.concatenate((F1, F2), axis=1))
F = reshape_pt1(np.zeros_like(u))
xdot = reshape_pt1(Amult + F + u)
return xdot
def harmonic_oscillator_continuous_prior_mean_Michelangelo_deriv(
x, u, prior_kwargs):
k = prior_kwargs.get('k')
m = prior_kwargs.get('m')
deriv = reshape_dim1(-k / m * np.ones_like(x[:, 0]))
phi_deriv = reshape_pt1(
np.concatenate((deriv, np.zeros_like(deriv)), axis=1))
return phi_deriv
