def get_sol_params(self, cld):
self.check_cld(cld)
K = tps_kernel_matrix(cld)
proj_mats = {}
offset_mats = {}
(proj_mats_arr, _), (offset_mats_arr, _) = batch_get_sol_params(cld, K, self.bend_coefs)
for i, b in enumerate(self.bend_coefs):
proj_mats[b] = proj_mats_arr[i]
offset_mats[b] = offset_mats_arr[i]
return proj_mats, offset_mats, K
def get_solver_mats(self, x_nd, rot_coef):
n, d = x_nd.shape
K_nn = tps.tps_kernel_matrix(x_nd)
A = np.r_[np.zeros((d + 1, d + 1)), np.c_[np.ones((n, 1)), x_nd]].T
n_cnts = A.shape[0]
_u, _s, _vh = np.linalg.svd(A.T)
N = _u[:, n_cnts:]
NR = N[1:1 + d, :].T * rot_coef
KN = K_nn.dot(N[1 + d:, :])
QN = np.c_[np.ones((n, 1)), x_nd].dot(N[:1 + d, :]) + KN
NKN = (N[1 + d:, :].T).dot(KN)
NRN = NR.dot(N[1:1 + d, :])
return N, QN, NKN, NRN, NR, K_nn
def get_solver_mats(self, x_nd, rot_coef):
n,d = x_nd.shape
K_nn = tps.tps_kernel_matrix(x_nd)
A = np.r_[np.zeros((d+1,d+1)), np.c_[np.ones((n,1)), x_nd]].T
n_cnts = A.shape[0]
_u,_s,_vh = np.linalg.svd(A.T)
N = _u[:,n_cnts:]
NR = N[1:1+d,:].T * rot_coef
KN = K_nn.dot(N[1+d:,:])
QN = np.c_[np.ones((n, 1)), x_nd].dot(N[:1+d,:]) + KN
NKN = (N[1+d:,:].T).dot(KN)
NRN = NR.dot(N[1:1+d,:])
return N, QN, NKN, NRN, NR, K_nn
def get_solver_mats(self, x_nd, rot_coef):
n, d = x_nd.shape
K_nn = tps.tps_kernel_matrix(x_nd)
A = np.r_[np.zeros((d + 1, d + 1)), np.c_[np.ones((n, 1)), x_nd]].T
n_cnts = A.shape[0]
_u, _s, _vh = np.linalg.svd(A.T)
N = _u[:, n_cnts:].copy()
NR = (N[1:1 + d, :].T * rot_coef).copy() # so that it is c-contiguous
N_gpu = gpuarray.to_gpu(N[1 + d:, :])
K_gpu = gpuarray.to_gpu(K_nn)
KN_gpu = culinalg.dot(K_gpu, N_gpu)
QN = np.c_[np.ones((n, 1)), x_nd].dot(N[:1 + d, :]) + KN_gpu.get()
NKN_gpu = culinalg.dot(N_gpu, KN_gpu, transa='T')
NKN = NKN_gpu.get()
NRN = NR.dot(N[1:1 + d, :])
return N, QN, NKN, NRN, NR, K_nn
def get_solver_mats(self, x_nd, rot_coef):
n,d = x_nd.shape
K_nn = tps.tps_kernel_matrix(x_nd)
A = np.r_[np.zeros((d+1,d+1)), np.c_[np.ones((n,1)), x_nd]].T
n_cnts = A.shape[0]
_u,_s,_vh = np.linalg.svd(A.T)
N = _u[:,n_cnts:].copy()
NR = (N[1:1+d,:].T * rot_coef).copy() # so that it is c-contiguous
N_gpu = gpuarray.to_gpu(N[1+d:,:])
K_gpu = gpuarray.to_gpu(K_nn)
KN_gpu = culinalg.dot(K_gpu, N_gpu)
QN = np.c_[np.ones((n, 1)), x_nd].dot(N[:1+d,:]) + KN_gpu.get()
NKN_gpu = culinalg.dot(N_gpu, KN_gpu, transa='T')
NKN = NKN_gpu.get()
NRN = NR.dot(N[1:1+d,:])
return N, QN, NKN, NRN, NR, K_nn
def tps_reg_cost(f):
K_nn = tps_kernel_matrix(f.x_na)
cost = 0
for w in f.w_ng.T:
cost += w.dot(K_nn.dot(w))
return cost
def tps_reg_cost(f):
K_nn = tps_kernel_matrix(f.x_na)
cost = 0
for w in f.w_ng.T:
cost += w.dot(K_nn.dot(w))
return cost
def main():
scikits.cuda.linalg.init()
args = parse_arguments()
f = h5py.File(args.datafile, 'r+')
bend_coefs = np.around(
loglinspace(args.bend_coef_init, args.bend_coef_final, args.n_iter),
BEND_COEF_DIGITS)
for seg_name, seg_info in f.iteritems():
if 'inv' in seg_info:
if args.replace:
del seg_info['inv']
inv_group = seg_info.create_group('inv')
else:
inv_group = seg_info['inv']
else:
inv_group = seg_info.create_group('inv')
ds_key = 'DS_SIZE_{}'.format(DS_SIZE)
if ds_key in inv_group:
scaled_x_na = inv_group[ds_key]['scaled_cloud_xyz'][:]
K_nn = inv_group[ds_key]['scaled_K_nn'][:]
else:
ds_g = inv_group.create_group(ds_key)
x_na = downsample_cloud(seg_info[args.cloud_name][:, :3])
scaled_x_na, scale_params = unit_boxify(x_na)
K_nn = tps_kernel_matrix(scaled_x_na)
if args.fill_traj:
r_traj = seg_info['r_gripper_tool_frame']['hmat'][:, :3, 3]
l_traj = seg_info['l_gripper_tool_frame']['hmat'][:, :3, 3]
scaled_r_traj = r_traj * scale_params[0] + scale_params[1]
scaled_l_traj = l_traj * scale_params[0] + scale_params[1]
scaled_r_traj_K = tps_kernel_matrix2(scaled_r_traj,
scaled_x_na)
scaled_l_traj_K = tps_kernel_matrix2(scaled_l_traj,
scaled_x_na)
ds_g['scaled_r_traj'] = scaled_r_traj
ds_g['scaled_l_traj'] = scaled_l_traj
ds_g['scaled_r_traj_K'] = scaled_r_traj_K
ds_g['scaled_l_traj_K'] = scaled_l_traj_K
# Precompute l,r closing indices
lr2finger_traj = {}
for lr in 'lr':
arm_name = {"l": "leftarm", "r": "rightarm"}[lr]
lr2finger_traj[
lr] = gripper_joint2gripper_l_finger_joint_values(
np.asarray(seg_info['%s_gripper_joint' %
lr]))[:, None]
opening_inds, closing_inds = get_opening_closing_inds(
lr2finger_traj[lr])
if '%s_closing_inds' % lr in seg_info:
del seg_info['%s_closing_inds' % lr]
if not closing_inds:
closing_inds = False
seg_info['%s_closing_inds' % lr] = closing_inds
#
ds_g['cloud_xyz'] = x_na
ds_g['scaled_cloud_xyz'] = scaled_x_na
ds_g['scaling'] = scale_params[0]
ds_g['scaled_translation'] = scale_params[1]
ds_g['scaled_K_nn'] = K_nn
for bend_coef in bend_coefs:
if str(bend_coef) in inv_group:
continue
bend_coef_g = inv_group.create_group(str(bend_coef))
_, res = get_sol_params(scaled_x_na, K_nn, bend_coef)
for k, v in res.iteritems():
bend_coef_g[k] = v
if args.verbose:
sys.stdout.write(
'\rprecomputed tps solver for segment {}'.format(seg_name))
sys.stdout.flush()
print ""
if args.test:
atol = 1e-7
print 'Running batch get sol params test with atol = ', atol
test_batch_get_sol_params(f, [bend_coef], atol=atol)
print 'batch sol params test succeeded'
print ""
bend_coefs = np.around(
loglinspace(args.exact_bend_coef_init, args.exact_bend_coef_final,
args.exact_n_iter), BEND_COEF_DIGITS)
for seg_name, seg_info in f.iteritems():
if 'solver' in seg_info:
if args.replace:
del seg_info['solver']
solver_g = seg_info.create_group('solver')
else:
solver_g = seg_info['solver']
else:
solver_g = seg_info.create_group('solver')
x_nd = seg_info['inv'][ds_key]['scaled_cloud_xyz'][:]
K_nn = seg_info['inv'][ds_key]['scaled_K_nn'][:]
N, QN, NON, NR = get_exact_solver(x_nd, K_nn, bend_coefs)
solver_g['N'] = N
solver_g['QN'] = QN
solver_g['NR'] = NR
solver_g['x_nd'] = x_nd
solver_g['K_nn'] = K_nn
NON_g = solver_g.create_group('NON')
for b in bend_coefs:
NON_g[str(b)] = NON[b]
if args.verbose:
sys.stdout.write(
'\rprecomputed exact tps solver for segment {}'.format(
seg_name))
sys.stdout.flush()
print ""
f.close()
def main():
args = parse_arguments()
f = h5py.File(args.datafile, 'r+')
bend_coefs = np.around(loglinspace(args.bend_coef_init, args.bend_coef_final, args.n_iter),
BEND_COEF_DIGITS)
for seg_name, seg_info in f.iteritems():
if 'inv' in seg_info:
if args.replace:
del seg_info['inv']
inv_group = seg_info.create_group('inv')
else:
inv_group = seg_info['inv']
else:
inv_group = seg_info.create_group('inv')
ds_key = 'DS_SIZE_{}'.format(DS_SIZE)
if ds_key in inv_group:
scaled_x_na = inv_group[ds_key]['scaled_cloud_xyz'][:]
K_nn = inv_group[ds_key]['scaled_K_nn'][:]
else:
ds_g = inv_group.create_group(ds_key)
x_na = downsample_cloud(seg_info[args.cloud_name][:, :])
scaled_x_na, scale_params = unit_boxify(x_na)
K_nn = tps_kernel_matrix(scaled_x_na)
ds_g['cloud_xyz'] = x_na
ds_g['scaled_cloud_xyz'] = scaled_x_na
ds_g['scaling'] = scale_params[0]
ds_g['scaled_translation'] = scale_params[1]
ds_g['scaled_K_nn'] = K_nn
for bend_coef in bend_coefs:
if str(bend_coef) in inv_group:
continue
bend_coef_g = inv_group.create_group(str(bend_coef))
_, res = get_sol_params(scaled_x_na, K_nn, bend_coef)
for k, v in res.iteritems():
bend_coef_g[k] = v
if args.verbose:
sys.stdout.write('\rprecomputed approximate tps solver for segment {}'.format(seg_name))
sys.stdout.flush()
print ""
bend_coefs = np.around(loglinspace(args.exact_bend_coef_init, args.exact_bend_coef_final,
args.exact_n_iter),
BEND_COEF_DIGITS)
for seg_name, seg_info in f.iteritems():
if 'solver' in seg_info:
if args.replace:
del seg_info['solver']
solver_g = seg_info.create_group('solver')
else:
solver_g = seg_info['solver']
else:
solver_g = seg_info.create_group('solver')
x_nd = seg_info['inv'][ds_key]['scaled_cloud_xyz'][:]
K_nn = seg_info['inv'][ds_key]['scaled_K_nn'][:]
N, QN, NON, NR = get_exact_solver(x_nd, K_nn, bend_coefs)
solver_g['N'] = N
solver_g['QN'] = QN
solver_g['NR'] = NR
solver_g['x_nd'] = x_nd
solver_g['K_nn'] = K_nn
NON_g = solver_g.create_group('NON')
for b in bend_coefs:
NON_g[str(b)] = NON[b]
if args.verbose:
sys.stdout.write('\rprecomputed exact tps solver for segment {}'.format(seg_name))
sys.stdout.flush()
print ""
f.close()
def main():
scikits.cuda.linalg.init()
args = parse_arguments()
f = h5py.File(args.datafile, 'r+')
bend_coefs = np.around(loglinspace(args.bend_coef_init, args.bend_coef_final, args.n_iter),
BEND_COEF_DIGITS)
for seg_name, seg_info in f.iteritems():
if 'inv' in seg_info:
if args.replace:
del seg_info['inv']
inv_group = seg_info.create_group('inv')
else:
inv_group = seg_info['inv']
else:
inv_group = seg_info.create_group('inv')
ds_key = 'DS_SIZE_{}'.format(DS_SIZE)
if ds_key in inv_group:
scaled_x_na = inv_group[ds_key]['scaled_cloud_xyz'][:]
K_nn = inv_group[ds_key]['scaled_K_nn'][:]
else:
ds_g = inv_group.create_group(ds_key)
x_na = downsample_cloud(seg_info[args.cloud_name][:, :3])
scaled_x_na, scale_params = unit_boxify(x_na)
K_nn = tps_kernel_matrix(scaled_x_na)
if args.fill_traj:
r_traj = seg_info['r_gripper_tool_frame']['hmat'][:, :3, 3]
l_traj = seg_info['l_gripper_tool_frame']['hmat'][:, :3, 3]
scaled_r_traj = r_traj * scale_params[0] + scale_params[1]
scaled_l_traj = l_traj * scale_params[0] + scale_params[1]
scaled_r_traj_K = tps_kernel_matrix2(scaled_r_traj, scaled_x_na)
scaled_l_traj_K = tps_kernel_matrix2(scaled_l_traj, scaled_x_na)
ds_g['scaled_r_traj'] = scaled_r_traj
ds_g['scaled_l_traj'] = scaled_l_traj
ds_g['scaled_r_traj_K'] = scaled_r_traj_K
ds_g['scaled_l_traj_K'] = scaled_l_traj_K
# Precompute l,r closing indices
lr2finger_traj = {}
for lr in 'lr':
arm_name = {"l":"leftarm", "r":"rightarm"}[lr]
lr2finger_traj[lr] = gripper_joint2gripper_l_finger_joint_values(np.asarray(seg_info['%s_gripper_joint'%lr]))[:,None]
opening_inds, closing_inds = get_opening_closing_inds(lr2finger_traj[lr])
if '%s_closing_inds'%lr in seg_info:
del seg_info['%s_closing_inds'%lr]
if not closing_inds:
closing_inds = False
seg_info['%s_closing_inds'%lr] = closing_inds
#
ds_g['cloud_xyz'] = x_na
ds_g['scaled_cloud_xyz'] = scaled_x_na
ds_g['scaling'] = scale_params[0]
ds_g['scaled_translation'] = scale_params[1]
ds_g['scaled_K_nn'] = K_nn
for bend_coef in bend_coefs:
if str(bend_coef) in inv_group:
continue
bend_coef_g = inv_group.create_group(str(bend_coef))
_, res = get_sol_params(scaled_x_na, K_nn, bend_coef)
for k, v in res.iteritems():
bend_coef_g[k] = v
if args.verbose:
sys.stdout.write('\rprecomputed tps solver for segment {}'.format(seg_name))
sys.stdout.flush()
print ""
if args.test:
atol = 1e-7
print 'Running batch get sol params test with atol = ', atol
test_batch_get_sol_params(f, [bend_coef], atol=atol)
print 'batch sol params test succeeded'
print ""
bend_coefs = np.around(loglinspace(args.exact_bend_coef_init, args.exact_bend_coef_final,
args.exact_n_iter),
BEND_COEF_DIGITS)
for seg_name, seg_info in f.iteritems():
if 'solver' in seg_info:
if args.replace:
del seg_info['solver']
solver_g = seg_info.create_group('solver')
else:
solver_g = seg_info['solver']
else:
solver_g = seg_info.create_group('solver')
x_nd = seg_info['inv'][ds_key]['scaled_cloud_xyz'][:]
K_nn = seg_info['inv'][ds_key]['scaled_K_nn'][:]
N, QN, NON, NR = get_exact_solver(x_nd, K_nn, bend_coefs)
solver_g['N'] = N
solver_g['QN'] = QN
solver_g['NR'] = NR
solver_g['x_nd'] = x_nd
solver_g['K_nn'] = K_nn
NON_g = solver_g.create_group('NON')
for b in bend_coefs:
NON_g[str(b)] = NON[b]
if args.verbose:
sys.stdout.write('\rprecomputed exact tps solver for segment {}'.format(seg_name))
sys.stdout.flush()
print ""
f.close()
