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