From 87f53fa7483afeaf62fd92a2ff8c0d04e6189506 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Thu, 14 Nov 2019 22:48:35 +0100 Subject: [PATCH] add KMP example (+fix KMP) --- examples/models/gmm.py | 2 +- examples/models/gmr.py | 2 +- examples/models/kmp.py | 57 ++++ pyrobolearn/models/gaussian.py | 10 +- pyrobolearn/models/gmm/gmm.py | 41 ++- pyrobolearn/models/kmp/kmp.py | 354 +++++++++++++++++------ pyrobolearn/models/kmp/quaternion_kmp.py | 203 +++++++++++++ 7 files changed, 557 insertions(+), 112 deletions(-) create mode 100644 examples/models/kmp.py create mode 100644 pyrobolearn/models/kmp/quaternion_kmp.py diff --git a/examples/models/gmm.py b/examples/models/gmm.py index e546fc4..f46485e 100644 --- a/examples/models/gmm.py +++ b/examples/models/gmm.py @@ -70,7 +70,7 @@ plt.show() means, std_devs = [], [] time_linspace = np.linspace(-6, 6, 100) for t in time_linspace: - g = gmm.condition(np.array([t]), idx_out=[1], idx_in=[0]).approximate_by_single_gaussian() + g = gmm.condition(t, idx_out=1, idx_in=0).approximate_by_single_gaussian() means.append(g.mean[0]) std_devs.append(np.sqrt(g.covariance[0, 0])) diff --git a/examples/models/gmr.py b/examples/models/gmr.py index a9508a4..fccd1af 100644 --- a/examples/models/gmr.py +++ b/examples/models/gmr.py @@ -67,7 +67,7 @@ plt.show() # GMR: condition on the input variable and plot gaussians = [] for t in time_linspace: - g = gmm.condition(np.array([t]), idx_out=[1, 2], idx_in=[0]).approximate_by_single_gaussian() + g = gmm.condition(t, idx_out=[1, 2], idx_in=0).approximate_by_single_gaussian() gaussians.append(g) # plot figures for GMR diff --git a/examples/models/kmp.py b/examples/models/kmp.py new file mode 100644 index 0000000..c7ab4ef --- /dev/null +++ b/examples/models/kmp.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""Provide some examples using KMP. +""" + +import numpy as np +import matplotlib.pyplot as plt + +from pyrobolearn.models.kmp import KMP + + +# create data: Generate random sample following a sine curve +# Ref: https://scikit-learn.org/stable/auto_examples/mixture/plot_gmm_sin.html#sphx-glr-auto-examples-mixture-\ +# plot-gmm-sin-py +n_samples = 100 +np.random.seed(0) +X = np.zeros((n_samples, 2)) +step = 4. * np.pi / n_samples + +for i in range(X.shape[0]): + x = i * step - 6. + X[i, 0] = x + np.random.normal(0, 0.1) + X[i, 1] = 3. * (np.sin(x) + np.random.normal(0, .2)) + +xlim, ylim = [-8, 8], [-8, 8] + +# plot data +plt.title('Training data') +plt.scatter(X[:, 0], X[:, 1]) +plt.show() + +# create KMP +print("Creating the KMP model") +kmp = KMP() + +# fit a KMP on the data +print("Training the KMP...") +kmp.fit(X=X[:, 0].reshape(1, -1, 1), Y=X[:, 1].reshape(1, -1, 1), gmm_num_components=5, mean_reg=1., + covariance_reg=60., database_size_limit=n_samples) +print("Finished the training") + +# predict using the KMP +means, std_devs = [], [] +time_linspace = np.linspace(-6, 6, 100) +for t in time_linspace: + g = kmp.predict_proba(t, return_gaussian=True) + means.append(g.mean[0]) + std_devs.append(np.sqrt(g.covariance[0, 0])) + +means, std_devs = np.asarray(means), np.asarray(std_devs) + +plt.plot(time_linspace, means) +plt.fill_between(time_linspace, means - 2 * std_devs, means + 2 * std_devs, facecolor='green', alpha=0.3) +plt.fill_between(time_linspace, means - std_devs, means + std_devs, facecolor='green', alpha=0.5) +plt.title('KMP') +plt.scatter(X[:, 0], X[:, 1]) +plt.show() diff --git a/pyrobolearn/models/gaussian.py b/pyrobolearn/models/gaussian.py index aec534a..4f6cc33 100755 --- a/pyrobolearn/models/gaussian.py +++ b/pyrobolearn/models/gaussian.py @@ -153,7 +153,7 @@ class Gaussian(object): if isinstance(mean, (int, float)): mean = np.array([mean]) if mean is not None: - mean = np.array(mean) + mean = np.asarray(mean) self._mean = mean # alias @@ -170,7 +170,7 @@ class Gaussian(object): if cov is not None: if isinstance(cov, (int, float)): cov = np.array([[cov]]) - cov = np.array(cov) + cov = np.asarray(cov) if not self.is_symmetric(cov): raise ValueError("The given covariance matrix is not symmetric") if not self.is_psd(cov): @@ -598,10 +598,10 @@ class Gaussian(object): # aliases value, o, i = input_value, output_idx, input_idx - value = np.array([value]) if isinstance(value, (int, float)) else np.array(value) + value = np.array([value]) if isinstance(value, (int, float)) else np.asarray(value) if i is None: - o = np.array([o]) if isinstance(o, int) else np.array(o) + o = np.array([o]) if isinstance(o, int) else np.asarray(o) # from all the indices remove the output indices i = np.array(list(set(range(self.size)) - set(o))) i.sort() @@ -609,7 +609,7 @@ class Gaussian(object): # make sure that the input indices have the same length as the value ones i = i[:len(value)] else: - i = np.array([i]) if isinstance(i, int) else np.array(i) + i = np.array([i]) if isinstance(i, int) else np.asarray(i) assert len(i) == len(value), "The value array and the idx2 array have different lengths" # compute conditional diff --git a/pyrobolearn/models/gmm/gmm.py b/pyrobolearn/models/gmm/gmm.py index 0f1f2ef..5579f8d 100755 --- a/pyrobolearn/models/gmm/gmm.py +++ b/pyrobolearn/models/gmm/gmm.py @@ -505,7 +505,7 @@ class GMM(object): # compute individual joint distribution likelihoods = np.array([g.pdf(x) for g in self.gaussians]).T # shape: K if data vector, or NxK if matrix - priors = np.array([self.priors[z_id] for z_id in z_idx]) # shape: 1 if data vector, or N if matrix + priors = np.array([self.priors[z_id] for z_id in z_idx]) # shape: 1 if data vector, or N if matrix joints = priors * likelihoods[range(Nx), z_idx] # shape: N # return product of joint distributions @@ -949,7 +949,7 @@ class GMM(object): # if not first basis vector, compute orthogonal vector using Gram-Schmidt if i != 0: - d_init = np.array(d) + d_init = np.asarray(d) for basis in bases: # TODO: vectorize this d -= np.sum(basis * d_init, axis=1) * basis # (T, D-1) else: @@ -968,7 +968,7 @@ class GMM(object): chi = np.sum(basis * d, axis=1) / norm_first_deriv # (T,) generalized_curvatures.append(chi) - generalized_curvatures = np.array(generalized_curvatures) # (D-2, T) + generalized_curvatures = np.asarray(generalized_curvatures) # (D-2, T) # compute total curvature norm total_curvature_norm = np.linalg.norm(generalized_curvatures, axis=0) # (T,) @@ -1539,8 +1539,25 @@ class GMM(object): - [1] "Robot Programming by Demonstration: a Probabilistic Approach" (chap 2), Calinon, 2009 - [2] "A Tutorial on Task-Parameterized Movement Learning and Retrieval", Calinon, 2015 """ + # check input state + x_in = np.array([x_in]) if isinstance(x_in, (int, float)) else np.asarray(x_in) + + # check output state + idx_out = np.array([idx_out]) if isinstance(idx_out, int) else np.asarray(idx_out) + + # check the idx_in. If None, infer it from idx_out. + if idx_in is None: + # from all the indices remove the output indices + idx_in = np.array(list(set(range(self.size)) - set(idx_out))) + idx_in.sort() + + # make sure that the input indices have the same length as the value ones + idx_in = idx_in[:len(x_in)] + else: + idx_in = np.array([idx_in]) if isinstance(idx_in, int) else np.asarray(idx_in) + priors = self.responsibilities(x_in, dims=idx_in) - gaussians = [g.condition(x_in, idx_out, idx_in) for g in self.gaussians] + gaussians = [gaussian.condition(x_in, idx_out, idx_in) for gaussian in self.gaussians] return GMM(priors=priors, gaussians=gaussians) def marginalize(self, idx): @@ -1647,7 +1664,7 @@ class GMM(object): coefficients.append(coeff) gaussians.append(gaussian) - priors, coefficients = np.array(priors), np.array(coefficients) + priors, coefficients = np.asarray(priors), np.asarray(coefficients) normalization = np.sum(priors * coefficients) priors = priors * coefficients / normalization return GMM(priors=priors, gaussians=gaussians) @@ -1688,7 +1705,7 @@ class GMM(object): coefficients.append(coeff) gaussians.append(gaussian) - priors, coefficients = np.array(priors), np.array(coefficients) + priors, coefficients = np.asarray(priors), np.asarray(coefficients) normalization = np.sum(priors * coefficients) priors = priors * coefficients / normalization return GMM(priors=priors, gaussians=gaussians) @@ -1762,7 +1779,7 @@ class GMM(object): Returns: float: p(lower <= x <= upper) """ - probs = np.array([g.integrate(lower, upper) for g in self.gaussians]) + probs = np.asarray([g.integrate(lower, upper) for g in self.gaussians]) return np.sum(self.priors * probs) def grad(self, x, k=None, wrt='x'): @@ -1808,10 +1825,10 @@ class GMM(object): if wrt == 'x': return np.sum([prior * g.grad(x, wrt=wrt) for prior, g in zip(self.priors, self.gaussians)], axis=0) elif wrt == 'pi' or wrt == 'prior': - return np.array([gaussian.pdf(x) for gaussian in self.gaussians]) + return np.asarray([gaussian.pdf(x) for gaussian in self.gaussians]) elif wrt == 'mu' or wrt == 'mean' or wrt == 'sigma' or wrt[:3] == 'cov' \ or wrt == 'lambda' or wrt == 'precision': - return np.array([prior * g.grad(x, wrt=wrt) for prior, g in zip(self.priors, self.gaussians)]) + return np.asarray([prior * g.grad(x, wrt=wrt) for prior, g in zip(self.priors, self.gaussians)]) else: raise ValueError("The given 'wrt' argument is not valid (see documentation)") @@ -2067,7 +2084,7 @@ class TPGMM(GMM): ###################### -def plot_gmm(gmm, dims=(0, 1), X=None, label=True, ax=None, title='GMM', xlim=(-6, 6), ylim=(-6, 6), option=1, +def plot_gmm(gmm, dims=[0, 1], X=None, label=True, ax=None, title='GMM', xlim=(-6, 6), ylim=(-6, 6), option=1, color='b'): r"""Plot GMM""" # create ax if not already created @@ -2106,10 +2123,10 @@ def plot_gmr(time_linspace, means=None, std_devs=None, covariances=None, gaussia if gaussians is None: if means is None: raise ValueError("Expecting the means to be provided if a list of gaussians is not provided.") - if std_devs is None: - raise ValueError("Expecting the std_devs to be provided if a list of gaussians is not provided.") if covariances is None: raise ValueError("Expecting the covariances to be provided if a list of gaussians is not provided.") + if std_devs is None: + std_devs = [np.sqrt(np.diag(covariance)) for covariance in covariances] else: means, std_devs, covariances = [], [], [] for g in gaussians: diff --git a/pyrobolearn/models/kmp/kmp.py b/pyrobolearn/models/kmp/kmp.py index b810c33..af05754 100644 --- a/pyrobolearn/models/kmp/kmp.py +++ b/pyrobolearn/models/kmp/kmp.py @@ -17,9 +17,14 @@ import copy # from pyrobolearn.models.model import Model from pyrobolearn.models.gmm import GMM, Gaussian +# to check Python version (if sys.version_info[0] < 3, then python 2) +import sys +if sys.version_info[0] < 3: # Python 2 + input = raw_input # redefine input to be raw_input + __author__ = "Brian Delhaisse" -__copyright__ = "Copyright 2018, PyRoboLearn" +__copyright__ = "Copyright 2019, PyRoboLearn" __credits__ = ["Yanlong Huang (paper + Matlab)", "Brian Delhaisse (Python)"] __license__ = "GNU GPLv3" __version__ = "1.0.0" @@ -79,27 +84,39 @@ class KMP(object): Kernelized Movement Primitives allows to encode a movement/trajectory using kernels. The use of kernels makes it practical for high-dimensional inputs. - KMP is a non-parametric (but a semi-parametric approach is used to initialize it) probabilistic discriminative - model. The performance of the KMP depends thus on the underlying probabilistic distribution from which it learns - from (which is often a GMM, and thus it depends on the GMM initialization). + KMP is a non-parametric (but a semi-parametric approach is often used to initialize it) probabilistic + discriminative model. The performance of the KMP depends thus on the underlying probabilistic distribution from + which it learns from (which is often a GMM, and thus it depends on the GMM initialization). + + KMP is well-suited for via-point, end-point, extrapolation and high-dimensional inputs problems. However, note that + it does not scale well with high-dimensional outputs or long trajectories. The reason is because of the kernel + matrix which has a shape of :math:`K \in \mathbb{R}^{TO \times TO}` where :math:`O` is the output dimension, and + :math:`T` is the length of the reference trajectory (i.e. number of data points sampled from that trajectory). References: - [1] "Kernelized Movement Primitives", Huang et al., 2017 """ def __init__(self, kernel_fct=None, database=None): - """ + r""" Initialize the KMP. Args: kernel_fct (None, callable): kernel function. If None, it will use the `RBF` kernel with a variance of 1, and a length scale of 2. - database (None, list): initial database. + database (None, list): initial reference database. The database should be a list where each item is a + tuple of 3 elements (the input state, the mean, and the covariance). That is, the reference database + is given by: :math:`\[s_t, \hat{\mu}_t, \hat{\Sigma}_t \]_{t=1}^T` where :math:`T` is the length of + a reference trajectory, :math:`s_t` is the input (vector/scalar) state, :math:`\hat{\mu}_t` is the + reference mean, and :math:`\hat{\Sigma}_t` is the reference covariance matrix. By reference, we mean + that after training your probabilistic discriminative model on multiple trajectories, you provide the + predicted mean and covariance given the input state. """ super(KMP, self).__init__() - self._input_dim = 0 - self._output_dim = 0 + self._input_dim = 0 # input dimension + self._output_dim = 0 # output dimension + self.N = 0 # number of data points # set kernel fct self.K = kernel_fct if kernel_fct is not None else RBF(variance=1., lengthscale=2.) @@ -110,9 +127,14 @@ class KMP(object): else: self._database = database + # mean + self.mu = None # mean used for the mean prediction + # Inverse Kernel matrix (useful when computing the prediction) - self.K_inv = None - self.prior_reg = 1. + self.lambda1 = 1. # lambda in the paper for the mean prediction (in Huang's code, it is often set to 1) + self.lambda2 = 60. # lambda in the paper for the covariance prediction (in Huang's code, it is set to 60) + self.K_inv1 = None # inverse kernel matrix for the mean + self.K_inv2 = None # inverse kernel matrix for the covariance # translation vector and rotation matrix self.bias = 0 @@ -152,27 +174,74 @@ class KMP(object): """Return the rotation matrix applied to the predicted mean and covariance by the KMP""" return self.rot + @property + def lambda1(self): + """Return the prior regularization term for the mean.""" + return self._l1 + + @lambda1.setter + def lambda1(self, value): + """Set the prior regularization term for the mean.""" + if not isinstance(value, (int, float)): + raise TypeError("Expecting the prior regularization term for the mean to be a scalar (int, float), but " + "got instead: {}".format(type(value))) + if value <= 0.: + raise ValueError("The prior regularization term needs to be strictly bigger than 0.") + self._l1 = value + + # aliases + prior_mean_regularization = lambda1 + mean_regularization = lambda1 + mean_reg = lambda1 + + @property + def lambda2(self): + """Return the prior regularization term for the covariance.""" + return self._l2 + + @lambda2.setter + def lambda2(self, value): + """Set the prior regularization term for the covariance.""" + if not isinstance(value, (int, float)): + raise TypeError("Expecting the prior regularization term for the covariance to be a scalar (int, float), " + "but got instead: {}".format(type(value))) + if value <= 0.: + raise ValueError("The prior regularization term needs to be strictly bigger than 0.") + self._l2 = value + + # aliases + prior_covariance_regularization = lambda2 + covariance_regularization = lambda2 + cov_reg = lambda2 + ################## # Static Methods # ################## @staticmethod - def copy(other): - """Copy the other KMP""" + def copy(other): # TODO: use deepcopy instead... + """Copy the other KMP.""" + if not isinstance(other, KMP): + raise TypeError("Expecting the other element to be an instance of `KMP`, but got instead: " + "{}".format(type(other))) kmp = KMP(kernel_fct=other.kernel_fct) kmp._database = copy.deepcopy(other.database) kmp.bias = other.bias kmp.rot = other.rot - kmp.K_inv = np.copy(other.K_inv) + kmp.lambda1 = other.lambda1 + kmp.lambda2 = other.lambda2 + kmp.K_inv1 = np.copy(other.K_inv1) + kmp.K_inv2 = np.copy(other.K_inv2) + return kmp @staticmethod def is_parametric(): - """The KMP is a non-parametric model which uses a kernel""" + """The KMP is a non-parametric model which uses a kernel.""" return False @staticmethod def is_linear(): - """The KMP has no parameters, and thus has no linear parameters""" + """The KMP has no parameters, and thus has no linear parameters.""" return False @staticmethod @@ -198,17 +267,25 @@ class KMP(object): return False @staticmethod - def create_reference_database(X, Y, gmm=None, gmm_num_components=10, dist=None, database_threshold=1e-3, + def create_reference_database(X, Y, gmm=None, gmm_num_components=10, distance=None, database_threshold=1e-3, database_size_limit=100, sample_from_gmm=False, gmm_init='kmeans', gmm_reg=1e-8, - gmm_num_iters=1000, gmm_convergence_threshold=1e-4, seed=None, verbose=True, + gmm_num_iters=1000, gmm_convergence_threshold=1e-4, seed=None, verbose=False, block=True): r""" Create reference database from the data. This database contains a list of input data with their corresponding predicted output distribution by the reference model (which in this case is a GMM). - X (np.array[N,T,I], list of np.array[T,I]): input data matrix of shape NxTxI, where N is the number of + That is from several trajectories :math:`\{ \{ s_{t,n}, \xi_{t,n} \}_{t=1}^T_n \}_{n=1}^N`, it computes the + reference database :math:`\{ s_t, \hat{\mu}_t, \hat{\Sigma}_t \}_{t=1}^T`, where :math:`N` is the number of + trajectories, :math:`T_n` is the length of the trajectory :math:`n`, :math:`s` is the input, :math:`\xi` is + the output, :math:`\hat{\mu}_t` is the reference mean, and :math:`\hat{\Sigma}_t` is the reference covariance + matrix. By reference, we mean that after training the probabilistic discriminative model on multiple + trajectories, the predicted mean and covariance given each input become the references. + + Args: + X (np.array[N,T,I], list[np.array[T,I]]): input data matrix of shape NxTxI, where N is the number of trajectories, T is its length, and I is the input data dimension. - Y (np.array[N,T,O], list of np.array[T,O]): corresponding output data matrix of shape NxTxO, where N is + Y (np.array[N,T,O], list[np.array[T,O]]): corresponding output data matrix of shape NxTxO, where N is the number of trajectories, T is its length, and O is the output data dimension. gmm (None, GMM): the reference generative model. If None, it will create a GMM. gmm_num_components (int): the number of components for the underlying reference GMM. @@ -237,7 +314,7 @@ class KMP(object): # TODO: replace gmm by joint generative model # check given arguments - X, Y = np.array(X), np.array(Y) + X, Y = np.asarray(X), np.asarray(Y) if X.shape[:2] != Y.shape[:2]: if X.shape[0] != Y.shape[0]: raise ValueError("The number of trajectories are different between the input and output data") @@ -257,10 +334,16 @@ class KMP(object): data = np.dstack((X, Y)) # shape: NxTxD data = data.reshape(-1, I + O) # shape: NTxD + if verbose: + print("Training the GMM...") + # train gmm gmm.fit(data, reg=gmm_reg, num_iters=gmm_num_iters, threshold=gmm_convergence_threshold, init=gmm_init, seed=seed, verbose=verbose) + if verbose: + print("GMM trained") + # create reference database database = [] @@ -271,8 +354,8 @@ class KMP(object): # use the distance function to check if we should add the input data into the database else: # define distance function - if dist is None: - def dist(x1, x2): + if distance is None: + def distance(x1, x2): return np.linalg.norm(x1 - x2) # check inputs to put in the reference database (time complexity: O((NT)^2)) @@ -281,7 +364,7 @@ class KMP(object): # compare current input with previous inputs, and add in database if unique enough can_add = True for x_prev in database: - if dist(x_curr, x_prev) < database_threshold: + if distance(x_curr, x_prev) < database_threshold: can_add = False break if can_add: @@ -289,16 +372,65 @@ class KMP(object): # if the size of the database is bigger than database size limit, sample uniformly from it if len(database) > database_size_limit: - idx = np.random.choice(range(len(database)), size=database_size_limit, replace=False) + idx = np.random.choice(list(range(len(database))), size=database_size_limit, replace=False) database = database[idx] + if verbose: + print("Creating database...") + # update database to also contain prediction from GMR - database = [(x, (gmm.condition(x, idx_out=range(I, O))).approximate_by_single_gaussian()) + database = [(x, gmm.condition(x, idx_out=list(range(I, I+O)), + idx_in=list(range(I))).approximate_by_single_gaussian()) for x in database] + if verbose: + print("Database created...") + # return constructed reference database return database + @staticmethod + def get_reference_database(x, means=None, covariances=None, gaussians=None): + """ + Get reference database from the state inputs, means and covariances (gaussians). + + Args: + x (np.array[float[T,I]]): input data matrix of shape TxI, where T is the length of a trajectory, and I is + the input data dimension. + means (np.array[float[T,O]]): list of means. + covariances (np.array[float[T,O,O]]): list of covariances. + gaussians (list[Gaussian]): list of Gaussian. + + Returns: + list[(np.ndarray, Gaussian)]: database which is a list of tuples where each one contains an input data + array and the corresponding predicted output Gaussian (by GMR) + """ + if gaussians is None: + if means is None: + raise ValueError("If the gaussians are not provided, the means are required.") + if covariances is None: + raise ValueError("If the gaussians are not provided, the covariances are required.") + gaussians = [Gaussian(mean=mean, covariance=covariance) for mean, covariance in zip(means, covariances)] + database = [(xi, gaussian) for xi, gaussian in zip(x, gaussians)] + return database + + def set_reference_database(self, x, means=None, covariances=None, gaussians=None): + """ + Set reference database from the state inputs, means and covariances (gaussians). + + Args: + x (np.array[float[T,I]]): input data matrix of shape TxI, where T is the length of a trajectory, and I is + the input data dimension. + means (np.array[float[T,O]]): list of means. + covariances (np.array[float[T,O,O]]): list of covariances. + gaussians (list[Gaussian]): list of Gaussian. + + Returns: + list[(np.ndarray, Gaussian)]: database which is a list of tuples where each one contains an input data + array and the corresponding predicted output Gaussian (by GMR) + """ + self._database = self.get_reference_database(x, means=means, covariances=covariances, gaussians=gaussians) + @staticmethod def combine(x, kmps, frames): r""" @@ -309,7 +441,7 @@ class KMP(object): than coordinates. For instance, it does not work if the inputs are images or sensor values. Args: - x (np.array[I], np.array[N,I]): new input data vector or matrix + x (np.array[float[I]], np.array[float[N,I]]): new input data vector or matrix kmps (KMP, list of KMP): list of local KMPs frames (tuple, list of tuples): list of tuples where each tuple contains a rotation matrix and a bias translation vector @@ -345,9 +477,9 @@ class KMP(object): # Methods # ########### - def fit(self, X, Y, gmm=None, gmm_num_components=10, prior_reg=1., dist=None, database_threshold=1e-3, - database_size_limit=100, sample_from_gmm=False, gmm_init='kmeans', gmm_reg=1e-8, gmm_num_iters=1000, - gmm_convergence_threshold=1e-4, seed=None, verbose=True, block=True): + def fit(self, X, Y, gmm=None, gmm_num_components=10, mean_reg=1., covariance_reg=1., distance=None, + database_threshold=1e-3, database_size_limit=100, sample_from_gmm=False, gmm_init='kmeans', gmm_reg=1e-8, + gmm_num_iters=1000, gmm_convergence_threshold=1e-4, seed=None, verbose=False, block=True): r""" Fit the given data composed of inputs and outputs. @@ -368,18 +500,18 @@ class KMP(object): .. math:: \mathcal{L}(\mu_w, \Sigma_w) = \sum_{n=1}^N KL[p(y|x_n;\theta) || p_{ref}(y | x_n)] - + \tau ( (\mu_w^T\mu_w) + tr(\Sigma_w) ) + + \lambda ( (\mu_w^T\mu_w) + tr(\Sigma_w) ) where :math:`\theta = \{\mu_w, \Sigma_w\}` are the parameters that are being optimized, :math:`p_{ref}(y | x_n) = \mathcal{N}(\mu_n, \Sigma_n)` is the predicted reference distribution - (e.g. Gaussian by GMR), and :math:`\tau` is the prior regularization term. + (e.g. Gaussian by GMR), and :math:`\lambda` is the prior regularization term. Once the parametric model has been optimized, the optimal mean and covariance of the weights are given by: .. math:: - \mu_w = \Omega (\Omega^T \Omega + \tau \Sigma)^{-1} \mu - \Sigma_w = N (\Omega \Sigma \Omega^T + \tau I)^{-1} + \mu_w = \Omega (\Omega^T \Omega + \lambda \Sigma)^{-1} \mu + \Sigma_w = N (\Omega \Sigma \Omega^T + \lambda I)^{-1} where :math:`\Omega = [\Phi(x_1) ... \Phi(x_N)] \in \mathbb{R}^{BO \times NO}`, :math:`\Sigma = blockdiag(\Sigma_1, ..., \Sigma_N) \in \mathbb{R}^{NO \times NO}`, and @@ -389,15 +521,15 @@ class KMP(object): .. math:: - \mu_y &= \Phi(x^*)^T \mu_w = \Phi(x^*) \Omega (\Omega^T \Omega + \tau \Sigma)^{-1} \mu \\ - \Sigma_y &= \Phi(x^*)^T \Sigma_w \Phi(x^*) = N \Phi(x^*)^T (\Omega\Sigma\Omega^T+\tau I)^{-1} \Phi(x^*) + \mu_y &= \Phi(x^*)^T \mu_w = \Phi(x^*) \Omega (\Omega^T \Omega + \lambda \Sigma)^{-1} \mu \\ + \Sigma_y &= \Phi(x^*)^T \Sigma_w \Phi(x^*) = N \Phi(x^*)^T (\Omega\Sigma\Omega^T+\lambda I)^{-1} \Phi(x^*) And by using the kernel trick (and the Woodbury identity for the covariance), this resumes to: .. math:: - \mu_y &= k^* (K + \tau \Sigma)^{-1} \mu \\ - \Sigma_y &= \frac{N}{\tau} (k(x^*, x^*) - k^* (K + \tau \Sigma)^{-1} k^*^T) + \mu_y &= k^* (K + \lambda \Sigma)^{-1} \mu \\ + \Sigma_y &= \frac{N}{\lambda} (k(x^*, x^*) - k^* (K + \lambda \Sigma)^{-1} k^*^T) where :math:`K(X,X) \in \mathbb{R}^{NO \times NO}` is the kernel matrix, :math:`k^* = [k(x^*, x_1) ... k(x^*,x_N)] \in \mathbb{R}^{O \times NO}` is the kernel evaluated @@ -405,14 +537,17 @@ class KMP(object): `I_O \in \mathbb{O \times O}` and :math:`\hat{k}(x_i, x_j)` the kernel function. Args: - X (np.array[N,T,I], list of np.array[T,I]): input data matrix of shape NxTxI, where N is the number of + X (np.array[N,T,I], list[np.array[T,I]]): input data matrix of shape NxTxI, where N is the number of trajectories, T is its length, and I is the input data dimension. - Y (np.array[N,T,O], list of np.array[T,O]): corresponding output data matrix of shape NxTxO, where N is + Y (np.array[N,T,O], list[np.array[T,O]]): corresponding output data matrix of shape NxTxO, where N is the number of trajectories, T is its length, and O is the output data dimension. gmm (None, GMM): the reference generative model. If None, it will create a GMM. gmm_num_components (int): the number of components for the underlying reference GMM. - prior_reg (float): prior regularization term - dist (callable, None): callable function which accepts two data points from X, and compute the distance + mean_reg (float): prior regularization term for the mean that is multiplied by the covariance in the KMP + (see lambda symbol in the paper [1]). + covariance_reg (float): prior regularization term for the covariance that is multiplied by the covariance + in the KMP (see lambda symbol in the paper [2]). + distance (callable, None): callable function which accepts two data points from X, and compute the distance between them. If None and `sample_from_gmm` is False, it will use the 2-norm. database_threshold (float): threshold associated with the `distance` argument above. If the distance between a new data point and data point in the database is below the threshold, it will be added to @@ -437,7 +572,7 @@ class KMP(object): # create reference database self._database = self.create_reference_database(X, Y, gmm=gmm, gmm_num_components=gmm_num_components, - dist=dist, database_threshold=database_threshold, + distance=distance, database_threshold=database_threshold, database_size_limit=database_size_limit, sample_from_gmm=sample_from_gmm, gmm_init=gmm_init, gmm_reg=gmm_reg, gmm_num_iters=gmm_num_iters, @@ -445,23 +580,29 @@ class KMP(object): seed=seed, verbose=verbose, block=block) # compute kernel inverse from database - K, K_inv = self.learn_from_database(self._database, prior_reg=prior_reg, verbose=verbose, block=block) - self.K_inv = K_inv # shape: NOxNO + K, K_inv1, K_inv2 = self.learn_from_database(self._database, mean_reg=mean_reg, covariance_reg=covariance_reg, + verbose=verbose, block=block) + + self.K_inv1 = K_inv1 # shape: NOxNO + self.K_inv2 = K_inv2 # shape: NOxNO # aliases learn = fit imitate = fit - def learn_from_database(self, database=None, prior_reg=1., verbose=True, block=True): + def learn_from_database(self, database=None, mean_reg=1., covariance_reg=1., verbose=False, block=True): r""" Learn the Kernel matrix from the database. Specifically, it computes :math:`K` and - :math:`(K + \tau \Sigma)^{-1}`. The latter is because this is used for the prediction part; for the predicted - mean and covariance, and is better to compute it during the learning phase than the prediction phase. + :math:`(K + \lambda \Sigma)^{-1}`. The latter is because this is used for the prediction part; for the + predicted mean and covariance, and is better to compute it during the learning phase than the prediction phase. Args: database (list of tuples): list of tuples which contain the input data array and the associated predicted output distribution by the reference model. - prior_reg (float): prior regularization term + mean_reg (float): prior regularization term for the mean that is multiplied by the covariance in the KMP + (see lambda symbol in the paper [1]). + covariance_reg (float): prior regularization term for the covariance that is multiplied by the covariance + in the KMP (see lambda symbol in the paper [2]). verbose (bool): if we should print details during the optimization process block (bool): if the size of the kernel matrix is bigger than 1000, it will ask for confirmation to continue. The kernel matrix has to be inversed, which has a time complexity of `O(N^3)` where @@ -469,15 +610,14 @@ class KMP(object): Returns: np.array[NO,NO]: Kernel matrix :math:`K` - np.array[NO,NO]: Inverse Kernel matrix :math:`(K + \tau \Sigma)^-1` + np.array[NO,NO]: Inverse Kernel matrix :math:`(K + \lambda_1 \Sigma)^-1` for mean prediction. + np.array[NO,NO]: Inverse Kernel matrix :math:`(K + \lambda_2 \Sigma)^-1` for covariance prediction. """ # Quick checks if database is None: database = self.database if len(database) == 0: raise ValueError("There are no elements in the database") - if prior_reg <= 0: - raise ValueError("The prior regularization term needs to be strictly bigger than 0") # output dimension and size of database output_dim = database[0][1].size @@ -488,24 +628,36 @@ class KMP(object): print("Warning: trying to inverse a {} by {} 2D matrix... This could be computationally " "expensive...".format(N * output_dim, N * output_dim)) if block: - raw_input("Please press enter to continue with the inversion of the matrix. Ctrl+C to stop " - "the program") + input("Please press enter to continue with the inversion of the matrix. Ctrl+C to stop " + "the program") + + # remember variables for prediction + self.N, self.lambda1, self.lambda2 = len(database), mean_reg, covariance_reg + self._output_dim = output_dim + self._input_dim = database[0][0].size # compute mean, covariance, and kernel from database - self.mu = np.array([gaussian.mean for _, gaussian in self.database]).reshape(-1, 1) # shape: NO x 1 + self.mu = np.array([gaussian.mean for _, gaussian in self.database]).reshape(-1) # shape: NO x 1 cov = block_diag(*[gaussian.cov for _, gaussian in self.database]) # shape: NOxNO I_O = np.identity(output_dim) # shape: OxO - K = np.array([[self.K(xi, xj) * I_O for xj, _ in self.database] + K = np.vstack([np.hstack([self.K(xi, xj) * I_O for xj, _ in self.database]) for xi, _ in self.database]) # shape: NOxNO # compute kernel inverse - K_inv = np.linalg.inv(K + prior_reg * cov) # shape: NOxNO + if verbose: + print("Mean shape: {}".format(self.mu.shape)) + print("Covariance shape: {}".format(cov.shape)) + print("I_O shape: {}".format(I_O.shape)) + print("Inversing the kernel matrices with shape: {}".format(K.shape)) - # remember variables for prediction - self.N, self.prior_reg = len(database), prior_reg + K_inv1 = np.linalg.inv(K + mean_reg * cov) # shape: NOxNO + K_inv2 = K_inv1 if mean_reg == covariance_reg else np.linalg.inv(K + covariance_reg * cov) # shape: NOxNO + + if verbose: + print("The kernel matrices have been inversed...") # return kernel and kernel inverse - return K, K_inv + return K, K_inv1, K_inv2 def loss(self): r""" @@ -515,8 +667,8 @@ class KMP(object): \mathcal{L} = \sum_{n=1}^N KL[\mathcal{N}(\mu_n^*, \Sigma_n^*) || \mathcal{N}_{ref}(\mu_n, \Sigma_n)] - where :math:`\mu_n^* = k^* (K + \tau \Sigma)^{-1} \mu` and - :math:`\Sigma_n^* = \frac{N}{\tau} (k(x^*, x^*) - k^* (K + \tau \Sigma)^{-1} k^*^T)` are the predicted + where :math:`\mu_n^* = k^* (K + \lambda \Sigma)^{-1} \mu` and + :math:`\Sigma_n^* = \frac{N}{\lambda} (k(x^*, x^*) - k^* (K + \lambda \Sigma)^{-1} k^*^T)` are the predicted mean and covariance by the KMP, and :math:`\mu_n` and :math:`\Sigma_n` are the predicted mean and covariance by GMR. @@ -552,8 +704,9 @@ class KMP(object): """ # compute k vector (which compares given input data with previous ones) I = np.identity(self.output_dim) - k = np.array([self.K(x, x_prev) * I for x_prev, _ in self.database]) # shape: NxOxO - k = k.reshape(-1, 1).T # shape: OxNO + # k = np.array([self.K(x, x_prev) * I for x_prev, _ in self.database]) # shape: NxOxO + # k = k.reshape(-1, 1).T # shape: OxNO + k = np.hstack([self.K(x, x_prev) * I for x_prev, _ in self.database]) # shape: OxNO return k def predict(self, x): @@ -562,7 +715,7 @@ class KMP(object): .. math:: - \mu_y(x^*) &= k^* (K + \tau \Sigma)^{-1} \mu \\ + \mu_y(x^*) &= k^* (K + \lambda \Sigma)^{-1} \mu \\ where :math:`K(X,X) \in \mathbb{R}^{NO \times NO}` is the kernel matrix, :math:`k^* = [k(x^*, x_1) ... k(x^*,x_N)] \in \mathbb{R}^{O \times NO}` is the kernel evaluated @@ -586,7 +739,7 @@ class KMP(object): k = self._compute_k(xi) # return mean - mean = k.dot(self.K_inv).dot(self.mu) + mean = k.dot(self.K_inv1).dot(self.mu) means.append(mean) # return the same shape as input @@ -603,8 +756,8 @@ class KMP(object): .. math:: - \mu_y(x^*) &= k^* (K + \tau \Sigma)^{-1} \mu \\ - \Sigma_y(x^*) &= \frac{N}{\tau} (k(x^*, x^*) - k^* (K + \tau \Sigma)^{-1} k^*^T) + \mu_y(x^*) &= k^* (K + \lambda \Sigma)^{-1} \mu \\ + \Sigma_y(x^*) &= \frac{N}{\lambda} (k(x^*, x^*) - k^* (K + \lambda \Sigma)^{-1} k^*^T) where :math:`K(X,X) \in \mathbb{R}^{NO \times NO}` is the kernel matrix, :math:`k^* = [k(x^*, x_1) ... k(x^*,x_N)] \in \mathbb{R}^{O \times NO}` is the kernel evaluated @@ -613,6 +766,8 @@ class KMP(object): Args: x (np.array[I], np.array[N,I]): input data vector or matrix + return_gaussian (bool): if True, it will return a list of Gaussians. Otherwise, it will return the means + and covariances. Returns: if return_gaussian: @@ -623,12 +778,14 @@ class KMP(object): """ # if only one sample only_one_sample = False + if isinstance(x, (int, float)): + x = np.array([x]) if len(x.shape) == 1: only_one_sample = True x = [x] # useful variables - coeff = self.N / self.prior_reg + coeff = self.N / self.cov_reg I = np.identity(self.output_dim) # compute predicted mean(s) and covariance(s) @@ -639,8 +796,8 @@ class KMP(object): k_input = self.K(xi, xi) * I # compute mean and covariance - mean = k.dot(self.K_inv).dot(self.mu) - cov = coeff * (k_input - k.dot(self.K_inv).dot(k.T)) + mean = k.dot(self.K_inv1).dot(self.mu) + cov = coeff * (k_input - k.dot(self.K_inv2).dot(k.T)) means.append(mean) covs.append(cov) @@ -659,8 +816,8 @@ class KMP(object): # else, return mean(s) and covariance(s) return means, covs - def modulate(self, x, y_mean, y_cov, dist=None, threshold=1, update_database=False, prior_reg=1., - verbose=True, block=True): + def modulate(self, x, y_mean, y_cov, distance=None, threshold=1, update_database=False, mean_reg=1., + covariance_reg=1., verbose=True, block=True): r""" Modulate the prediction given new data point with their associated covariances. @@ -673,14 +830,17 @@ class KMP(object): y_mean (np.array[O], np.array[N,O]): mean of new data point(s) y_cov (np.array[O,O], np.array[N,O,O]): covariance of new data point(s). A small covariance means the user wants a high precision around the new data point. - dist (callable, None): callable function which accepts two data points from X, and compute the distance + distance (callable, None): callable function which accepts two data points from X, and compute the distance between them. If None and `sample_from_gmm` is False, it will use the 2-norm. threshold (float): threshold associated with the `distance` argument above. If the distance between a new data point and data point in the database is below the threshold, it will be added to the database. update_database (bool): If True, it will modify permanently the original database by including the new given points. - prior_reg (float): prior regularization term + mean_reg (float): prior regularization term for the mean that is multiplied by the covariance in the KMP + (see lambda symbol in the paper [1]). + covariance_reg (float): prior regularization term for the covariance that is multiplied by the covariance + in the KMP (see lambda symbol in the paper [2]). verbose (bool): if we should print details during the optimization process block (bool): if the size of the kernel matrix is bigger than 1000, it will ask for confirmation to continue. The kernel matrix has to be inversed, which has a time complexity of `O(N^3)` where @@ -701,8 +861,8 @@ class KMP(object): raise ValueError("The number of means and covariances for the output data points doesn't match") # define distance function - if dist is None: - def dist(x1, x2): + if distance is None: + def distance(x1, x2): return np.linalg.norm(x1 - x2) # copy database @@ -713,15 +873,15 @@ class KMP(object): # check the closest input inside the reference database (time complexity: O((NT)^2)) idx_closest = 0 x_closest = database[idx_closest][0] - dist_closest = dist(x_closest, xi) + dist_closest = distance(x_closest, xi) for idx, (x_curr, _) in enumerate(database): # if the current distance between the new point and the current point is smaller than the previous # closest one, update the closest point - dist_curr = dist(x_curr, xi) + dist_curr = distance(x_curr, xi) if dist_curr < dist_closest: idx_closest = idx x_closest = x_curr - dist_closest = dist(x_closest, xi) + dist_closest = distance(x_closest, xi) # check with the threshold if the closest point should be replaced by the new input data point, # or if the new point should just be appended in the database @@ -732,8 +892,10 @@ class KMP(object): database.append((xi, gaussian)) # compute kernel inverse from the extended database - K, K_inv = self.learn_from_database(database, prior_reg=prior_reg, verbose=verbose, block=block) - self.K_inv = K_inv # shape: NOxNO + K, K_inv1, K_inv2 = self.learn_from_database(database, mean_reg=mean_reg, covariance_reg=covariance_reg, + verbose=verbose, block=block) + self.K_inv1 = K_inv1 # shape: NOxNO + self.K_inv2 = K_inv2 # shape: NOxNO if update_database: self._database = database @@ -744,7 +906,8 @@ class KMP(object): # alias add_via_points = modulate - def superpose(self, databases, priorities, update_database=False, prior_reg=1., verbose=True, block=True): + def superpose(self, databases, priorities, update_database=False, mean_reg=1., covariance_reg=1., + verbose=True, block=True): r""" Superpose different trajectories based on priorities. @@ -767,7 +930,10 @@ class KMP(object): priorities(np.array[L,N]]): list of priorities (float) for each point in each database. update_database (bool): If True, it will modify permanently the original database by including the new given points. - prior_reg (float): prior regularization term + mean_reg (float): prior regularization term for the mean that is multiplied by the covariance in the KMP + (see lambda symbol in the paper [1]). + covariance_reg (float): prior regularization term for the covariance that is multiplied by the covariance + in the KMP (see lambda symbol in the paper [2]). verbose (bool): if we should print details during the optimization process block (bool): if the size of the kernel matrix is bigger than 1000, it will ask for confirmation to continue. The kernel matrix has to be inversed, which has a time complexity of `O(N^3)` where @@ -783,7 +949,7 @@ class KMP(object): L, N = len(databases), len(databases[0]) databases = np.array(databases) # shape: LxNx2 priorities = np.array(priorities) # shape: LxN - if priorities.shape != (L,N): + if priorities.shape != (L, N): raise ValueError("Expecting the priorities to be of shape (L,N) where L is the number of databases, " "and N is the number of elements in these databases.") if not np.allclose(np.sum(priorities, axis=0), np.ones(L)): @@ -793,13 +959,13 @@ class KMP(object): # create mixed reference database mixed_database = [] for i in range(N): - priority = priorities[:,i] # shape: L - database = databases[:,i,1] # shape: L - x_input = databases[0,i,0] + priority = priorities[:, i] # shape: L + database = databases[:, i, 1] # shape: L + x_input = databases[0, i, 0] # quick check if similar input for each database - for j in range(1,L): - if np.allclose(databases[j-1,i,0], databases[j,i,0]): + for j in range(1, L): + if np.allclose(databases[j-1, i, 0], databases[j, i, 0]): raise ValueError("The element {} in the database {} and {} are different input " "arrays".format(i, j-1, j)) @@ -814,8 +980,10 @@ class KMP(object): mixed_database.append((x_input, gaussians)) # compute kernel inverse from the extended database - K, K_inv = self.learn_from_database(mixed_database, prior_reg=prior_reg, verbose=verbose, block=block) - self.K_inv = K_inv # shape: NOxNO + K, K_inv1, K_inv2 = self.learn_from_database(mixed_database, mean_reg=mean_reg, covariance_reg=covariance_reg, + verbose=verbose, block=block) + self.K_inv1 = K_inv1 # shape: NOxNO + self.K_inv2 = K_inv2 # shape: NOxNO if update_database: self._database = mixed_database @@ -909,8 +1077,8 @@ class KMP(object): .. math:: - \mu_y(x^*) &= k^* (K + \tau \Sigma)^{-1} \mu \\ - \Sigma_y(x^*) &= \frac{T}{\tau} (k(x^*, x^*) - k^* (K + \tau \Sigma)^{-1} k^*^T) + \mu_y(x^*) &= k^* (K + \lambda \Sigma)^{-1} \mu \\ + \Sigma_y(x^*) &= \frac{T}{\lambda} (k(x^*, x^*) - k^* (K + \lambda \Sigma)^{-1} k^*^T) where :math:`K(X,X) \in \mathbb{R}^{NO \times NO}` is the kernel matrix, :math:`k^* = [k(x^*, x_1) ... k(x^*,x_N)] \in \mathbb{R}^{O \times NO}` is the kernel evaluated diff --git a/pyrobolearn/models/kmp/quaternion_kmp.py b/pyrobolearn/models/kmp/quaternion_kmp.py new file mode 100644 index 0000000..9d4f219 --- /dev/null +++ b/pyrobolearn/models/kmp/quaternion_kmp.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""Define the quaternion kernelized movement primitive class. + +This file provides the Quaternion-KMP model [1]. + +References: + - [1] "Generalized Orientation Learning in Robot Task Space", Huang et al., 2019 + - [2] "Kernelized Movement Primitives", Huang et al., 2017 + - [3] https://github.com/yanlongtu/robInfLib +""" + +import numpy as np + +from pyrobolearn.models.kmp import KMP + +# import the quaternion transformation mapping for Quaternion-KMP +from pyrobolearn.utils.transformation import logarithm_map, exponential_map, get_quaternion_product + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Yanlong Huang (paper + Matlab)", "Brian Delhaisse (Python)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class QuaternionKMP(KMP): + r"""Quaternion KMP + + The Quaternion KMP consists first to project the quaternion data :math:`q_i \in \mathbb{S}^3; \forall i`, where + :math:`\mathbb{S}^3` is a unit sphere in :math:`\mathbb{R}^4`, into :math:`\mathbb{R}^3` using the logarithm map + :math:`\log: \mathbb{S}^3 \rightarrow \mathbb{R}^3`. Then, the KMP is trained on the projected data and the + predicted data is reprojected onto :math:`\mathbb{S}^3` using the exponential map :math:`\exp: \mathbb{R}^3 + \rightarrow \mathbb{S}^3`. + + Note that "the logarithmic map defined on :math:`\mathbb{S}^3` has no discontinuity boundary, just a singularity + at a single quaternion :math:`\bm{q} = -1 + [0,0,0]^\top = (-1, [0,0,0]^\top)`." [2] + + Note also that the KMP is initialized here using a Gaussian mixture model [3]. + + Warnings: The output of this model is expected to be the concatenation of orientations (expressed as quaternions + [x,y,z,w]) and their angular velocities. + + References: + - [1] "Generalized Orientation Learning in Robot Task Space", Huang et al., 2019 + - [2] "Orientation in Cartesian Space Dynamic Movement Primitives", Ude et al., 2014 + - [3] "Kernelized Movement Primitives", Huang et al., 2017 + """ + + def __init__(self, kernel_fct=None): + """ + Initialize the Quaternion KMP. + + Args: + kernel_fct (None, callable): kernel function. If None, it will use the `RBF` kernel with a variance + of 1, and a length scale of 2. + """ + super(QuaternionKMP, self).__init__(kernel_fct=kernel_fct) + + # define the auxiliary quaternion + self.auxiliary_quaternion = np.array([0., 0., 0., 1.]) # (x,y,z,w) + + def fit(self, X, Y, gmm=None, gmm_num_components=10, prior_reg=1., dist=None, database_threshold=1e-3, + database_size_limit=100, sample_from_gmm=False, gmm_init='kmeans', gmm_reg=1e-8, gmm_num_iters=1000, + gmm_convergence_threshold=1e-4, seed=None, verbose=True, block=True): + r""" + Fit the given data composed of inputs and outputs. + + This works by minimizing the KL-divergence between a parametric probabilistic discriminative model + and the predicted output distribution of a reference probabilistic model. First, the reference model (e.g. + a Gaussian mixture model) is trained on the given data (i.e. inputs :math:`x \in \mathbb{R}^{I} and outputs + :math:`y \in \mathbb{R}^{O}`). A reference database is then constructed containing `N` data inputs with the + corresponding output Gaussian distribution resulting from GMR given the data inputs. + + Then, a parametric model is given by :math:`y(x) = \Phi(x)^T w` where a Gaussian distribution is put on the + weights :math:`w \in \mathbb{R}^{BO}` such that :math:`w \sim \mathcal{N}(\mu_w, \Sigma_w)`, and thus + :math:`y(x) \sim \mathcal{N}(\Phi(x)^T \mu_w, \Phi(x)^T \Sigma_w \Phi(x))`. The matrix + :math:`\Phi(x) \in \mathbb{R}^{BO \times O}` is a block diagonal matrix containing basis functions + on its diagonal. + + The loss that is being minimized by KMP is given by: + + .. math:: + + \mathcal{L}(\mu_w, \Sigma_w) = \sum_{n=1}^N KL[p(y|x_n;\theta) || p_{ref}(y | x_n)] + + \tau ( (\mu_w^T\mu_w) + tr(\Sigma_w) ) + + where :math:`\theta = \{\mu_w, \Sigma_w\}` are the parameters that are being optimized, + :math:`p_{ref}(y | x_n) = \mathcal{N}(\mu_n, \Sigma_n)` is the predicted reference distribution + (e.g. Gaussian by GMR), and :math:`\tau` is the prior regularization term. + + Once the parametric model has been optimized, the optimal mean and covariance of the weights are given by: + + .. math:: + + \mu_w = \Omega (\Omega^T \Omega + \tau \Sigma)^{-1} \mu + \Sigma_w = N (\Omega \Sigma \Omega^T + \tau I)^{-1} + + where :math:`\Omega = [\Phi(x_1) ... \Phi(x_N)] \in \mathbb{R}^{BO \times NO}`, + :math:`\Sigma = blockdiag(\Sigma_1, ..., \Sigma_N) \in \mathbb{R}^{NO \times NO}`, and + :math:`\mu = [\mu_1^T ... \mu_N^T]^T \in \mathbb{R}^{NO \times 1}`. + + Thus, the predicted output mean and covariance on a new input :math:`x^*` is given by: + + .. math:: + + \mu_y &= \Phi(x^*)^T \mu_w = \Phi(x^*) \Omega (\Omega^T \Omega + \tau \Sigma)^{-1} \mu \\ + \Sigma_y &= \Phi(x^*)^T \Sigma_w \Phi(x^*) = N \Phi(x^*)^T (\Omega\Sigma\Omega^T+\tau I)^{-1} \Phi(x^*) + + And by using the kernel trick (and the Woodbury identity for the covariance), this resumes to: + + .. math:: + + \mu_y &= k^* (K + \tau \Sigma)^{-1} \mu \\ + \Sigma_y &= \frac{N}{\tau} (k(x^*, x^*) - k^* (K + \tau \Sigma)^{-1} k^*^T) + + where :math:`K(X,X) \in \mathbb{R}^{NO \times NO}` is the kernel matrix, + :math:`k^* = [k(x^*, x_1) ... k(x^*,x_N)] \in \mathbb{R}^{O \times NO}` is the kernel evaluated + on the new input, and where :math:`k(x_i, x_j) = \hat{k}(x_i, x_j) I_O` with the identity matrix + `I_O \in \mathbb{O \times O}` and :math:`\hat{k}(x_i, x_j)` the kernel function. + + Args: + X (np.array[N,T,I], list of np.array[T,I]): input data matrix of shape NxTxI, where N is the number of + trajectories, T is its length, and I is the input data dimension. + Y (np.array[N,T,O], list of np.array[T,O]): corresponding output data matrix of shape NxTxO, where N is + the number of trajectories, T is its length, and O is the output data dimension. + gmm (None, GMM): the reference generative model. If None, it will create a GMM. + gmm_num_components (int): the number of components for the underlying reference GMM. + prior_reg (float): prior regularization term + dist (callable, None): callable function which accepts two data points from X, and compute the distance + between them. If None and `sample_from_gmm` is False, it will use the 2-norm. + database_threshold (float): threshold associated with the `distance` argument above. If the distance between + a new data point and data point in the database is below the threshold, it will be added to + the database. + database_size_limit (int): limit size of the database. + sample_from_gmm (bool): If we should sample from the generative model to get the inputs to put in the + database. If True, it doesn't use the `distance` and `database_threshold` parameters. + gmm_init (str): how the Gaussians should be initialized. Possible values are 'random' or 'kmeans'. + gmm_reg (float): regularization term for the GMM (that are added to the Gaussians) + gmm_num_iters (int): the maximum number of iterations to train the reference model (GMM) + gmm_convergence_threshold (float): convergence threshold when training the reference model (GMM) + seed (int, None): random seed for the initialization and training of the GMM, and when sampling + verbose (bool): if we should print details during the optimization process + block (bool): if the size of the kernel matrix is bigger than 1000, it will ask for confirmation to + continue. The kernel matrix has to be inversed, which has a time complexity of `O(N^3)` where + `N` is the size of the kernel matrix. + + References: + - [1] "Kernelized Movement Primitives", Huang et al., 2017 + """ + # map the output quaternions from S^3 to R^3 + Y = logarithm_map(Y[:, :, :4]) + + # call the parent + super(QuaternionKMP, self).fit(X=X, Y=Y, gmm=gmm, gmm_num_components=gmm_num_components, prior_reg=prior_reg, + dist=dist, database_threshold=database_threshold, + database_size_limit=database_size_limit, sample_from_gmm=sample_from_gmm, + gmm_init=gmm_init, gmm_reg=gmm_reg, gmm_num_iters=gmm_num_iters, + gmm_convergence_threshold=gmm_convergence_threshold, seed=seed, + verbose=verbose, block=block) + + def predict(self, x): + r""" + Predict output mean :math:`\mu_y` given input data :math:`x^*`. + + .. math:: + + \mu_y(x^*) &= k^* (K + \tau \Sigma)^{-1} \mu \\ + + where :math:`K(X,X) \in \mathbb{R}^{NO \times NO}` is the kernel matrix, + :math:`k^* = [k(x^*, x_1) ... k(x^*,x_N)] \in \mathbb{R}^{O \times NO}` is the kernel evaluated + on the new input, and where :math:`k(x_i, x_j) = \hat{k}(x_i, x_j) I_O` with the identity matrix + `I_O \in \mathbb{O \times O}` and :math:`\hat{k}(x_i, x_j)` the kernel function. + + Args: + x (np.array[I], np.array[N,I]): new input data vector or matrix + + Returns: + np.array[O], np.array[N,O]: output mean(s) + """ + # predict the output means using the KMP in R^3 + means = super(QuaternionKMP, self).predict(x) + + # map the predicted output in R^3 to S^3 using the exponential map + # The 3 first components of the predicted outputs represent the quaternion, while the last 3 represent the + # angular velocities. + if len(means.shape) == 1: + quaternion = get_quaternion_product(exponential_map(means[:3]), self.auxiliary_quaternion) # shape: (4,) + means = np.concatenate(quaternion, means[3:]) # shape: (7,) + else: + quaternions = get_quaternion_product(exponential_map(means[:, :3]), self.auxiliary_quaternion) # (N, 4) + means = np.hstack((quaternions, means[:, 3:])) # shape: (N,7) + + return means + + +# Tests +if __name__ == '__main__': + pass