}, {
"type": "poly",
"nparams": 0,
"conf": {
"order": 1
}
}],
'params': [np.log(0.25), 0.25, 0.5, 0.75]
}
robot_promp = promp.FullProMP(basis=full_basis)
#3) Train ProMP with NIW prior on the covariance matrix (as described in the paper)
dof = 7
dim_basis_fun = 5
inv_whis_mean = lambda v, Sigma: utils.make_block_diag(Sigma, dof)
prior_Sigma_w = {'v': dim_basis_fun * dof, 'mean_cov_mle': inv_whis_mean}
train_summary = robot_promp.train(time,
q=Q,
max_iter=10,
prior_Sigma_w=prior_Sigma_w,
print_inner_lb=True)
#4) Plot some samples from the learned ProMP and conditioned ProMP
n_samples = 5 # Number of samples to draw
plot_dof = 3 # Degree of freedom to plot
sample_time = [np.linspace(0, 1, 200) for i in range(n_samples)]
#4.1) Make some samples from the unconditioned ProMP
promp_samples = robot_promp.sample(sample_time)
}, {
"type": "poly",
"nparams": 0,
"conf": {
"order": 1
}
}],
'params': [np.log(0.25), 0.25, 0.5, 0.75]
}
robot_promp = promp.FullProMP(basis=full_basis)
#3) Train ProMP with NIW prior on the covariance matrix (as described in the paper)
dof = 7
dim_basis_fun = 5
inv_whis_mean = lambda v, Sigma: 9e-1 * utils.make_block_diag(
Sigma, dof) + 1e-1 * np.eye(dof * dim_basis_fun)
prior_Sigma_w = {'v': dim_basis_fun * dof, 'mean_cov_mle': inv_whis_mean}
train_summary = robot_promp.train(time,
q=Q,
max_iter=10,
prior_Sigma_w=prior_Sigma_w,
print_inner_lb=True)
#4) Plot some samples from the learned ProMP and conditioned ProMP
n_samples = 5 # Number of samples to draw
plot_dof = 3 # Degree of freedom to plot
sample_time = [np.linspace(0, 1, 200) for i in range(n_samples)]
#4.1) Make some samples from the unconditioned ProMP
promp_samples = robot_promp.sample(sample_time)
def __M_step(self, times, Y, expectations, **args):
Phi = self.__get_Phi(times, **args)
N = len(times)
w_means = expectations['w_means']
w_covs = expectations['w_covs']
n_var = lambda X: sum(map(lambda x: np.outer(x, x), X))
#1) Optimize mu_w
wn_sum = sum(w_means)
if 'prior_mu_w' in args:
prior_mu_w = args['prior_mu_w']
mu_w = (wn_sum + prior_mu_w['k0'] * prior_mu_w['m0']) / (
N + prior_mu_w['k0'])
else:
mu_w = (wn_sum) / N
#2) Optimize Sigma_w
diff_w = map(lambda x: x - mu_w, w_means)
if 'no_Sw' in args and args['no_Sw'] == True:
sw_sum = utils.force_sym(n_var(diff_w))
else:
sw_sum = utils.force_sym(sum(w_covs) + n_var(diff_w))
self.__Sigma_w_mle = sw_sum / N # Maximum likelyhood estimate for Sigma_w
if 'prior_Sigma_w' in args:
prior_Sigma_w = args['prior_Sigma_w']
v0 = prior_Sigma_w['v']
D = np.shape(self.Sigma_w)[0]
if 'mean_cov_mle' in prior_Sigma_w:
S0 = prior_Sigma_w['mean_cov_mle'](
v0, self.__Sigma_w_mle) * (v0 + D + 1)
else:
S0 = prior_Sigma_w['invS0']
Sigma_w = (S0 + sw_sum) / (N + v0 + D + 1)
else:
Sigma_w = self.__Sigma_w_mle
#3) Optimize Sigma_y
diff_y = []
uncert_w_y = []
for n in xrange(N):
for t in xrange(len(times[n])):
diff_y.append(Y[n][t] - np.dot(Phi[n][t], w_means[n]))
uncert_w_y.append(
np.dot(np.dot(Phi[n][t], w_covs[n]), Phi[n][t].T))
if 'no_Sw' in args and args['no_Sw'] == True:
Sigma_y = (n_var(diff_y)) / len(diff_y)
else:
Sigma_y = (n_var(diff_y) + sum(uncert_w_y)) / len(diff_y)
#4) Update
self.mu_w = mu_w
if args['print_inner_lb']:
print 'lb(mu_w)=', self.__EM_lowerbound(times, Y, expectations,
**args)
self.Sigma_w = utils.force_sym(Sigma_w)
if args['joint_indep']:
self.Sigma_w = utils.make_block_diag(self.Sigma_w,
args['num_joints'])
if args['print_inner_lb']:
print 'lb(Sigma_w)=', self.__EM_lowerbound(times, Y, expectations,
**args)
if args['diag_sy']:
self.Sigma_y = np.diag(np.diag(Sigma_y))
else:
self.Sigma_y = utils.force_sym(Sigma_y)
if args['print_inner_lb']:
print 'lb(Sigma_y)=', self.__EM_lowerbound(times, Y, expectations,
**args)
#5) Update optional parameters
if args['opt_basis_pars']:
obj = lambda pars: -self.__em_lb_likelihood(times, Y, expectations, mu_w=mu_w, \
Sigma_w=Sigma_w, Sigma_y=Sigma_y, basis_params=pars, q=True)
obj_debug = lambda x: lambda_debug(obj, x, "cost")
jac = autograd.grad(obj)
#print "Objective at x0: ", obj(self.get_basis_pars())
#print "Gradient at x0: ", jac(self.get_basis_pars())
#o_basis_pars = opt.minimize(lambda x: lambda_debug(obj,x,"cost"), self.get_basis_pars(), method="CG", jac=lambda x: lambda_debug(jac,x,"grad"))
o_basis_pars = opt.minimize(obj,
self.get_basis_pars(),
method="Powell")
#o_basis_pars = opt.minimize(obj, self.get_basis_pars(), method="Nelder-Mead")
if o_basis_pars.success:
self.set_basis_pars(o_basis_pars.x)
else:
print "Warning: The optimization of the basis parameters failed. Message: ", o_basis_pars.message
if args['print_inner_lb']:
print 'lb(basis_params)=', self.__EM_lowerbound(
times, Y, expectations, **args)
def main(args):
logging.basicConfig(filename='/tmp/promp_train.log', level=logging.DEBUG)
with open(args.data, 'r') as f:
data = np.load(args.data)
time = data['time']
Q = data['Q']
Qdot = data['Qdot']
durations = [t[-1] - t[0] for t in time]
dm = np.mean(durations)
dstd = np.std(durations)
print("Durations {} +/- {}".format(dm, dstd))
if args.trajlim:
time = time[0:args.trajlim]
Q = Q[0:args.trajlim]
Qdot = Qdot[0:args.trajlim]
full_basis = {
'conf': [{
"type": "sqexp",
"nparams": 4,
"conf": {
"dim": 3
}
}, {
"type": "poly",
"nparams": 0,
"conf": {
"order": 1
}
}],
'params': [np.log(0.25), 0.25, 0.5, 0.75]
}
dof = np.shape(Q[0])[1]
dim_basis_fun = promp.dim_comb_basis(**full_basis)
w_dim = dof * dim_basis_fun
inv_whis_mean = lambda v, Sigma: utils.make_block_diag(
Sigma, dof) + args.pdiag_sw * np.eye(w_dim)
params = {
'new_kernel': full_basis,
#'prior_mu_w': {"m0": np.zeros(5*7), "k0": 1},
'model_fname': "/tmp/promp.json",
'diag_sy': not args.full_sy,
'joint_indep': args.joint_indep,
'use_velocity': args.use_vel,
'opt_basis_pars': False,
'print_inner_lb': False,
'no_Sw': False, #Approx E-Step with Dirac delta?
'num_joints': dof,
'max_iter': args.training_iter,
'save_lh_plot': args.save_lh_plot,
'init_params': {
'mu_w': np.zeros(w_dim),
'Sigma_w': 1e8 * np.eye(w_dim),
'Sigma_y': np.eye(dof)
},
'print_inner_lb': False
}
if args.init_promp:
params['init_promp'] = args.init_promp
if not args.max_lh:
params['prior_Sigma_w'] = {
'v': dim_basis_fun * dof,
'mean_cov_mle': inv_whis_mean
}
if args.use_vel:
model = trainFullPromp(time, Q, Qdot, **params)
else:
model = trainFullPromp(time, Q, **params)