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 cleanFromGripperShit(data):
data_out = []
index = [15, 16, 32, 33]
for i, traj in enumerate(data):
traj_out = []
for j, q in enumerate(traj):
q_done = np.delete(q, index)
traj_out.append(q_done)
data_out.append(np.array(traj_out))
return data_out
#data = cleanFromGripperShit(data)
#%%fasdf
import pbdlib as pbd
#%%
model = pbd.HMM(nb_states=4, nb_dim=14)
#%%
model.init_hmm_kbins(data)
#%%
model.em(data, obs_fixed=True, left_to_right=False)
#%%
np.set_printoptions(precision=4)
for i in range(100):
msg = model.predict(data[1][i][0:7], i)
print("pr: ", msg)
print("gt: ", data[1][i][7:])
print('\n')
#%%
#%%
#%%
def cleanFromGripperShit(data):
data_out = []
index = [15, 16, 32, 33]
for i, traj in enumerate(data):
traj_out = []
for j, q in enumerate(traj):
q_done = np.delete(q, index)
traj_out.append(q_done)
data_out.append(np.array(traj_out))
return data_out
data = cleanFromGripperShit(data)
#%%
import pbdlib as pbd
#%%
model = pbd.HMM(nb_states=7, nb_dim=30)
gmr = pbd.GMR
#%%
model.init_hmm_kbins(data)
#%%
model.em(data, reg=1e-8)
#%%
robot.goHome(hard=True)
for i in range(100):
time.sleep(1)
q = robot.C.getJointState()
q = np.delete(q, [15, 16])
q_dot = model.predict(q, i)
print(q_dot)
# add the folder where libry.so is located to the path. Otherwise the import will crash.
sys.path.append('../robotics-course/ry/')
import libry as ry
import time
os.getcwd()
# load names of joints to map from ros 2 rai joints. Only relevant joints (of right arm) are loaded
with open('bagfiles/names.pkl', 'rb') as f:
names = pickle.load(f)
# load demonstration dataset type: List of np.array() with shape=(np_steps_in_trajectory, 2*dim(q))
with open('bagfiles/dataset.pkl', 'rb') as f:
demos = pickle.load(f)
# initialize HMM model and train using expectation-maximization (already implemented in https://gitlab.idiap.ch/rli/pbdlib-python)
model = pbd.HMM(4, 14)
model.init_hmm_kbins(demos)
model.em(demos, left_to_right=True)
# clear views, config and operate by setting shared pointers to 0. Otherwise the notebook has to be restarted,
# which is pretty annoying.
C = 0
v = 0
B = 0
# initialize config
C = ry.Config()
v = C.view()
C.clear()
C.addFile('../robotics-course/rai-robotModels/baxter/baxter_new.g')
vel = msg.velocity[9:16]
tmp.append(np.concatenate([np.array(pos), np.array(vel)]))
demos.append(np.array(tmp))
#%%
len(demos[3])
#%%
import pickle
#%%
#with open("/home/niklas/git/uni/imitation-learning/bagfiles/dataset.pkl", "wb") as f:
# pickle.dump(demos, f, pickle.HIGHEST_PROTOCOL)
#%%
import pbdlib as pbd
#%%
model = pbd.HMM(7, 14)
#%%
model.init_hmm_kbins(demos)
#%%
model.em(demos, obs_fixed=True, left_to_right=False)
#%%
np.set_printoptions(precision=6)
#%%
#model.plot()
#%%
for i in range(600):
msg = model.predict_qdot(demos[1][i][0:7], i)
data_out = loadmat(datapath + '%s.mat' % letter_out)
demos_out = [d['pos'][0][0].T for d in data_out['demos'][0]]
demos_in = [
np.random.multivariate_normal(np.zeros(2), np.eye(2), d.shape[0])
for d in demos_out
]
demos = [
np.concatenate([d_in, d_out], axis=1)
for d_in, d_out in zip(demos_in, demos_out)
]
gmm = pbd.GMM(nb_states=nb_states)
hmm = pbd.HMM(nb_states=nb_states)
hsmm = pbd.HSMM(nb_states=nb_states)
# initializing model
[model.init_hmm_kbins(demos) for model in [gmm, hmm, hsmm]]
# 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):
