Commit a3cdf732 authored by Emmanuel PIGNAT's avatar Emmanuel PIGNAT
Browse files

COMMIT NOTEBOOKS

parents 767538a8 d853a5a6
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
......@@ -9,16 +9,17 @@ from .mvn import *
from .plot import *
from .poglqr import PoGLQR, LQR, GMMLQR
from .mtmm import MTMM, VBayesianGMM, VMBayesianGMM, VBayesianHMM
from .dmp import DMP
try:
import gui
except ImportError as e:
print "Could not import gui: {0}".format(e.message)
print "run : sudo apt-get install tkinter"
print("Could not import gui: {0}".format(e.message))
print("run : sudo apt-get install tkinter")
except:
print "Unexpected error:", sys.exc_info()[0]
print("Unexpected error:", sys.exc_info()[0])
raise
import utils
import plot
from . import utils
from . import plot
import numpy as np
import matplotlib.pyplot as plt
from .gmm import GMM
from .hmm import HMM
from .hsmm import HSMM
from .mtmm import VBayesianGMM
from .plot import plot_gmm
class DMP(object):
"""
Dynamic Movement Primitives with shaping function f(.) learned by Locally Weighted Regression (LWR) or
Bayesian Gaussian Mixture Model (BGMM).
"""
def __init__(self, bf_number, K, x, state_dependent=False, mixed=False):
# Assumes the same data_length for each demo of each task
# also for each task, assumes the same number of demos
# duration might differ
# X contains [task_number, demo_number, data_length, [time, features]]
self.task_number = x.shape[0]
self.demo_number = x.shape[1]
self.data_length = x.shape[2]
self.dof = x.shape[-1] - 1 # Number of degrees of freedom for DMP
# Making the time start from 0s.
x_subs = np.zeros_like(x)
x_subs[:, :, :, 0] = np.tile(x[:, :, 0, 0][:, :, None], (1, 1, x.shape[2]))
x = x - x_subs
self.t = x[:, :, :, 0] # Time information : [task_number, demo_number, data_length]
self.x = x[:, :, :, 1:] # Demonstrated trajectory : [task_number, demo_number, data_length, features]
# Initial position : [task_number, demo_number, data_length, features]
self.x0 = np.tile(self.x[:, :, 0, :][:, :, None, :], (1, 1, self.data_length, 1))
# Goal position : [task_number, demo_number, data_length, features]
self.g = np.tile(self.x[:, :, -1, :][:, :, None, :], (1, 1, self.data_length, 1))
self.bf_number = bf_number # Basis function number
self.K = np.diag(K) # Spring constant
self.D = np.diag(2 * np.sqrt(K)) # Damping constant, critically damped
# Integrating the canonical system and mapping 's' to the time
self.tau = self.t[:, :, -1] # Duration of the demonstrations : [task_number, demo_number]
convergence_rate = 0.001
self.alpha = -np.log(convergence_rate)
# self.s : [task_number, demo_number, data_length, 1]
self.s = np.exp(
np.einsum('td, tdl->tdl', -self.alpha / self.tau, self.t)
)[:, :, :, None]
s = np.tile(self.s, (1, 1, 1, self.bf_number))
# Numerical Differentiations
dt = np.diff(self.t[0, 0, :], axis=-1)[0]
x_dot = np.gradient(self.x, dt, axis=-2) # [task_number, demo_number, data_length, features]
x_ddot = np.gradient(x_dot, dt, axis=-2)
v = np.einsum('td,tdlj->tdlj', self.tau, x_dot)
v_dot = np.einsum('td,tdlj->tdlj', self.tau, x_ddot)
tau_v_dot = np.einsum('td,tdlj->tdlj', self.tau, v_dot)
# Finding the target shaping function
K_inv = np.linalg.inv(self.K) # [features, features]
Dv = np.einsum('ij,tdlj->tdli', self.D, v) # [task_number, demo_number, data_length, features]
self.f_target = np.einsum('ij,tdlj->tdli', K_inv, tau_v_dot + Dv) + \
(self.x - self.g) + \
np.einsum('tdl,tdlj->tdlj', s[:, :, :, 0], self.g - self.x0)
# self.f_target = tau_v_dot + Dv + np.einsum('ij,tdlj->tdli', self.K, self.x - self.g)
if state_dependent:
self.inp = self.x
elif mixed:
self.inp = np.concatenate([self.s, self.x], axis=-1)
else:
self.inp = self.s
self.method = None
self.state_dependent = state_dependent
self.mixed = mixed
def learn_lwr(self):
assert self.task_number == 1, \
"Only one task can be learned with LWR," \
" try learn_contextual_bgmm to learn a parametric task"
self.method = "lwr"
# Creating basis functions and psi_matrix
# Centers logarithmically distributed between 0.001 and 1
# self.c : [task_number, demo_number, data_length, bf_number]
self.c_exec = np.logspace(-3, 0, num=self.bf_number)
self.h_exec = self.bf_number / (self.c_exec ** 2)
c = np.tile(
self.c_exec[None, None, None],
(self.task_number, self.demo_number, self.data_length, 1)
) # centers of basis functions
h = self.bf_number / (c ** 2) # widths of basis functions
# self.psi_matrix : [task_number, demo_number, data_length, bf_number]
psi_matrix = np.exp(-h * (self.s - c) ** 2)
# self.inv_sum_bfs : [task_number, demo_number, data_length]
inv_sum_bfs = 1.0 / np.sum(psi_matrix, axis=-1)
bf_target = np.einsum('tdlb,tdl->tdlb', psi_matrix * self.s, inv_sum_bfs)
sol = np.linalg.lstsq(
np.concatenate(np.concatenate(bf_target, axis=0), axis=0),
np.concatenate(np.concatenate(self.f_target, axis=0), axis=0)
)
self._weights = sol[0]
def learn_gmm(self, n_comp=20, hmm=False, hsmm=False, plot=False):
x_joint = np.concatenate([
np.concatenate(self.inp, axis=0),
np.concatenate(self.f_target, axis=0),
], axis=-1)
self.n_joint = x_joint.shape[-1]
self.inp_dim = self.inp.shape[-1]
self.x_joint = x_joint
if hmm:
self.joint_model = HMM(nb_states=n_comp)
self.method = "HMM"
elif hsmm:
self.joint_model = HSMM(nb_states=n_comp)
self.method = "HSMM"
else:
self.joint_model = GMM(nb_states=n_comp)
self.method = "GMM"
self.joint_model.init_hmm_kbins(x_joint)
x_joint_hmm = x_joint
x_joint = np.concatenate(x_joint)
if not hmm and not hsmm:
self.joint_model.em(x_joint, reg=1E-8)
else:
self.joint_model.em(x_joint_hmm, reg=1E-3, nb_max_steps=100)
if plot:
fig, ax = plt.subplots(nrows=self.n_joint - 1, ncols=self.n_joint - 1, figsize=(10, 10))
for i in range(self.n_joint):
for j in range(self.n_joint):
if not i == j and j > i:
ax[i][j - 1].plot(x_joint[:, i], x_joint[:, j], 'kx')
ax[i][j - 1].autoscale(False)
plot_gmm(self.joint_model.mu, self.joint_model.sigma, dim=[i, j], ax=ax[i][j - 1], alpha=0.2)
def learn_bgmm(self, n_comp=20, cov_tril_prior=0.01, plot=False, plot_data=False):
assert self.task_number == 1, \
"Only one task can be learned with LWR," \
" try learn_contextual_bgmm to learn a parametric task"
self.method = "bgmm"
# x_joint = np.concatenate([
# np.concatenate(np.concatenate(self.inp, axis=0), axis=0)[:, 0][:, None],
# np.concatenate(np.concatenate(self.f_target, axis=0), axis=0)
# ], axis=-1)
x_joint = np.concatenate([
np.reshape(self.inp, (-1, self.inp.shape[-1])),
np.reshape(self.f_target, (-1, self.f_target.shape[-1])),
], axis=-1)
self.n_joint = x_joint.shape[1]
self.inp_dim = self.inp.shape[-1]
self.x_joint = x_joint
# cov_tril = cov_tril_prior * np.eye(self.n_joint)
# cov = cov_tril.T.dot(cov_tril)
# self.joint_model = VBayesianGMM({
# 'n_components': n_comp, 'n_init': 1, 'reg_covar': 1E-6,
# 'covariance_prior': cov * 1, 'mean_precision_prior': 20. ** -2,
# 'weight_concentration_prior_type': 'dirichlet_process', 'weight_concentration_prior': 1e3,
# 'degrees_of_freedom_prior': self.n_joint - 1. + 0.3, 'warm_start': False})
self.joint_model = VBayesianGMM({
'n_components': n_comp, 'n_init': 5})
self.joint_model.posterior(data=x_joint)
if plot:
print("Used state number: ", self.joint_model.get_used_states().nb_states, "out of ", n_comp)
fig, ax = plt.subplots(nrows=self.n_joint - 1, ncols=self.n_joint - 1, figsize=(10, 10))
for i in range(self.n_joint):
for j in range(self.n_joint):
if not i == j and j > i:
ax[i][j - 1].plot(x_joint[:, i], x_joint[:, j], 'kx')
ax[i][j - 1].autoscale(False)
self.joint_model.get_used_states().plot(dim=[i, j], ax=ax[i][j - 1], alpha=0.2)
def learn_contextual_bgmm(self, params, n_comp=20, cov_tril_prior=0.01, plot=False):
assert self.task_number > 1, \
"If only one task is to be trained," \
" try learn_bgmm to learn a non-parametric task"
self.method = "contextual bgmm"
x_joint = np.concatenate([
np.concatenate(np.concatenate(self.s, axis=0), axis=0)[:, 0][:, None],
np.kron(params, np.ones(int(self.data_length * self.demo_number)))[:, None],
np.concatenate(np.concatenate(self.f_target, axis=0), axis=0)
], axis=-1)
self.n_joint = x_joint.shape[1]
cov_tril = cov_tril_prior * np.eye(self.n_joint)
cov = cov_tril.T.dot(cov_tril)
self.joint_model = VBayesianGMM({
'n_components': n_comp, 'n_init': 5, 'reg_covar': 1E-8,
'covariance_prior': cov * 1, 'mean_precision_prior': 20. ** -2,
'weight_concentration_prior_type': 'dirichlet_process', 'weight_concentration_prior': 1e3,
'degrees_of_freedom_prior': self.n_joint - 1. + 0.3, 'warm_start': False})
self.joint_model.posterior(data=x_joint)
if plot:
print("Used state number: ",
self.joint_model.get_used_states().nb_states,
"out of ", n_comp)
fig, ax = plt.subplots(nrows=self.n_joint - 1, ncols=self.n_joint - 1, figsize=(10, 10))
for i in range(self.n_joint):
for j in range(self.n_joint):
if not i == j and j > i:
ax[i][j - 1].plot(x_joint[:, i], x_joint[:, j], 'kx')
# joint_model.get_used_states().plot(dim=[i, j], ax=ax[i][j-1], alpha=0.2)
ax[i][j - 1].autoscale(False)
self.joint_model.get_used_states().plot(dim=[i, j], ax=ax[i][j - 1], alpha=0.2)
# joint_model.models[1].get_used_states().plot(dim=[i, j], ax=ax[i][j-1], alpha=0.2, color='b')
@property
def weights(self):
return self._weights
def execute(self, time, dt, des_tau, x0, g, x, xdot, zeta=0., param=None):
# Integrate the canonical system
s = np.exp(((-self.alpha / des_tau) * time))
if self.state_dependent:
inp = x
elif self.mixed:
inp = np.concatenate([np.array([s]), x],axis=0)
else:
inp = np.array([s])
# Nonlinear function computation
if self.method == "lwr":
# Basis functions for the new state
psi = np.exp(-self.h_exec * ((s - self.c_exec) ** 2))
sum_of_bfs = np.sum(psi)
fs_nom = np.sum(np.einsum('bj,b->bj', self.weights, psi), axis=0)
fs = (fs_nom / sum_of_bfs) * s
elif self.method == "bgmm":
mtmm = self.joint_model.condition(
inp,
slice(0, self.inp_dim),
slice(self.inp_dim, self.n_joint))
# mtmm = self.joint_model.get_used_states().condition(
# inp,
# slice(0, self.inp_dim),
# slice(self.inp_dim, self.n_joint))
fs = mtmm.sample(1)[0]
# fs = mtmm.mu[np.argmax(mtmm.priors)]
# fs = mtmm.get_matching_gaussian()[0][0]
elif self.method == "contextual bgmm":
mtmm = self.joint_model.condition(
np.array([s, param]),
slice(0, 2),
slice(2, self.n_joint)
)
# fs = mtmm.sample(1)[0]
# fs = mtmm.get_matching_gaussian()[0][0]
fs = mtmm.mu[np.argmax(mtmm.priors)]
else:
# gmm_mu, gmm_sigma = self.joint_model.condition(
# inp[None],
# slice(0, self.inp_dim),
# slice(self.inp_dim, self.n_joint))
# fs = gmm_mu[0]
gmm = GMM(nb_states=self.joint_model.nb_states)
priors, mu, sigma = self.joint_model.condition(
inp[None],
slice(0, self.inp_dim),
slice(self.inp_dim, self.n_joint), return_gmm=True)
gmm.mu = mu[:, 0]
gmm.sigma = sigma
gmm.priors = priors[:, 0]
fs = gmm.sample(1)[0]
# Scaled Velocity needed
v = des_tau * xdot
# Main equations
v_dot = (1.0 / des_tau) * (
np.dot(self.K, g - x)
- np.dot(self.D, v)
- np.dot(self.K, (g - x0)) * s
+ np.dot(self.K, fs)
+ zeta
)
# v_dot = (1.0 / des_tau) * (
# np.dot(self.K, g - x)
# - np.dot(self.D, v)
# + fs
# )
v = v + v_dot * dt
xdot = v / des_tau
x = x + xdot * dt
return x, xdot
def rollout(self, dt, des_tau, x0, g, param=None):
time = 0
x = x0
x_dot = np.zeros_like(x0)
x_holder = [x0]
while time <= des_tau:
x, x_dot = self.execute(time, dt, des_tau, x0, g, x, x_dot, param=param)
time += dt
x_holder.append(x)
return np.stack(x_holder)
......@@ -5,8 +5,8 @@ from scipy.special import gamma, gammaln
colvec = lambda x: np.array(x).reshape(-1, 1)
rowvec = lambda x: np.array(x).reshape(1, -1)
realmin = np.finfo(np.float32).tiny
realmax = np.finfo(np.float32).max
realmin = np.finfo(np.float64).tiny
realmax = np.finfo(np.float64).max
def limit_gains(gains, gain_limit):
"""
......
import numpy as np
from model import *
from functions import multi_variate_normal
from .model import *
from .functions import multi_variate_normal
from scipy.linalg import block_diag
from termcolor import colored
from mvn import MVN
from .mvn import MVN
class GMM(Model):
def __init__(self, nb_states=1, nb_dim=None, init_zeros=False, mu=None, lmbda=None, sigma=None, priors=None):
def __init__(self, nb_states=1, nb_dim=None, init_zeros=False, mu=None, lmbda=None,
sigma=None, priors=None):
if mu is not None:
nb_states = mu.shape[0]
nb_dim = mu.shape[-1]
Model.__init__(self, nb_states, nb_dim)
# flag to indicate that publishing was not init
self.publish_init = False
......@@ -25,6 +27,7 @@ class GMM(Model):
if init_zeros:
self.init_zeros()
def get_matching_mvn(self, max=False, mass=None):
if max:
priors = (self.priors == np.max(self.priors)).astype(np.float32)
......@@ -44,6 +47,7 @@ class GMM(Model):
return mvn
def moment_matching(self, h):
"""
Perform moment matching to approximate a mixture of Gaussian as a Gaussian
......@@ -55,12 +59,13 @@ class GMM(Model):
h = h[None]
mus = np.einsum('ak,ki->ai', h, self.mu)
dmus = self.mu[None] - mus[:, None] # nb_timesteps, nb_states, nb_dim
dmus = self.mu[None] - mus[:, None] # nb_timesteps, nb_states, nb_dim
sigmas = np.einsum('ak,kij->aij', h, self.sigma) + \
np.einsum('ak,akij->aij',h , np.einsum('aki,akj->akij', dmus, dmus))
np.einsum('ak,akij->aij', h, np.einsum('aki,akj->akij', dmus, dmus))
return mus, sigmas
def __add__(self, other):
if isinstance(other, MVN):
gmm = GMM(nb_dim=self.nb_dim, nb_states=self.nb_states)
......@@ -74,6 +79,7 @@ class GMM(Model):
else:
raise NotImplementedError
def __mul__(self, other):
"""
Renormalized product of Gaussians, component by component
......@@ -89,15 +95,15 @@ class GMM(Model):
gmm.lmbda = self.lmbda + other.lmbda[None]
gmm.mu = np.einsum('aij,aj->ai', gmm.sigma, gmm.mu)
Z = np.linalg.slogdet(self.lmbda)[1]\
Z = np.linalg.slogdet(self.lmbda)[1] \
+ np.linalg.slogdet(other.lmbda)[1] \
- 0.5 * np.linalg.slogdet(gmm.lmbda)[1] \
- self.nb_dim / 2. * np.log(2 * np.pi) \
+ 0.5 * (np.einsum('ai,aj->a',
np.einsum('ai,aij->aj', gmm.mu, gmm.lmbda), gmm.mu)
-np.einsum('ai,aj->a',
np.einsum('ai,aij->aj', self.mu, self.lmbda), self.mu)
-np.sum(np.einsum('i,ij->j', other.mu, other.lmbda) * other.mu)
- np.einsum('ai,aj->a',
np.einsum('ai,aij->aj', self.mu, self.lmbda), self.mu)
- np.sum(np.einsum('i,ij->j', other.mu, other.lmbda) * other.mu)
)
gmm.priors = np.exp(Z) * self.priors
gmm.priors /= np.sum(gmm.priors)
......@@ -114,6 +120,7 @@ class GMM(Model):
return gmm
def __mod__(self, other):
"""
Renormalized product of Gaussians, component by component
......@@ -133,9 +140,9 @@ class GMM(Model):
gmm.mu = np.einsum('abij,abj->abi', gmm.sigma, gmm.mu)
return gmm
def marginal_model(self, dims):
"""
Get a GMM of a slice of this GMM
......@@ -143,13 +150,14 @@ class GMM(Model):
:type dims: slice
:return:
"""
gmm = GMM(nb_dim=dims.stop-dims.start, nb_states=self.nb_states)
gmm = GMM(nb_dim=dims.stop - dims.start, nb_states=self.nb_states)
gmm.priors = self.priors
gmm.mu = self.mu[:, dims]
gmm.sigma = self.sigma[:, dims, dims]
return gmm
def lintrans(self, A, b):
"""
Linear transformation of a GMM
......@@ -163,10 +171,11 @@ class GMM(Model):
gmm.priors = self.priors
gmm.mu = np.einsum('ij,aj->ai', A, self.mu) + b
gmm.lmbda = np.einsum('aij,kj->aik',
np.einsum('ij,ajk->aik', A, self.lmbda), A)
np.einsum('ij,ajk->aik', A, self.lmbda), A)
return gmm
def concatenate_gaussian(self, q, get_mvn=True, reg=None):
"""
Get a concatenated-block-diagonal replication of the GMM with sequence of state
......@@ -180,7 +189,8 @@ class GMM(Model):
"""
if reg is None:
if not get_mvn:
return np.concatenate([self.mu[i] for i in q]), block_diag(*[self.sigma[i] for i in q])
return np.concatenate([self.mu[i] for i in q]), block_diag(
*[self.sigma[i] for i in q])
else:
mvn = MVN()
mvn.mu = np.concatenate([self.mu[i] for i in q])
......@@ -201,7 +211,6 @@ class GMM(Model):
return mvn
def compute_resp(self, demo=None, dep=None, table=None, marginal=None, norm=True):
sample_size = demo.shape[0]
......@@ -226,10 +235,11 @@ class GMM(Model):
sigma[dGrid][:, :, 0], log=False)
B *= self.priors[:, None]
if norm:
return B/np.sum(B, axis=0)
return B / np.sum(B, axis=0)
else:
return B
def init_params_scikit(self, data, cov_type='full'):
from sklearn.mixture import BayesianGaussianMixture, GaussianMixture
gmm_init = GaussianMixture(self.nb_states, cov_type, n_init=5, init_params='random')
......@@ -237,7 +247,8 @@ class GMM(Model):
self.mu = gmm_init.means_
if cov_type == 'diag':
self.sigma = np.array([np.diag(gmm_init.covariances_[i]) for i in range(self.nb_states)])
self.sigma = np.array(
[np.diag(gmm_init.covariances_[i]) for i in range(self.nb_states)])
else:
self.sigma = gmm_init.covariances_
......@@ -247,12 +258,13 @@ class GMM(Model):
self.init_priors = np.ones(self.nb_states) * 1. / self.nb_states
def init_params_kmeans(self, data):
from sklearn.cluster import KMeans
km_init = KMeans(n_clusters=self.nb_states)
km_init.fit(data)
self.mu = km_init.cluster_centers_
self.priors = np.ones(self.nb_states)/ self.nb_states
self.priors = np.ones(self.nb_states) / self.nb_states
self.sigma = np.array([np.eye(self.nb_dim) for i in range(self.nb_states)])
self.Trans = np.ones((self.nb_states, self.nb_states)) * 0.01
......@@ -266,15 +278,16 @@ class GMM(Model):
(data.shape[0] - 1)
self.mu = np.array([np.random.multivariate_normal(mu, sigma)
for i in range(self.nb_states)])
for i in range(self.nb_states)])
self.sigma = np.array([sigma + self.reg for i in range(self.nb_states)])
self.priors = np.ones(self.nb_states) / self.nb_states
def em(self, data, reg=1e-8, maxiter=100, minstepsize=1e-5, diag=False, reg_finish=False,
kmeans_init=False, random_init=True, dep_mask=None, verbose=False, only_scikit=False,
no_init=False):
no_init=False):
"""
:param data: [np.array([nb_timesteps, nb_dim])]
......@@ -299,7 +312,7 @@ class GMM(Model):
nb_min_steps = 5 # min num iterations
nb_max_steps = maxiter # max iterations
max_diff_ll = minstepsize # max log-likelihood increase
max_diff_ll = minstepsize # max log-likelihood increase
nb_samples = data.shape[0]
......@@ -326,7 +339,8 @@ class GMM(Model):
for i in range(self.nb_states):
L_log[i, :] = np.log(self.priors[i]) + multi_variate_normal(data.T, self.mu[i],
self.sigma[i], log=True)
self.sigma[i],
log=True)
L = np.exp(L_log)
GAMMA = L / np.sum(L, axis=0)
......@@ -334,12 +348,12 @@ class GMM(Model):