def solve_lqr(letter='C', dt=0.01):
demos_x, demos_dx, demos_xdx, model = hmm(letter=letter, gui=False)
demo_idx = 3
sq = model.viterbi(demos_xdx[demo_idx])
lqr = pbd.PoGLQR(nb_dim=2, dt=dt, horizon=demos_xdx[demo_idx].shape[0])
lqr.mvn_xi = model.concatenate_gaussian(sq)
lqr.mvn_u = -4.
lqr.x0 = demos_xdx[demo_idx][0]
xi = lqr.seq_xi
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]);
plt.show()
def augment_data():
demos_x, demos_dx, demos_xdx = load_sample_data()
demos_x, demos_dx, demos_xdx = pbd.utils.align_trajectories(demos_x, [demos_dx, demos_xdx])
t = np.linspace(0, 100, demos_x[0].shape[0])
demos = [np.hstack([t[:,None], d]) for d in demos_xdx]
data = np.vstack([d for d in demos])
model = pbd.GMM(nb_states=4, nb_dim=5)
model.init_hmm_kbins(demos) # initializing model
# EM to train model
model.em(data, reg=[0.1, 1., 1., 1., 1.])
# plotting
fig, ax = plt.subplots(nrows=4)
fig.set_size_inches(12,7.5)
# position plotting
for i in range(4):
for p in demos:
ax[i].plot(p[:, 0], p[:, i + 1])
pbd.plot_gmm(model.mu, model.sigma, ax=ax[i], dim=[0, i + 1]);
plt.show()
def test_HMM(letter='C', score=False, nb_states=4):
if letter.isalpha() == 1:
print('load alphabet')
demos = import_data(letter)
else:
print('load from path')
demos = import_data_from_path(letter)
# 3 dimentionalize
demos = [
np.concatenate([demo, np.zeros((demo.shape[0], 1))], axis=1)
for demo in demos
]
model = pbd.HMM(nb_states=nb_states, nb_dim=2)
model.init_hmm_kbins(demos) # initializing model
# plotting
fig, ax = plt.subplots(ncols=2)
fig.set_size_inches(5, 2.5)
if model.mu.shape[1] == 2:
pbd.plot_gmm(model.mu,
model.sigma,
alpha=0.5,
color='steelblue',
ax=ax[0])
# plot after init only
else:
pbd.plot_gmm(model.mu[:, :2],
model.sigma[:, :2, :2],
alpha=0.5,
color='steelblue',
ax=ax[0])
# plot after init only
# EM to train model
model.em(demos, reg=1e-3)
# plotting demos
for p in demos:
ax[0].plot(p[:, 0], p[:, 1])
if model.mu.shape[1] == 2:
pbd.plot_gmm(model.mu, model.sigma, ax=ax[0])
else:
pbd.plot_gmm(model.mu[:, :2], model.sigma[:, :2, :2], ax=ax[0])
if score:
print(model.score(demos))
# plotting transition matrix
ax[1].imshow(np.log(model.Trans + 1e-10),
interpolation='nearest',
vmin=-5,
cmap='viridis')
plt.tight_layout()
plt.show()
def hmm(letter='C', gui=True):
if letter.isalpha() == 1:
print('load alphabet')
demos_x, demos_dx, demos_xdx = import_data(letter)
else:
print('load from path')
demos_x, demos_dx, demos_xdx = import_data_from_path(letter)
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()
if gui:
plt.show()
return demos_x, demos_dx, demos_xdx, model
def learn_model(demos_xdx_augm=None, demos_xdx_f=None):
if demos_xdx_augm is None or demos_xdx_f is None:
_, _, _, _, _, demos_xdx_augm, demos_xdx_f = load_sample_data()
model = pbd.HMM(nb_states=4, nb_dim=8)
model.init_hmm_kbins(demos_xdx_augm) # initializing model
# EM to train model
model.em(demos_xdx_augm, reg=1e-3)
# plotting
fig, ax = plt.subplots(ncols=2, nrows=2)
fig.set_size_inches(8, 8)
for j in range(2):
# position plotting
ax[j, 0].set_title('pos - coord. %d' % j)
for p in demos_xdx_f:
ax[j, 0].plot(p[:, j, 0], p[:, j, 1])
pbd.plot_gmm(model.mu,
model.sigma,
ax=ax[j, 0],
dim=[0 + j * 4, 1 + j * 4],
color='orangered')
# velocity plotting
ax[j, 1].set_title('vel - coord. %d' % j)
for p in demos_xdx_f:
ax[j, 1].plot(p[:, j, 2], p[:, j, 3])
pbd.plot_gmm(model.mu,
model.sigma,
ax=ax[j, 1],
dim=[2 + j * 4, 3 + j * 4],
color='orangered')
plt.tight_layout()
plt.show()
return model
def synthesis():
demos_x, demos_dx, demos_xdx = load_sample_data()
demos_x, demos_dx, demos_xdx = pbd.utils.align_trajectories(demos_x, [demos_dx, demos_xdx])
t = np.linspace(0, 100, demos_x[0].shape[0])
demos = [np.hstack([t[:,None], d]) for d in demos_xdx]
data = np.vstack([d for d in demos])
model = pbd.GMM(nb_states=4, nb_dim=5)
model.init_hmm_kbins(demos) # initializing model
# EM to train model
model.em(data, reg=[0.1, 1., 1., 1., 1.])
mu, sigma = model.condition(t[:, None], dim_in=slice(0, 1), dim_out=slice(1, 5))
pbd.plot_gmm(mu, sigma, dim=[0, 1], color='orangered', alpha=0.3)
for d in demos_x:
plt.plot(d[:, 0], d[:, 1])
plt.show()
# EM to train model
gmm.em(np.concatenate(demos), reg=1e-3)
hmm.em(demos, reg=1e-3)
hsmm.em(demos, reg=1e-3)
# plotting demos
fig, ax = plt.subplots(ncols=2)
fig.set_size_inches(5., 2.5)
for p_in, p_out in zip(demos_in, demos_out):
ax[0].plot(p_in[:, 0], p_in[:, 1])
ax[1].plot(p_out[:, 0], p_out[:, 1])
# use dim for selecting dimensions of GMM to plot
pbd.plot_gmm(gmm.mu, gmm.sigma, ax=ax[0], dim=[0, 1])
pbd.plot_gmm(gmm.mu, gmm.sigma, ax=ax[1], dim=[2, 3])
plt.show()
n = 0
resp_gmm = gmm.compute_resp(demos_in[n][:10], marginal=slice(0, 2))
alpha_hsmm, _, _, _, _ = hsmm.compute_messages(demos_in[n][:10],
marginal=slice(0, 2))
alpha_hmm, _, _, _, _ = hmm.compute_messages(demos_in[n][:10],
marginal=slice(0, 2))
fig, ax = plt.subplots(nrows=3)
fig.set_size_inches(7.5, 3.6)
def reproduction():
demos_x, demos_dx, demos_xdx, demos_A_xdx, demos_b_xdx, demos_xdx_augm, demos_xdx_f = load_sample_data(
)
model = learn_model(demos_xdx_augm, demos_xdx_f)
demo_idx = 0
nbcol = 5
fig, ax = plt.subplots(ncols=nbcol,
nrows=np.ceil(float(len(demos_x)) / nbcol).astype(
np.int))
fig.set_size_inches(14, 3 * ax.shape[0])
ax = ax.reshape(-1)
for i in range(len(demos_x)):
_A, _b = demos_A_xdx[i][0], demos_b_xdx[i][0]
_mod1 = model.marginal_model(slice(0, 4)).lintrans(_A[0], _b[0])
_mod2 = model.marginal_model(slice(4, 8)).lintrans(_A[1], _b[1])
# product
_prod = _mod1 * _mod2
# get the most probable sequence of state for this demonstration
sq = model.viterbi(demos_xdx_augm[i])
# solving LQR with Product of Gaussian, see notebook on LQR
lqr = pbd.PoGLQR(nb_dim=2, dt=0.05, horizon=demos_xdx[i].shape[0])
lqr.mvn_xi = _prod.concatenate_gaussian(
sq) # augmented version of gaussian
lqr.mvn_u = -4.
lqr.x0 = demos_xdx[demo_idx][0]
xi = lqr.seq_xi
ax[i].plot(xi[:, 0], xi[:, 1], color='r', lw=2)
pbd.plot_gmm(_mod1.mu,
_mod1.sigma,
swap=True,
ax=ax[i],
dim=[0, 1],
color='steelblue',
alpha=0.3)
pbd.plot_gmm(_mod2.mu,
_mod2.sigma,
swap=True,
ax=ax[i],
dim=[0, 1],
color='orangered',
alpha=0.3)
pbd.plot_gmm(_prod.mu,
_prod.sigma,
swap=True,
ax=ax[i],
dim=[0, 1],
color='gold')
ax[i].plot(demos_x[i][:, 0], demos_x[i][:, 1], 'k--', lw=2)
demo_idx += 1
plt.tight_layout()
plt.show()
def product():
_, _, _, _, _, demos_xdx_augm, demos_xdx_f = load_sample_data()
model = learn_model(demos_xdx_augm, demos_xdx_f)
import matplotlib.patches as mpatches
demo_idx = 4
# get transformation for given demonstration.
# We use the transformation of the first timestep as they are constant
A, b = demos_A_xdx[demo_idx][0], demos_b_xdx[demo_idx][0]
# transformed model for coordinate system 1
mod1 = model.marginal_model(slice(0, 4)).lintrans(A[0], b[0])
# transformed model for coordinate system 2
mod2 = model.marginal_model(slice(4, 8)).lintrans(A[1], b[1])
# product
prod = mod1 * mod2
fig, ax = plt.subplots(ncols=3)
fig.set_size_inches((12, 3))
for a in ax:
a.set_aspect('equal')
ax[0].set_title('model 1')
pbd.plot_gmm(model.mu,
model.sigma,
swap=True,
ax=ax[0],
dim=[0, 1],
color='steelblue',
alpha=0.3)
ax[1].set_title('model 2')
pbd.plot_gmm(model.mu,
model.sigma,
swap=True,
ax=ax[1],
dim=[4, 5],
color='orangered',
alpha=0.3)
ax[2].set_title('tranformed models and product')
pbd.plot_gmm(mod1.mu,
mod1.sigma,
swap=True,
ax=ax[2],
dim=[0, 1],
color='steelblue',
alpha=0.3)
pbd.plot_gmm(mod2.mu,
mod2.sigma,
swap=True,
ax=ax[2],
dim=[0, 1],
color='orangered',
alpha=0.3)
pbd.plot_gmm(prod.mu,
prod.sigma,
swap=True,
ax=ax[2],
dim=[0, 1],
color='gold')
patches = [
mpatches.Patch(color='steelblue', label='transformed model 1'),
mpatches.Patch(color='orangered', label='transformed model 2'),
mpatches.Patch(color='gold', label='product')
]
plt.legend(handles=patches,
bbox_to_anchor=(1.05, 1),
loc=2,
borderaxespad=0.)
plt.show()