def __init__(self, model, outcomes=["stairing", "staired"]):
smach.State.__init__(self, outcomes=outcomes)
rospy.wait_for_service(model.model_name + '_joint_cmd')
self.send = rospy.wait_for_service(model.model_name + '_joint_cmd')
self._model = model
project_root = dirname(dirname(__file__))
fileZ = join(project_root, 'config/toeZ_all.pickle')
fileY = join(project_root, 'config/toeY_all.pickle')
self.runnerZ = GMMRunner.GMMRunner(
fileZ) # make_toeZ([file1, file2], hills3, nb_states, "toe_IK")
self.runnerY = GMMRunner.GMMRunner(
fileY) # make_toeY([file1, file2], hills3, nb_states, "toe_IK")
self.rate = rospy.Rate(10)
self.msg = DesiredJoints()
self.pub = rospy.Publisher("set_points", DesiredJoints, queue_size=1)
self.count = 0
self.init_joint_angles = []
def get_gmm(file_name):
font = {'family': 'normal',
'weight': 'bold',
'size': 30}
matplotlib.rc('font', **font)
nb_states = 10
runner = GMMRunner.GMMRunner(file_name)
fig0, ax = plt.subplots(3,sharex=True)
sIn = runner.get_sIn()
tau = runner.get_tau()
l = runner.get_length()
motion = runner.get_motion()
mu = runner.get_mu()
sigma = runner.get_sigma()
currF = runner.get_expData()
# plot the forcing functions
angles = get_data()
for i in range(len(angles["Lhip"])):
ax[0].plot(sIn, tau[1, i * l: (i + 1) * l].tolist(), color="b")
ax[1].plot(sIn, tau[2, i * l: (i + 1) * l].tolist(), color="b")
ax[2].plot(sIn, tau[3, i * l: (i + 1) * l].tolist(), color="b")
ax[0].plot(sIn, currF[0].tolist(), color="y", linewidth=5)
ax[1].plot(sIn, currF[1].tolist(), color="y", linewidth=5)
ax[2].plot(sIn, currF[2].tolist(), color="y", linewidth=5)
sigma0 = sigma[:, :2, :2]
sigma1 = sigma[:, :3, :2]
sigma2 = sigma[:, :4, :2]
sigma1 = np.delete(sigma1, 1, axis=1)
sigma2 = np.delete(sigma2, 1, axis=1)
sigma2 = np.delete(sigma2, 1, axis=1)
p = plot_gmm(Mu=np.array([mu[0,:], mu[1,:] ]), Sigma=sigma0, ax=ax[0])
p = plot_gmm(Mu=np.array([mu[0, :], mu[2, :]]), Sigma=sigma1, ax=ax[1])
p = plot_gmm(Mu=np.array([mu[0, :], mu[3, :]]), Sigma=sigma2, ax=ax[2])
fig0.suptitle('Forcing Function')
ax[2].set_xlabel('S')
ax[0].set_ylabel('F')
ax[1].set_ylabel('F')
ax[2].set_ylabel('F')
# fig0.tight_layout(pad=1.0, h_pad=0.15, w_pad=None, rect=None)
ax[0].set_title("Left Hip")
ax[1].set_title("Left Knee")
ax[2].set_title("Left Ankle")
plt.show()
matplotlib.rc('font', **font)
nb_states = 10
files = data.files
sides = data.sides
frames = data.frames
hills = utilities.get_index(frames, files, sides)
pathsZ, pathsY = utilities.make_toe(files, hills, sides)
trainer = GMMTrainer.GMMTrainer(demo=pathsZ,
file_name="plotGMM",
n_rf=12,
dt=0.01)
trainer.train()
runner = GMMRunner.GMMRunner("plotGMM.pickle")
fig0, ax0 = plt.subplots(1)
sIn = runner.get_sIn()
tau = runner.get_tau()
l = runner.get_length()
motion = runner.get_motion()
currF = runner.get_expData()[0].tolist()
# plot the forcing functions
for i in range(len(files)):
ax0.plot(sIn, tau[1, i * l:(i + 1) * l].tolist(), color="b")
ax0.plot(sIn, currF, color="y", linewidth=5)
p = plot_gmm(Mu=runner.get_mu()[:2, :],
ax.add_collection(p)
return p
nb_states = 15
files = data.files
sides = data.sides
frames = data.frames
hills = utilities.get_index(frames, files, sides)
pathsZ, pathsY = utilities.make_toe(files, hills, sides)
trainerZ = GMMTrainer.GMMTrainer(pathsZ, "trainZ", 15, 0.01)
trainerZ.train()
runnerZ = GMMRunner.GMMRunner("trainZ.pickle")
trainerY = GMMTrainer.GMMTrainer(pathsY, "trainY", 15, 0.01)
trainerY.train()
runnerY = GMMRunner.GMMRunner("trainY.pickle")
sample_size = 10000
for p in pathsZ:
if len(p) < sample_size:
sample_size = len(p)
fig0, ax0 = plt.subplots(1)
fig1, ax1 = plt.subplots(1)
sIn = runnerZ.get_sIn()
ax.add_collection(p)
return p
nb_states = 15
files = data.files
sides = data.sides
frames = data.frames
hills = utilities.get_index(frames, files, sides)
pathsZ, pathsY = utilities.make_toe(files, hills, sides)
# trainerZ1 = GMMTrainer.GMMTrainer(pathsZ, "trainZ", 15, 0.01)
# trainerZ1.train()
runnerZ1 = GMMRunner.GMMRunner("trainZ.pickle")
#
# trainerY1 = GMMTrainer.GMMTrainer(pathsY, "trainY", 15, 0.01)
# trainerY1.train()
runnerY1 = GMMRunner.GMMRunner("trainY.pickle")
nb_states = 15
files = data.files[0:1]
sides = data.sides[0:1]
frames = data.frames
hills = utilities.get_index(frames, files, sides)
pathsZ, pathsY = utilities.make_toe(files, hills, sides)
# trainerZ2 = GMMTrainer.GMMTrainer(pathsZ, "trainZ_single", 15, 0.01)
# trainerZ2.train()
runnerZ2 = GMMRunner.GMMRunner("trainZ_single.pickle")
pathsZ = make_toeZ(filesZ, hillsZ)
pathsY = make_toeY(filesY, hillsY)
#runner_toeZ = make_toeZ(filesZ, hillsZ, nb_states, "toeZ_all2")
#runner_toeY = make_toeY(filesY, hillsY, nb_states, "toeY_all")
# runnerY = GMMRunner.GMMRunner("toeY_all" + ".pickle")
# runnerZ = GMMRunner.GMMRunner("toeZ_all1" + ".pickle")
#
# imitationY = runnerY.run()
# imitationZ = runnerZ.run()
metricsZ = []
metricsY = []
for i in range(2,25):
runnerZ = GMMRunner.GMMRunner("toeZ_all" + str(i) + ".pickle")
imitationZ = runnerZ.run()
pathZ, metric = calculate_imitation_metric_1(pathsZ, imitationZ)
metricsZ.append( [i, metric])
for i in range(2,25):
runnerY = GMMRunner.GMMRunner("toeY_all" + str(i) + ".pickle")
imitationY = runnerY.run()
pathY, metric = calculate_imitation_metric_1(pathsY, imitationY)
metricsY.append( [i, metric])
plt.rcParams.update({'font.size': 22})
fig2, ax2 = plt.subplots(nrows=1, ncols=2, figsize=(5, 3))
fig1, ax1 = plt.subplots(nrows=2, ncols=1, sharex=True, figsize=(5, 3))
metricsY = np.array(metricsY)