Commit 4c5df165 authored by Emmanuel PIGNAT's avatar Emmanuel PIGNAT
Browse files

Merge remote-tracking branch 'origin/master'

# Conflicts:
#	pbdlib/gmm.py
parents 5b6b0aa1 9433c690
......@@ -20,8 +20,8 @@ This requires v2.7 of Python.
Following these instructions install the library without copying the files, allowing to edit the sources files without reinstalling.
git clone git@gitlab.idiap.ch:epignat/pbdlib.git
cd pbdlib
git clone https://gitlab.idiap.ch/rli/pbdlib-python.git
cd pbdlib-python
pip install -e .
If pip is not install, you can get it that way:
......@@ -42,3 +42,22 @@ Then navigate through folders and click on desired notebook.
| pbdlib - lqr.ipynb| Linear quadratic regulator to regerate trajectories.|
| pbdlib - Multiple coordinate systems.ipynb| This example shows how motions can adapt to various positions and orientations of objects by projecting the demonstrations in several coordinate systems.|
## User interface for recording data with the mouse
### Installation
sudo apt-get intall python-tk
### Use
cd notebooks
python record_demo.py -p /path/to/folder -f filename
You can click on move the mouse on the left panel to record demonstrations. Press "h" for help. Save with "q".
To record demos with additional moving objects, run
python record_demo.py -p /path/to/folder -f filename -m -c number_of_object
By pressing, the number corresponding to the desired object on your keyboard, you will make it appear and be able to move it.
Rotate them by holding the key of the number and turning the scrollwheel of your mouse.
from pbdlib.gui import InteractiveDemos, MutliCsInteractiveDemos
import argparse
"""
Utilities to record demonstrations
Press "h" for help once in the GUI.
On left is the position, on right the velocity. dt is 0.05 by default.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt)
parser.add_argument(
'-f', '--filename', dest='filename', type=str,
default='test', help='filename for saving the demos'
)
parser.add_argument(
'-p', '--path', dest='path', type=str,
default='', help='path for saving the demos'
)
parser.add_argument(
'-m', '--multi_cs', dest='multi_cs', action='store_true',
default=False, help='record demos in multiple coordinate systems'
)
parser.add_argument(
'-c', '--cs', dest='nb_cs', type=int,
default=2, help='number of coordinate systems'
)
args = parser.parse_args()
if args.multi_cs:
interactive_demo = MutliCsInteractiveDemos(
filename=args.filename, path=args.path, nb_experts=args.nb_cs)
else:
interactive_demo = InteractiveDemos(
filename=args.filename, path=args.path)
interactive_demo.start()
\ No newline at end of file
......@@ -8,7 +8,17 @@ from .model import Model
from .mvn import *
from .plot import *
from .pylqr import *
from .poglqr import PoGLQR
from .poglqr import PoGLQR, SparsePoGLQR
from .mtmm import MTMM, VBayesianGMM
try:
import gui
except ImportError as e:
print "Could not import gui: {0}".format(e.message)
print "run : sudo apt-get install tkinter"
except:
print "Unexpected error:", sys.exc_info()[0]
raise
import utils
import plot
......
......@@ -251,7 +251,47 @@ def mvn_pdf(x, mu, sigma_chol, lmbda, sigma=None, reg=None):
# np.log(sigma_chol[i].diagonal(axis1=0, axis2=1)), axis=0)
# for i in range(N)])
def multi_variate_normal(x, mu, sigma, log=True, gmm=False, lmbda=None):
def multi_variate_t(x, nu, mu, sigma=None, log=True, gmm=False, lmbda=None):
"""
Multivariatve T-distribution PDF
https://en.wikipedia.org/wiki/Multivariate_t-distribution
:param x: np.array([nb_samples, nb_dim])
:param mu: np.array([nb_dim])
:param sigma: np.array([nb_dim, nb_dim])
:param log: bool
:return:
"""
from scipy.special import gamma
if not gmm:
if type(sigma) is float:
sigma = np.array(sigma, ndmin=2)
if type(mu) is float:
mu = np.array(mu, ndmin=1)
if sigma is not None:
sigma = sigma[None, None] if sigma.shape == () else sigma
mu = mu[None] if mu.shape == () else mu
x = x[:, None] if x.ndim == 1 else x
p = mu.shape[0]
dx = mu - x
lmbda_ = np.linalg.inv(sigma) if lmbda is None else lmbda
dist = np.einsum('...j,...j', dx, np.einsum('...jk,...j->...k', lmbda_, dx))
# (nb_timestep, )
lik = gamma((nu + p)/2) * np.linalg.det(lmbda_) ** 0.5/\
(gamma(nu/2) * nu ** (p/2) * np.pi ** (p/2) ) * \
(1 + 1/nu * dist) ** (-(nu+p)/2)
return np.log(lik) if log else lik
else:
raise NotImplementedError
def multi_variate_normal(x, mu, sigma=None, log=True, gmm=False, lmbda=None):
"""
Multivariatve normal distribution PDF
......@@ -266,15 +306,23 @@ def multi_variate_normal(x, mu, sigma, log=True, gmm=False, lmbda=None):
sigma = np.array(sigma, ndmin=2)
if type(mu) is float:
mu = np.array(mu, ndmin=1)
sigma = sigma[None, None] if sigma.shape == () else sigma
if sigma is not None:
sigma = sigma[None, None] if sigma.shape == () else sigma
mu = mu[None] if mu.shape == () else mu
x = x[:, None] if x.ndim == 1 else x
dx = mu - x
lmbda_ = np.linalg.inv(sigma) if lmbda is None else lmbda
log_lik = -0.5 * np.einsum('...j,...j', dx, np.einsum('...jk,...j->...k', lmbda_, dx)) \
- 0.5 * np.log(np.linalg.det(2 * np.pi * sigma))
log_lik = -0.5 * np.einsum('...j,...j', dx, np.einsum('...jk,...j->...k', lmbda_, dx))
if sigma is not None:
log_lik -= 0.5 * np.log(np.linalg.det(2 * np.pi * sigma))
else:
log_lik += 0.5 * np.log(np.linalg.det(2 * np.pi * lmbda))
return log_lik if log else np.exp(log_lik)
else:
raise NotImplementedError
......
......@@ -16,6 +16,24 @@ 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)
priors /= np.sum(priors)
elif mass is not None:
prior_lim = np.sort(self.priors)[::-1][np.max(
[0, np.argmin(np.cumsum(np.sort(self.priors)[::-1]) < mass)])]
priors = (self.priors >= prior_lim) * self.priors
priors /= np.sum(priors)
else:
priors = self.priors
# print priors, self.priors
mus, sigmas = self.moment_matching(priors)
mvn = MVN(nb_dim=self.nb_dim, mu=mus[0], sigma=sigmas[0])
return mvn
def moment_matching(self, h):
"""
......@@ -34,6 +52,18 @@ class GMM(Model):
return mus, sigmas
def __add__(self, other):
if isinstance(other, MVN):
gmm = GMM(nb_dim=self.nb_dim, nb_states=self.nb_states)
gmm.priors = self.priors
gmm.mu = self.mu + other.mu[None]
gmm.sigma = self.sigma + other.sigma[None]
return gmm
else:
raise NotImplementedError
def __mul__(self, other):
"""
......@@ -42,14 +72,36 @@ class GMM(Model):
:param other:
:return:
"""
gmm = GMM(nb_dim=self.nb_dim, nb_states=self.nb_states)
gmm.priors = self.priors
gmm.mu = np.einsum('aij,aj->ai', self.lmbda, self.mu) + \
np.einsum('aij,aj->ai', other.lmbda, other.mu)
if isinstance(other, MVN):
gmm = GMM(nb_dim=self.nb_dim, nb_states=self.nb_states)
gmm.mu = np.einsum('aij,aj->ai', self.lmbda, self.mu) + \
np.einsum('ij,j->i', other.lmbda, other.mu)[None]
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]\
+ 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)
)
gmm.priors = np.exp(Z) * self.priors
gmm.priors /= np.sum(gmm.priors)
else:
gmm = GMM(nb_dim=self.nb_dim, nb_states=self.nb_states)
gmm.priors = self.priors
gmm.mu = np.einsum('aij,aj->ai', self.lmbda, self.mu) + \
np.einsum('aij,aj->ai', other.lmbda, other.mu)
gmm.lmbda = self.lmbda + other.lmbda
gmm.lmbda = self.lmbda + other.lmbda
gmm.mu = np.einsum('aij,aj->ai', gmm.sigma, gmm.mu)
gmm.mu = np.einsum('aij,aj->ai', gmm.sigma, gmm.mu)
return gmm
......@@ -119,7 +171,7 @@ class GMM(Model):
def compute_resp(self, demo=None, dep=None, table=None, marginal=None, ):
def compute_resp(self, demo=None, dep=None, table=None, marginal=None, norm=True):
sample_size = demo.shape[0]
B = np.ones((self.nb_states, sample_size))
......@@ -142,15 +194,22 @@ class GMM(Model):
B[[i], :] *= multi_variate_normal(demo, mu[i, d],
sigma[dGrid][:, :, 0], log=False)
B *= self.priors[:, None]
return B/np.sum(B, axis=0)
if norm:
return B/np.sum(B, axis=0)
else:
return B
def init_params_scikit(self, data):
def init_params_scikit(self, data, cov_type='full'):
from sklearn.mixture import BayesianGaussianMixture, GaussianMixture
gmm_init = GaussianMixture(self.nb_states, 'full', n_init=10, init_params='random')
gmm_init = GaussianMixture(self.nb_states, cov_type, n_init=5, init_params='random')
gmm_init.fit(data)
self.mu = gmm_init.means_
self.sigma = gmm_init.covariances_
if cov_type == 'diag':
self.sigma = np.array([np.diag(gmm_init.covariances_[i]) for i in range(self.nb_states)])
else:
self.sigma = gmm_init.covariances_
self.priors = gmm_init.weights_
self.Trans = np.ones((self.nb_states, self.nb_states)) * 0.01
......@@ -183,7 +242,8 @@ class GMM(Model):
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, no_init=False):
kmeans_init=False, random_init=True, dep_mask=None, verbose=False, only_scikit=False,
no_init=False):
"""
:param data: [np.array([nb_timesteps, nb_dim])]
......@@ -212,15 +272,18 @@ class GMM(Model):
nb_samples = data.shape[0]
if not no_init:
if random_init:
if random_init and not only_scikit:
self.init_params_random(data)
elif kmeans_init:
elif kmeans_init and not only_scikit:
self.init_params_kmeans(data)
else:
self.init_params_scikit(data)
if diag:
self.init_params_scikit(data, 'diag')
else:
self.init_params_scikit(data, 'full')
if only_scikit: return
data = data.T
LL = np.zeros(nb_max_steps)
......@@ -269,10 +332,11 @@ class GMM(Model):
self.sigma = np.einsum(
'acj,aic->aij', np.einsum('aic,ac->aci', dx, GAMMA2), dx) + reg_finish
print colored('Converged after %d iterations: %.3e' % (it, LL[it]), 'red', 'on_white')
if verbose:
print colored('Converged after %d iterations: %.3e' % (it, LL[it]), 'red', 'on_white')
return GAMMA
print "GMM did not converge before reaching max iteration. Consider augmenting the number of max iterations."
if verbose:
print "GMM did not converge before reaching max iteration. Consider augmenting the number of max iterations."
return GAMMA
......
from interactive import Interactive
from multi_cs_demos import MutliCsInteractiveDemos, MultiCsInteractive
from demos import InteractiveDemos
\ No newline at end of file
import numpy as np
import os
import matplotlib.pyplot as plt
from pbdlib.gui import Interactive
from termcolor import colored
try:
import tkinter as tk
except ImportError:
import Tkinter as tk
except:
raise
from tkFileDialog import asksaveasfilename
from matplotlib import gridspec
import pbdlib as pbd
class Robot(object):
def __init__(self, T):
self.x, self.dx, self.dt = np.array([0., 0.]), np.array([0., 0.]), 1. / T
self.ddx, self.fsx, self.fx = np.array([0., 0.]), np.array([0.]), np.array([0., 0.])
self.sensor_mode = 0
class InteractiveDemos(Interactive, Robot):
"""
GUI for recording demonstrations in 2D
"""
def __init__(self, filename='test', path='', **kwargs):
Interactive.__init__(self)
self.path = os.path.dirname(pbd.__file__) + '/data/gui/' if path == '' else path
self.fig = plt.figure(figsize=(15, 8), facecolor='white')
self.bindings.update({
'q': (self.save_demos, [], "save demos"),
'c': (self.clear_demos, [], "clear demos"),
'x': (self.clear_demos, [True], "clear last demos"),
'i': ([self.incr_param, self.highlight_demos], [['current_demo'], []], "next demos"),
'd': (self.clear_demos, [False, True], "clear selected demos"),
})
Robot.__init__(self, self.simulation_T)
gs = gridspec.GridSpec(1, 2)
self.filename = filename
self.ax_x = plt.subplot(gs[0])
self.ax_dx = plt.subplot(gs[1])
self.set_events()
self.set_plots()
self.is_demonstrating = False
self.velocity_mode = False
self.curr_demo, self.curr_demo_dx = [], []
self.curr_demo_obj, self.curr_demo_obj_dx = [], []
self._current_demo = {'x': [], 'dx': []}
self.curr_mouse_pos = None
self.robot_pos = np.zeros(2)
self.nb_demos = 0
self.demos = {'x': [], 'dx': []}
self.params.update({'current_demo': [0, 0, self.nb_demos]})
self.loaded = False
#
# win = plt.gcf().canvas.manager.window
#
# win.lift()
# win.attributes("-topmost", True)
# win.attributes("-alpha", 0.4)
try:
self.demos = np.load(self.path + filename + '.npy')[()]
self.nb_demos = self.demos['x'].__len__(); self.params['current_demo'][2] = self.nb_demos - 1
print colored('Existing skill, demos loaded', 'green')
self.replot_demos()
self.fig.canvas.draw()
self.loaded = True
except:
self.demos = {'x': [], 'dx': []}
print colored('Not existing skill', 'red')
def highlight_demos(self):
data = self.demos['x'][self.params['current_demo'][0]]
self.plots['current_demo'].set_data(data[:, 0], data[:, 1] )
self.fig.canvas.draw()
def plot_sensor_value(self, s, scale=1.):
data = np.vstack([self.x + np.array([0., 1.]) * s * scale, self.x])
data -= 5. * np.array([0., 1.])[None]
self.plots['sensor_value'].set_data(data[:, 0], data[:, 1])
def set_plots(self):
self.plots.update({
'robot_plot':self.ax_x.plot([], [], 'o-', mew=4, mec='orangered', ms=10, mfc='w')[0],
'sensor_value':self.ax_x.plot([], [],ls=(0,(1,1)), lw=10)[0],
'attractor_plot':self.ax_x.plot([], [], 'o-', mew=4, mec='teal', ms=10, mfc='w')[0],
'obj_plot':self.ax_x.plot([], [], 'o-', mew=4, mec='steelblue', ms=10, mfc='w')[0],
'current_demo': self.ax_x.plot([], [], lw=3, ls='--', color='orangered')[0],
'current_demo_dx': self.ax_dx.plot([], [], lw=3, ls='--', color='orangered')[0]
})
for ax, lim in zip([self.ax_x, self.ax_dx], [100, 25]):
ax.set_xlim([-lim, lim])
ax.set_ylim([-lim, lim])
def sim_dynamics(self, ffx, n_steps=10):
if not self.velocity_mode:
m = 1.0
ddx = ffx/m
self.x += self.dt / n_steps * self.dx + 0.5 * self.ddx * (
self.dt / n_steps) ** 2
self.dx += self.dt / n_steps * 0.5 * (self.ddx + ddx)
self.dxx = np.copy(ddx)
else:
kp = 0.;
kv = kp ** 0.5 * 2
for i in range(50):
ddx = kp * (self.curr_mouse_pos - self.dx)
self.dx += self.dt * ddx
self.x += self.dt * self.dx + (self.dt ** 2) / 2. * ddx
def timer_event(self, event):
if self.is_demonstrating:
if self.curr_mouse_pos is None: self.pretty_print('Outside'); return
# print self.x, self.dx
kp = 400.;
kv = kp ** 0.5 * 2
n_steps = 10
for i in range(n_steps):
ffx = kp * (self.curr_mouse_pos - self.x) - kv * self.dx
self.sim_dynamics(ffx)
# self.curr_demo += [np.copy(self.x)]; self.curr_demo_dx += [np.copy(self.dx)]
self._current_demo['x'] += [np.copy(self.x)]
self._current_demo['dx'] += [np.copy(self.dx)]
def move_event(self, event):
self.curr_mouse_pos = None if None in [event.xdata, event.ydata] else np.array([event.xdata, event.ydata])
if event.key == 'shift' or self.is_demonstrating:
self.robot_pos = np.copy(self.curr_mouse_pos)
if not self.is_demonstrating:
self.plots['robot_plot'].set_data(self.robot_pos[0], self.robot_pos[1])
self.fig.canvas.draw()
def plot_timer_event(self, event):
self.robot_pos = self.curr_mouse_pos if self.robot_pos is None else self.robot_pos
self.plots['attractor_plot'].set_data(self.robot_pos[0], self.robot_pos[1])
self.plots['robot_plot'].set_data(self.x[0], self.x[1])
if self.is_demonstrating:
curr_demo_arr = np.array(self._current_demo['x'])
curr_demo_dx_arr = np.array(self._current_demo['dx'])
self.plots['current_demo'].set_data(curr_demo_arr[:, 0],
curr_demo_arr[:, 1])
self.plots['current_demo_dx'].set_data(curr_demo_dx_arr[:, 0],
curr_demo_dx_arr[:, 1])
self.fig.canvas.draw()
def click_event(self, event):
if event.key is None:
self.pretty_print('Demonstration started')
self.velocity_mode = event.inaxes == self.ax_dx
self.is_demonstrating = True
if not self.velocity_mode:
self.x = self.curr_mouse_pos
else:
self.x = self.demos['x'][-1][0] if self.nb_demos > 0 else np.array([0., 0.])
self.dx = self.curr_mouse_pos
[t.start() for t in [self.timer, self.plot_timer]]
def release_event(self, event):
if event.key is None:
self.pretty_print('Demonstration finished')
self.is_demonstrating = False
self.finish_demo()
[t.stop() for t in [self.timer, self.plot_timer]]
def replot_demos(self):
for i in range(self.nb_demos):
data = self.demos['x'][i]
self.plots['demo_%d' % i] = \
self.ax_x.plot(data[:, 0], data[:, 1], lw=2, ls='--')[0]
data = self.demos['dx'][i]
self.plots['demo_dx_%d' % i] = \
self.ax_dx.plot(data[:, 0], data[:, 1], lw=2, ls='--')[0]
def clear_demos(self, last=False, selected=False):
"""
:param last: [bool]
Delete only last one
"""
if last or selected:
idx = -1 if last else self.params['current_demo'][0]
for s in self.demos:
self.demos[s].pop(idx)
for i in range(self.nb_demos):
self.plots['demo_%d' % (i)].remove()
self.plots['demo_dx_%d' % (i)].remove()
self.nb_demos = len(self.demos['x']); self.params['current_demo'][2] = self.nb_demos - 1
self.replot_demos()
if selected:
self.plots['current_demo'].set_data([], [])
self.fig.canvas.draw()
else:
for i in range(self.nb_demos):
self.plots['demo_%d' % i].remove()
self.plots['demo_dx_%d' % i].remove()