Commit 217dcf31 authored by Emmanuel PIGNAT's avatar Emmanuel PIGNAT
Browse files

merging with master

parents bb636408 346c5930
......@@ -20,7 +20,7 @@ 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-python.git
git clone https://gitlab.idiap.ch/rli/pbdlib-python.git
cd pbdlib-python
pip install -e .
......@@ -44,6 +44,12 @@ Then navigate through folders and click on desired notebook.
## 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
......@@ -55,6 +61,3 @@ To record demos with additional moving objects, run
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.
%% Cell type:code id: tags:
``` python
import numpy as np
import matplotlib.pyplot as plt
import pbdlib as pbd
import os
%matplotlib inline
%load_ext autoreload
%autoreload 2
```
%% Output
The autoreload extension is already loaded. To reload it, use:
%reload_ext autoreload
%% Cell type:markdown id: tags:
### Read data from file
%% Cell type:code id: tags:
``` python
filename = os.path.dirname(pbd.__file__) + '/data/gui/' + 'test_conditioning_002.npy'
data = np.load(filename)[()]
x = np.concatenate(data['x'], axis=0)[::3]
```
%% Cell type:code id: tags:
``` python
plt.plot(x[:, 0], x[:, 1], 'kx')
plt.axes().set_aspect('equal')
```
%% Output
%% Cell type:markdown id: tags:
### Learning a joint distribution
%% Cell type:code id: tags:
``` python
gmm = pbd.GMM(nb_dim=2, nb_states=6)
gmm.em(x, reg=0.1);
# based on sklearn.mixture.BayesianGaussianMixture
bgmm = pbd.VBayesianGMM({'n_components':15, 'n_init':5, 'reg_covar': 0.1 ** 2,
'covariance_prior': 10. ** 2 * np.eye(2),'mean_precision_prior':1e-9})
bgmm.posterior(x);
```
%% Cell type:code id: tags:
``` python
plt.plot(x[:, 0], x[:, 1], 'kx')
gmm.plot(color='steelblue')
bgmm.plot(color='gold')
plt.axes().set_aspect('equal')
```
%% Output
%% Cell type:code id: tags:
``` python
x_in = np.linspace(-200, 200, 300)[:, None]
mu, sigma = gmm.condition(x_in, slice(0, 1), slice(1, 2))
bmu, bsigma = bgmm.condition(x_in, slice(0, 1), slice(1, 2))
```
%% Cell type:code id: tags:
``` python
plt.figure(figsize=(20, 5))
gen.plot(color='orangered', alpha=0.2)
gmm.plot(color='steelblue')
plt.plot(x_in, mu, label='predictive distribution - frequentist', color='steelblue')
plt.fill_between(x_in[:, 0],
mu[:, 0] - sigma[:, 0, 0]**0.5,
mu[:, 0] + sigma[:, 0, 0]**0.5,
alpha=0.3, color='steelblue')
plt.fill_between(x_in[:, 0],
bmu[:, 0] - bsigma[:, 0, 0]**0.5,
bmu[:, 0] + bsigma[:, 0, 0]**0.5,
alpha=0.3, color='gold')
plt.plot(x_in, bmu, label='posterior predictive distribution', color='gold')
plt.plot(x[:, 0], x[:, 1], 'kx')
plt.legend()
```
%% Output
<matplotlib.legend.Legend at 0x7fdb85579410>
%% Cell type:code id: tags:
``` python
```
%% Cell type:code id: tags:
``` python
```
......
%% Cell type:markdown id: tags:
$
\newcommand{\Tau}{\mathcal{T}}
\newcommand{\bm}[1]{{\boldsymbol{#1}}}
\newcommand{\dt}[1]{{\frac{d#1}{dt}}}
%\newcommand{\bm}{\mathbf{#1}}
\newcommand{\trsp}{{\scriptscriptstyle\top}}$
%% Cell type:code id: tags:
``` python
import os
import numpy as np
import matplotlib.pyplot as plt
import pbdlib as pbd
%matplotlib inline
%load_ext autoreload
%autoreload 2
```
%% Cell type:markdown id: tags:
# Hidden Markov Model and LQR
This is an example of learning a HMM over some trajectories demonstrations and reproducing it using optimal control.
More infos : http://calinon.ch/papers/Calinon-JIST2015.pdf, http://calinon.ch/papers/Calinon-HFR2016.pdf
%% Cell type:markdown id: tags:
## Learning Hidden Markov Model (HMM)
%% Cell type:code id: tags:
``` python
from scipy.io import loadmat # loading data from matlab
letter = 'C' # choose a letter in the alphabet
datapath = os.path.dirname(pbd.__file__) + '/data/2Dletters/'
data = loadmat(datapath + '%s.mat' % letter)
demos_x = [d['pos'][0][0].T for d in data['demos'][0]] # Position data
demos_dx = [d['vel'][0][0].T for d in data['demos'][0]] # Velocity data
demos_xdx = [np.hstack([_x, _dx]) for _x ,_dx in zip(demos_x, demos_dx)] # Position-velocity
```
%% Cell type:code id: tags:
``` python
model = pbd.HMM(nb_states=4, nb_dim=4)
model.init_hmm_kbins(demos_xdx) # initializing model
# EM to train model
model.em(demos_xdx, reg=1e-3)
# plotting
fig, ax = plt.subplots(ncols=3)
fig.set_size_inches(12,3.5)
# position plotting
ax[0].set_title('pos')
for p in demos_x:
ax[0].plot(p[:, 0], p[:, 1])
pbd.plot_gmm(model.mu, model.sigma, ax=ax[0], dim=[0, 1]);
# velocity plotting
ax[1].set_title('vel')
for p in demos_dx:
ax[1].plot(p[:, 0], p[:, 1])
pbd.plot_gmm(model.mu, model.sigma, ax=ax[1], dim=[2, 3]);
# plotting transition matrix
ax[2].set_title('transition')
ax[2].imshow(np.log(model.Trans+1e-10), interpolation='nearest', vmin=-5, cmap='viridis');
plt.tight_layout()
```
%% Output
EM did not converge
%% Cell type:markdown id: tags:
# Reproduction (LQR)
Using Product of Gaussian formulation with augmented transfer matrices see : http://calinon.ch/papers/Calinon-HFR2016.pdf
%% Cell type:markdown id: tags:
### Get sequence of states
%% Cell type:code id: tags:
``` python
demo_idx = 0
sq = model.viterbi(demos_xdx[demo_idx])
plt.figure(figsize=(5, 1))
# plt.axis('off')
plt.plot(sq, lw=3);
plt.xlabel('timestep');
```
%% Output
/home/pignate/code/pbdlib/public/pbdlib/pbdlib/hmm.py:95: RuntimeWarning: divide by zero encountered in log
logDELTA[:, 0] = np.log(self.init_priors + realmin * reg) + logB[:, 0]
/home/pignate/code/pbdlib/public/pbdlib/pbdlib/hmm.py:100: RuntimeWarning: divide by zero encountered in log
PSI[i, t] = np.argmax(logDELTA[:, t - 1] + np.log(self.Trans[:, i] + realmin * reg))
/home/pignate/code/pbdlib/public/pbdlib/pbdlib/hmm.py:101: RuntimeWarning: divide by zero encountered in log
logDELTA[i, t] = np.max(logDELTA[:, t - 1] + np.log(self.Trans[:, i] + realmin * reg)) + logB[i, t]
%% Cell type:markdown id: tags:
## Create and solve LQR
%% Cell type:code id: tags:
``` python
lqr = pbd.PoGLQR(nb_dim=2, dt=0.01, horizon=demos_xdx[demo_idx].shape[0])
lqr.mvn_xi = pbd.MVN(mu=, lmbda=)
lqr.mvn_xi = model.concatenate_gaussian(sq)
lqr.mvn_u = -4.
lqr.x0 = demos_xdx[demo_idx][0]
xi = lqr.seq_xi
```
%% Cell type:markdown id: tags:
## Plotting reproduced trajectory (position and velocity)
%% Cell type:code id: tags:
``` python
fig, ax = plt.subplots(ncols=2)
fig.set_size_inches(8,3.5)
# position plotting
ax[0].set_title('position')
for p in demos_x:
ax[0].plot(p[:, 0], p[:, 1], alpha=0.4)
ax[0].plot(xi[:, 0], xi[:, 1], 'b', lw=3)
pbd.plot_gmm(model.mu, model.sigma, ax=ax[0], dim=[0, 1]);
# velocity plotting
ax[1].set_title('velocity')
for p in demos_dx:
ax[1].plot(p[:, 0], p[:, 1], alpha=0.4)
ax[1].plot(xi[:, 2], xi[:, 3], 'b', lw=3, label='repro')
plt.legend()
pbd.plot_gmm(model.mu, model.sigma, ax=ax[1], dim=[2, 3]);
```
%% Output
%% Cell type:code id: tags:
``` python
```
......
......@@ -8,10 +8,8 @@ from .model import Model
from .mvn import *
from .plot import *
from .pylqr import *
from .poglqr import PoGLQR, SparsePoGLQR
from .poglqr import PoGLQR
from .poglqr import PoGLQR, SparsePoGLQR, LQR
from .mtmm import MTMM, VBayesianGMM, VMBayesianGMM
from .lqr import LQR
try:
import gui
......
......@@ -2,9 +2,9 @@ import numpy as np
from model import *
from functions import multi_variate_normal
from scipy.linalg import block_diag
import scipy.sparse as ss
from termcolor import colored
from mvn import MVN, SparseMVN
from mvn import MVN
class GMM(Model):
......@@ -136,7 +136,7 @@ class GMM(Model):
return gmm
def concatenate_gaussian(self, q, get_mvn=True, sparse=False):
def concatenate_gaussian(self, q, get_mvn=True, reg=None):
"""
Get a concatenated-block-diagonal replication of the GMM with sequence of state
given by q.
......@@ -147,21 +147,29 @@ class GMM(Model):
:return:
"""
if not get_mvn:
return np.concatenate([self.mu[i] for i in q]), block_diag(*[self.sigma[i] for i in q])
else:
if not sparse:
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])
else:
mvn = MVN()
mvn.mu = np.concatenate([self.mu[i] for i in q])
mvn._sigma = block_diag(*[self.sigma[i] for i in q])
mvn._lmbda = block_diag(*[self.lmbda[i] for i in q])
else:
mvn = SparseMVN()
return mvn
else:
if not get_mvn:
return np.concatenate([self.mu[i] for i in q]), block_diag(
*[self.sigma[i] + reg for i in q])
else:
mvn = MVN()
mvn.mu = np.concatenate([self.mu[i] for i in q])
mvn._sigma = ss.block_diag([self.sigma[i] for i in q])
mvn._lmbda = ss.block_diag([self.lmbda[i] for i in q])
return mvn
mvn._sigma = block_diag(*[self.sigma[i] + reg for i in q])
mvn._lmbda = block_diag(*[np.linalg.inv(self.sigma[i] + reg) for i in q])
return mvn
def compute_resp(self, demo=None, dep=None, table=None, marginal=None, norm=True):
sample_size = demo.shape[0]
......
......@@ -4,7 +4,14 @@ import os
import matplotlib.pyplot as plt
from pbdlib.gui import Interactive
from termcolor import colored
import tkinter as tk
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
......
import numpy as np
from utils.utils import lifted_transfer_matrix
import pbdlib as pbd
import scipy.sparse as ss
class PoGLQR(object):
class LQR(object):
def __init__(self, A=None, B=None, nb_dim=2, dt=0.01, horizon=50):
self._horizon = horizon
self.A = A
self.B = B
self.dt = dt
self.nb_dim = nb_dim
self._s_xi, self._s_u = None, None
self._x0 = None
self._gmm_xi, self._gmm_u = None, None
self._mvn_sol_xi, self._mvn_sol_u = None, None
self._seq_xi, self._seq_u = None, None
@property
def horizon(self):
return self._horizon
@horizon.setter
def horizon(self, value):
self.reset_params()
self._horizon = value
@property
def u_dim(self):
"""
Number of dimension of input
:return:
"""
if self.B is not None:
return self.B.shape[1]
else:
return self.nb_dim
@property
def xi_dim(self):
"""
Number of dimension of state
:return:
"""
if self.A is not None:
return self.A.shape[0]
else:
return self.nb_dim * 2
@property
def gmm_xi(self):
"""
Distribution of state
:return:
"""
return self._gmm_xi
@gmm_xi.setter
def gmm_xi(self, value):
"""
:param value [pbd.GMM] or [(pbd.GMM, list)]
"""
# resetting solution
self._mvn_sol_xi = None
self._mvn_sol_u = None
self._seq_u = None
self._seq_xi = None
self._gmm_xi = value
@property
def gmm_u(self):
"""
Distribution of control input
:return:
"""
return self._gmm_u
@gmm_u.setter
def gmm_u(self, value):
"""
:param value [float] or [pbd.MVN] or [pbd.GMM] or [(pbd.GMM, list)]
"""
# resetting solution
self._mvn_sol_xi = None
self._mvn_sol_u = None
self._seq_u = None
self._seq_xi = None
if isinstance(value, float):
self._gmm_u = pbd.MVN(
mu=np.zeros(self.u_dim), lmbda=10 ** value * np.eye(self.u_dim))
else:
self._gmm_u = value
@property
def x0(self):
return self._x0
@x0.setter
def x0(self, value):
# resetting solution
self._mvn_sol_xi = None
self._mvn_sol_u = None
self._x0 = value
def get_Q_z(self, t):
"""
get Q and target z for time t
:param t:
:return:
"""
if isinstance(self._gmm_xi, tuple):
gmm, seq = self._gmm_xi
return gmm.lmbda[seq[t]], gmm.mu[seq[t]]
elif isinstance(self._gmm_xi, pbd.GMM):
return self._gmm_xi.lmbda[t], self._gmm_xi.mu[t]
elif isinstance(self._gmm_xi, pbd.MVN):
return self._gmm_xi.lmbda, self._gmm_xi.mu
else:
raise ValueError, "Not supported gmm_xi"
def get_R(self, t):
if isinstance(self._gmm_u, pbd.MVN):
return self._gmm_u.lmbda
elif isinstance(self._gmm_u, tuple):
gmm, seq = self._gmm_u
return gmm.lmbda[seq[t]], gmm.mu[seq[t]]
elif isinstance(self._gmm_u, pbd.GMM):
return self._gmm_u.lmbda[t], self._gmm_u.mu[t]
else:
raise ValueError, "Not supported gmm_u"
def ricatti(self):
"""
http://web.mst.edu/~bohner/papers/tlqtots.pdf
:return:
"""
Q, z = self.get_Q_z(-1)
#
_S = [None for i in range(self._horizon)]
_v = [None for i in range(self._horizon)]
_K = [None for i in range(self._horizon-1)]
_Kv = [None for i in range(self._horizon-1)]
# _S = np.empty((self._horizon, self.xi_dim, self.xi_dim))
# _v = np.empty((self._horizon, self.xi_dim))
# _K = np.empty((self._horizon-1, self.u_dim, self.xi_dim))
# _Kv = np.empty((self._horizon-1, self.u_dim, self.xi_dim))
_S[-1] = Q
_v[-1] = Q.dot(z)
for t in range(self.horizon-2, -1, -1):
Q, z = self.get_Q_z(t)
R = self.get_R(t)
_Kv[t] = np.linalg.inv(R + self.B.T.dot(_S[t+1]).dot(self.B)).dot(self.B.T)
_K[t] = _Kv[t].dot(_S[t+1]).dot(self.A)
AmBK = self.A - self.B.dot(_K[t])