wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

update Gaussian + GMM/GMR

Browse Source
This commit is contained in:
Brian Delhaisse
2019-11-13 20:10:06 +01:00
parent bc059da3cb
commit 8464300325
4 changed files with 110 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/models/gmm.py
+1 -1
View File
@@ -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()
plt.show()
pyrobolearn/models/dmp/dmp.py
+3 -3
View File
@@ -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
pyrobolearn/models/gaussian.py
+23 -11
View File
@@ -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
pyrobolearn/models/gmm/gmm.py
+83 -6
View File
@@ -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