diff --git a/examples/models/gmm.py b/examples/models/gmm.py index 392b61a..e546fc4 100644 --- a/examples/models/gmm.py +++ b/examples/models/gmm.py @@ -81,4 +81,4 @@ plt.fill_between(time_linspace, means - 2 * std_devs, means + 2 * std_devs, face plt.fill_between(time_linspace, means - std_devs, means + std_devs, facecolor='green', alpha=0.5) plt.title('GMR') plt.scatter(X[:, 0], X[:, 1]) -plt.show() \ No newline at end of file +plt.show() diff --git a/pyrobolearn/models/dmp/dmp.py b/pyrobolearn/models/dmp/dmp.py index bc0dc7f..0852542 100644 --- a/pyrobolearn/models/dmp/dmp.py +++ b/pyrobolearn/models/dmp/dmp.py @@ -83,7 +83,7 @@ class DMP(object): Note that this code was inspired by the `pydmps` code [2,3], but differ in several ways, notably: - we undertake a more object-oriented programming (OOP) approach - - the equations are a little bit differents (e.g. :math:`tau`) in which we use the ones presented in the refs + - the equations are a little bit different (e.g. :math:`tau`) in which we use the ones presented in the refs - we decouple the Euler's method time step with the time step for the number of data points - timesteps: we go from 0 to T included, while DeWolf goes from 0 to T-1 - we use array operation instead of iterating over each element to update them @@ -106,7 +106,7 @@ class DMP(object): - [8] "A Framework for Learning Biped Locomotion with Dynamical Movement Primitives", Nakanishi et al., 2004 - [9] "Policy Search for Motor Primitives in Robotics", Kober et al., 2010 - [10] "A Generalized Path Integral Control Approach to Reinforcement Learning", Theodorou et al., 2010 - - [11] "A correct formulation for the Orientation Dynamic Movement Primitives for robot control in the + - [11] "A correct formulation for the Orientation Dynamic Movement Primitives for robot control in the Cartesian space", Koutras et al., 2019 """ @@ -116,7 +116,7 @@ class DMP(object): Args: canonical_system (CS): canonical system which drives the DMP transformation system forcing_term (list): list of forcing terms (one forcing term for each DMP). Each forcing term can have - different number of basis functions. + different number of basis functions. y0 (float, np.array[float[M]]): initial state of DMPs goal (float, np.array[float[M]]): goal state of DMPs stiffness (float): stiffness term in the transformation system for DMPs diff --git a/pyrobolearn/models/gaussian.py b/pyrobolearn/models/gaussian.py index e4bb2b3..aec534a 100755 --- a/pyrobolearn/models/gaussian.py +++ b/pyrobolearn/models/gaussian.py @@ -183,6 +183,16 @@ class Gaussian(object): covariance = cov sigma = cov + @property + def variances(self): + """Return the diagonal elements of the covariance matrix, i.e. the variances.""" + return np.diag(self.cov) + + @property + def standard_deviations(self): + """Return the square root of the diagonal of the covariance matrix, i.e. the standard deviations.""" + return np.sqrt(self.variances) + @property def mode(self): """value that is the most likely to be sampled""" @@ -603,7 +613,7 @@ class Gaussian(object): assert len(i) == len(value), "The value array and the idx2 array have different lengths" # compute conditional - c = self.cov[np.ix_(o, i)].dot(np.linalg.inv(self.cov[np.ix_(i, i)])) + c = self.cov[np.ix_(o, i)].dot(np.linalg.inv(self.cov[np.ix_(i, i)])) # = \Sigma_{12} \Sigma_{22}^{-1} mu = self.mean[o] + c.dot(value - self.mean[i]) cov = self.cov[np.ix_(o, o)] - c.dot(self.cov[np.ix_(i, o)]) return Gaussian(mu, cov) @@ -1280,10 +1290,12 @@ def plot_3d_and_2d_countour(gaussians, step=500, bound=10, fig=None, title='', b plt.show(block=block) -def plot_2d_ellipse(ax, gaussian, dims, color='g', fill=False, plot_2devs=False, plot_arrows=True): +def plot_2d_ellipse(ax, gaussian, dims=[0, 1], color='g', fill=False, plot_2devs=False, plot_arrows=True): # alias - g = Gaussian(mean=gaussian.mean[np.ix_(dims)], covariance=gaussian.cov[np.ix_(dims, dims)]) # g = gaussian + if dims is None: + dims = [0, 1] + g = Gaussian(mean=gaussian.mean[dims], covariance=gaussian.cov[np.ix_(dims, dims)]) # compute std deviation and eigenvectors from the gaussian std_dev, evecs = g.ellipsoid_axes() @@ -1363,18 +1375,18 @@ if __name__ == '__main__': # from matplotlib.patches import Ellipse # create two 2D Gaussian distributions - m1, c1 = np.array([0.,0.]), np.identity(2)*0.5 - m2, c2 = np.array([1.5,1.5]), np.array([[1.,0.5], [0.5,2.]]) + m1, c1 = np.array([0., 0.]), np.identity(2)*0.5 + m2, c2 = np.array([1.5, 1.5]), np.array([[1., 0.5], [0.5, 2.]]) g1 = Gaussian(m1, c1) g2 = Gaussian(m2, c2) # sample from the Gaussian distributions, and plot them d1 = g1.sample(size=200) d2 = g2.sample(size=200) - fig, ax = plt.subplots(1,1) + fig, ax = plt.subplots(1, 1) ax.set(title='sampling from 2 Gaussians', aspect='equal') - ax.scatter(d1[:,0], d1[:,1], color='b', alpha=0.7) - ax.scatter(d2[:,0], d2[:,1], color='r', alpha=0.7) + ax.scatter(d1[:, 0], d1[:, 1], color='b', alpha=0.7) + ax.scatter(d2[:, 0], d2[:, 1], color='r', alpha=0.7) plt.show() # 3D and 2D plots of the Gaussian distributions @@ -1404,7 +1416,7 @@ if __name__ == '__main__': # samples from the Gaussian and plot ellipse samples = g2.sample(size=100) - fig, ax = plt.subplots(1,1) + fig, ax = plt.subplots(1, 1) ax.set(title='Sampling from one Gaussian', aspect='equal') ax.scatter(samples[:, 0], samples[:, 1], color='b') plot_2d_ellipse(ax, g2, fill=True, plot_2devs=True, plot_arrows=True) @@ -1449,7 +1461,7 @@ if __name__ == '__main__': # addition of two independent Gaussians g_sum = g1 + g2 - fig, ax = plt.subplots(1,1) + fig, ax = plt.subplots(1, 1) ax.set(title='addition', xlim=[-5, 5], ylim=[-5, 5], aspect='equal') e1 = plot_2d_ellipse(ax, g1, color='g', plot_arrows=False) e2 = plot_2d_ellipse(ax, g2, color='b', plot_arrows=False) @@ -1470,7 +1482,7 @@ if __name__ == '__main__': # Fit a Gaussian on given data # # create data - g_data = Gaussian(mean=np.array([2,3]), covariance=np.array([[1, -0.5], [-0.5, 1]])) + g_data = Gaussian(mean=np.array([2, 3]), covariance=np.array([[1, -0.5], [-0.5, 1]])) samples = np.random.multivariate_normal(mean=g_data.mean, cov=g_data.cov, size=1000) # fit one Gaussian and plot it along the data diff --git a/pyrobolearn/models/gmm/gmm.py b/pyrobolearn/models/gmm/gmm.py index faeffe5..0f1f2ef 100755 --- a/pyrobolearn/models/gmm/gmm.py +++ b/pyrobolearn/models/gmm/gmm.py @@ -1276,7 +1276,8 @@ class GMM(object): Warnings: the initialization can affect greatly the results; this is often true with EM algorithms. Args: - X (np.array[N,D]): data matrix + X (np.array[N,D]): data matrix of shape (N,D) where N is the number of data points, and D is the + dimensionality of a data point. reg (float): regularization term num_iters (int): number of iterations threshold (float): convergence threshold @@ -1291,7 +1292,8 @@ class GMM(object): """ # quick check if len(X.shape) != 2: - raise ValueError("Expecting a 2D array of shape NxD for the data") + raise ValueError("Expecting a 2D array of shape NxD for the data (where N is the number of data points, " + "and D is the dimensionality).") # compute dictionary results results = {'losses': [], 'success': False, 'num_iters': 0} @@ -1300,7 +1302,8 @@ class GMM(object): # 1. Initialize self.init(X, method=init, seed=seed, reg=reg) if init is None: - np.random.seed(seed) + if seed is not None: + np.random.seed(seed) # compute initial loss loss = self.log_likelihood(X) @@ -1437,7 +1440,7 @@ class GMM(object): return self.gaussians[idx].sample() # shape: D return np.array([self.gaussians[i].sample() for i in idx]) # shape: NxD - def responsibilities(self, x, dims = None, k=None, axis=1): + def responsibilities(self, x, dims=None, k=None, axis=1): r""" Compute the responsibilities (posterior probability of component k once we have observed the data `x`). These are given by: @@ -1451,6 +1454,9 @@ class GMM(object): Args: x (np.array): data vector/matrix + dims (None, list of int): dimension indices specifying which indices of the mean and covariance of each + Gaussian we have to consider when computing the responsibilities. The number of indices must be between + 1 and the length of the data vector/matrix. k (np.array, int, slice, None): component index(ices). If None, compute the responsibilities wrt to each component. axis (int): This argument is useful when the argument 'k' is an array; k can then be an array of size `N` @@ -1474,7 +1480,8 @@ class GMM(object): if dims is None: input_gaussian = Gaussian(mean=g.mean, covariance=g.cov) else: - input_gaussian = Gaussian(mean=g.mean[np.ix_(dims)], covariance=g.cov[np.ix_(dims, dims)]) + # input_gaussian = Gaussian(mean=g.mean[np.ix_(dims)], covariance=g.cov[np.ix_(dims, dims)]) + input_gaussian = Gaussian(mean=g.mean[dims], covariance=g.cov[np.ix_(dims, dims)]) gaussian_pdfs.append(input_gaussian.pdf(x)) gaussian_pdfs = np.asarray(gaussian_pdfs).T @@ -2060,7 +2067,8 @@ 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, color='b'): +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 if ax is None: @@ -2087,6 +2095,75 @@ def plot_gmm(gmm, dims=[0, 1], X=None, label=True, ax=None, title='GMM', xlim=[- draw_ellipse(g.mean, g.cov, ax=ax, alpha=priors[i] * w_factor) +def plot_gmr(time_linspace, means=None, std_devs=None, covariances=None, gaussians=None, suptitle='GMR', + ylabels=('x', 'y'), xlim=(-6, 6), ylim=(-6, 6)): + """Plot GMR. + + Warnings: this assumes that the first column is the time, and the other columns are the cartesian positions. + """ + + # check given arguments + 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.") + else: + means, std_devs, covariances = [], [], [] + for g in gaussians: + means.append(g.mean) + std_devs.append(np.sqrt(g.variances)) + covariances.append(g.covariance) + + means, std_devs, covariances = np.asarray(means), np.asarray(std_devs), np.asarray(covariances) + + # plot figures for GMR + fig = plt.figure() + grid = plt.GridSpec(nrows=len(ylabels), ncols=len(ylabels)+1, figure=fig) + axes = [] + for i in range(len(ylabels)): + axes.append(fig.add_subplot(grid[i, 0])) + axes.append(fig.add_subplot(grid[:, 1:])) + plt.suptitle(suptitle) + + # plot t-x and t-y + for i in range(len(ylabels)): + mean = means[:, i] + std_dev = std_devs[:, i] + axes[i].plot(time_linspace, mean) + axes[i].fill_between(time_linspace, mean - 2 * std_dev, mean + 2 * std_dev, facecolor='green', alpha=0.3) + axes[i].fill_between(time_linspace, mean - std_dev, mean + std_dev, facecolor='green', alpha=0.5) + axes[i].set_xlabel('t') + axes[i].set_ylabel(ylabels[i]) + # axes[i].scatter(X[:, 0], X[:, i+1]) + + axes[-1].plot(means[:, 0], means[:, 1]) + axes[-1].set_xlabel('x') + axes[-1].set_ylabel('y') + axes[-1].set_xlim(xlim) + axes[-1].set_ylim(ylim) + + # plot x-y + # Ref: https://stackoverflow.com/questions/38291692/combining-patches-to-obtain-single-transparency-in-matplotlib + pts = [] + stoppoint = np.array([[np.nan, np.nan]]) + t = np.linspace(-np.pi, np.pi, 35) + circle = np.array([np.cos(t), np.sin(t)]) + for i, (mean, cov) in enumerate(zip(means, covariances)): + evals, evecs = np.linalg.eigh(cov) + # radius = np.sqrt(evals[1]) + radius = np.sqrt(evals) * evecs + pt = radius.dot(circle).T + mean + pts.append(pt) + if i < len(means) - 1: + pts.append(stoppoint) + + pts = np.concatenate(pts) + axes[2].add_patch(plt.Polygon(pts, closed=True, lw=0.0, color='green', alpha=0.2, zorder=2)) + + def draw_ellipse(position, covariance, ax=None, **kwargs): """Draw an ellipse with a given position and covariance