def proc_grasp_queries(grasp_data_path):
'''
Process grasp queries to get grasp voxel data.
'''
# TODO: testing.
data_proc_lib = DataProcLib()
objects_per_batch = 2
grasps_per_object = 2
voxel_grid_dim = [20, 20, 20]
object_frame_id = 'object_pose'
#listener = tf.TransformListener()
#grasp_id_file = h5py.File(grasp_data_path + '/grasp_data/grasp_obj_id.h5', 'r')
#objects_num = grasp_id_file['cur_object_id'][()] + 1
##objects_num = 4
#grasp_id_file.close()
grasp_voxel_file_path = grasp_data_path + '/grasp_voxel_data.h5'
grasp_voxel_file = h5py.File(grasp_voxel_file_path, 'a')
grasp_configs_obj = []
grasp_labels = []
grasp_voxel_grids = []
voxel_grasps_id = 0
for obj_id in xrange(objects_per_batch):
for grasp_id in xrange(grasps_per_object):
obj_grasp_data_path = glob.glob(grasp_data_path +
'/grasp_data/object_' +
str(obj_id) + '*grasp_' +
str(grasp_id) + '*.h5')[0]
obj_grasp_data_file = h5py.File(obj_grasp_data_path, 'r')
obj_id_grasp_id = 'object_' + str(obj_id) + '_grasp_' + str(
grasp_id)
grasp_label = obj_grasp_data_file[obj_id_grasp_id +
'_grasp_label'][()]
js_position = obj_grasp_data_file[obj_id_grasp_id +
'_true_preshape_js_position'][()]
palm_pose_array = obj_grasp_data_file[
obj_id_grasp_id + '_true_preshape_palm_world_pose'][()]
#object_pose_array = obj_grasp_data_file[obj_id_grasp_id +
# '_object_pcd_pose'][()]
object_pose_array = obj_grasp_data_file['object_' + str(obj_id) +
'_grasp_' + str(grasp_id) +
'_object_pose'][()]
obj_grasp_data_file.close()
pcd_file_path = glob.glob(grasp_data_path + '/pcd/object_' +
str(obj_id) + '*_grasp_' +
str(grasp_id) + '.pcd')[0]
palm_pose_world = data_proc_lib.convert_array_to_pose(
palm_pose_array, 'world')
data_proc_lib.update_palm_pose_client(palm_pose_world)
# Object pose is already aligned in the new version data collection.
object_pose_world = data_proc_lib.convert_array_to_pose(
object_pose_array, 'world')
#object_aligned_pose = align_obj_frame.align_obj_ort(object_pose_world,
# data_proc_lib.listener)
#data_proc_lib.update_object_pose_client(object_aligned_pose)
data_proc_lib.update_object_pose_client(object_aligned_pose)
voxel_grasp_id = 'voxel_grasp_' + str(voxel_grasps_id)
print voxel_grasp_id
sparse_voxel_key = voxel_grasp_id + '_sparse_voxel'
if sparse_voxel_key not in grasp_voxel_file:
# Resume from previous voxel grids generation.
print 'gen voxel'
sparse_voxel_grid = data_proc_lib.voxel_gen_client(
pcd_file_path)
grasp_voxel_file.create_dataset(voxel_grasp_id + '_obj_id',
data=obj_id)
grasp_voxel_file.create_dataset(voxel_grasp_id + '_grasp_id',
data=grasp_id)
grasp_voxel_file.create_dataset(sparse_voxel_key,
data=sparse_voxel_grid)
else:
print 'read voxel'
sparse_voxel_grid = grasp_voxel_file[sparse_voxel_key][()]
#show_voxel.plot_voxel(sparse_voxel_grid, './voxel.png')
voxel_grid = np.zeros(tuple(voxel_grid_dim))
voxel_grid_index = sparse_voxel_grid.astype(int)
voxel_grid[voxel_grid_index[:, 0], voxel_grid_index[:, 1],
voxel_grid_index[:, 2]] = 1
print sparse_voxel_grid.shape
print np.sum(voxel_grid)
#print sparse_voxel_grid
#print np.array(np.where(voxel_grid == 1.))
#print voxel_grid
grasp_voxel_grids.append(voxel_grid)
palm_pose_object_array = data_proc_lib.trans_pose_to_obj_tf(
palm_pose_world)
#print palm_pose_object
grasp_config_obj = np.concatenate(
(palm_pose_object_array, js_position), axis=0)
print 'grasp_config_obj:', grasp_config_obj
grasp_configs_obj.append(grasp_config_obj)
grasp_labels.append(grasp_label)
voxel_grasps_id += 1
print '######'
grasp_voxel_file.create_dataset('grasp_voxel_grids',
data=grasp_voxel_grids)
print grasp_configs_obj
grasp_voxel_file.create_dataset('grasp_configs_obj',
data=grasp_configs_obj)
grasp_voxel_file.create_dataset('grasp_labels', data=grasp_labels)
print np.sum(grasp_voxel_file['grasp_voxel_grids'][()])
print 'Grasp data processing is done.'
grasp_voxel_file.close()
def proc_grasp_data(raw_data_path, proc_data_path):
grasp_file_path = raw_data_path + 'grasp_data.h5'
grasp_data_file = h5py.File(grasp_file_path, 'r')
proc_grasp_file_path = proc_data_path + 'grasp_rgbd_data.h5'
#proc_grasp_file_path = proc_data_path + 'proc_grasp_data_2.h5'
proc_grasp_file = h5py.File(proc_grasp_file_path, 'a')
grasps_number_key = 'grasps_number'
if grasps_number_key not in proc_grasp_file:
proc_grasp_file.create_dataset(grasps_number_key, data=0)
grasp_no_obj_num_key = 'grasp_no_obj_num'
if grasp_no_obj_num_key not in proc_grasp_file:
proc_grasp_file.create_dataset(grasp_no_obj_num_key, data=0)
cvtf = gcf.ConfigConvertFunctions()
grasps_num = grasp_data_file['total_grasps_num'][()]
grasps_no_seg_num = proc_grasp_file[grasp_no_obj_num_key][()]
max_object_id = grasp_data_file['max_object_id'][()]
print grasps_num, max_object_id
data_proc_lib = DataProcLib()
# Notice: the max grasp id of one object could be 19 (0-19) + 3 = 22, because we try
# 3 grasp heuristic grasp trials for each object pose.
max_grasps_per_obj = 23
cur_grasps_num = 0
cur_grasps_no_seg_num = 0
#suc_grasps_num = 0
#top_grasps_num = 0
#top_suc_grasps_num = 0
for obj_id in xrange(max_object_id + 1):
object_id = 'object_' + str(obj_id)
object_name = grasp_data_file[object_id + '_name'][()]
#print object_name
for grasp_id_cur_obj in xrange(max_grasps_per_obj):
#print grasp_id_cur_obj
print cur_grasps_num
object_grasp_id = object_id + '_grasp_' + str(grasp_id_cur_obj)
print object_grasp_id
grasp_label_key = object_grasp_id + '_grasp_label'
if grasp_label_key in grasp_data_file:
# Only check the grasp already exists if it's a valid grasp.
grasp_source_info_key = 'grasp_' + str(
cur_grasps_num) + '_source_info'
if grasp_source_info_key in proc_grasp_file:
# Skip processed grasps from previous runs.
if cur_grasps_no_seg_num < grasps_no_seg_num:
cur_grasps_no_seg_num += 1
else:
cur_grasps_num += 1
continue
grasp_label = grasp_data_file[grasp_label_key][()]
# Relabel the grasp labels to overcome grasp control errors
object_world_seg_pose_key = object_grasp_id + '_object_world_seg_pose'
object_world_seg_pose_array = grasp_data_file[
object_world_seg_pose_key][()]
preshape_js_position_key = object_grasp_id + '_preshape_joint_state_position'
preshape_js = grasp_data_file[preshape_js_position_key][()]
palm_world_pose_key = object_grasp_id + '_preshape_palm_world_pose'
palm_world_pose_array = grasp_data_file[palm_world_pose_key][(
)]
top_grasp_key = object_grasp_id + '_top_grasp'
top_grasp = grasp_data_file[top_grasp_key][()]
pcd_file_name = find_pcd_file(data_path, grasp_data_file,
object_id, grasp_id_cur_obj,
object_name)
pcd_file_path = data_path + 'pcd/' + pcd_file_name
print pcd_file_path
seg_obj_resp = gcf.seg_obj_from_file_client(
pcd_file_path, data_proc_lib.listener)
if not seg_obj_resp.object_found:
print 'No object found for segmentation!'
cur_grasps_no_seg_num += 1
proc_grasp_file[grasp_no_obj_num_key][(
)] = cur_grasps_no_seg_num
continue
palm_world_pose = cvtf.convert_array_to_pose(
palm_world_pose_array, 'world')
data_proc_lib.update_palm_pose_client(palm_world_pose)
#object_world_pose = cvtf.convert_array_to_pose(object_world_seg_pose_array, 'world')
#data_proc_lib.update_object_pose_client(object_world_pose)
obj_world_pose_stamp = PoseStamped()
obj_world_pose_stamp.header.frame_id = seg_obj_resp.obj.header.frame_id
obj_world_pose_stamp.pose = seg_obj_resp.obj.pose
data_proc_lib.update_object_pose_client(obj_world_pose_stamp)
sparse_voxel_grid = data_proc_lib.voxel_gen_client(
seg_obj_resp.obj)
#show_voxel.plot_voxel(sparse_voxel_grid)
#voxel_grid = np.zeros(tuple(data_proc_lib.voxel_grid_full_dim))
#voxel_grid_index = sparse_voxel_grid.astype(int)
#voxel_grid[voxel_grid_index[:, 0], voxel_grid_index[:, 1], voxel_grid_index[:, 2]] = 1
# print sparse_voxel_grid.shape
# print np.sum(voxel_grid)
# print sparse_voxel_grid
# print np.array(np.where(voxel_grid == 1.))
# print voxel_grid
palm_object_pose_array = data_proc_lib.trans_pose_to_obj_tf(
palm_world_pose)
grasp_config_obj = np.concatenate(
(palm_object_pose_array, preshape_js), axis=0)
#print 'grasp_config_obj:', grasp_config_obj
grasp_source_info = object_grasp_id + '_' + object_name
proc_grasp_file.create_dataset(grasp_source_info_key,
data=grasp_source_info)
grasp_config_obj_key = 'grasp_' + str(
cur_grasps_num) + '_config_obj'
proc_grasp_file.create_dataset(grasp_config_obj_key,
data=grasp_config_obj)
grasp_sparse_voxel_key = 'grasp_' + str(
cur_grasps_num) + '_sparse_voxel'
proc_grasp_file.create_dataset(grasp_sparse_voxel_key,
data=sparse_voxel_grid)
obj_dim_w_h_d = np.array([
seg_obj_resp.obj.width, seg_obj_resp.obj.height,
seg_obj_resp.obj.depth
])
obj_dim_key = 'grasp_' + str(cur_grasps_num) + '_dim_w_h_d'
proc_grasp_file.create_dataset(obj_dim_key, data=obj_dim_w_h_d)
grasp_label_key = 'grasp_' + str(cur_grasps_num) + '_label'
proc_grasp_file.create_dataset(grasp_label_key,
data=grasp_label)
grasp_top_grasp_key = 'grasp_' + str(
cur_grasps_num) + '_top_grasp'
proc_grasp_file.create_dataset(grasp_top_grasp_key,
data=top_grasp)
cur_grasps_num += 1
proc_grasp_file[grasps_number_key][()] = cur_grasps_num
else:
print 'Grasp label does not exist!'
#proc_grasp_file.close()
#raw_input('Hold')
#suc_grasps_num += grasp_label
#top_grasps_num += top_grasp
#if grasp_label == 1 and top_grasp:
# top_suc_grasps_num += 1
#print cur_grasps_num, suc_grasps_num, top_grasps_num, top_suc_grasps_num
grasp_data_file.close()
proc_grasp_file.close()
class GraspVoxelInfServer:
def __init__(self):
rospy.init_node('grasp_voxel_inf_server')
vis_preshape = rospy.get_param('~vis_preshape', False)
virtual_hand_parent_tf = rospy.get_param('~virtual_hand_parent_tf', '')
grasp_net_model_path = pkg_path + '/models/grasp_al_net/' + \
'grasp_net_freeze_enc_10_sets.ckpt'
prior_model_path = pkg_path + '/models/grasp_al_prior/' + \
'prior_net_freeze_enc_10_sets.ckpt'
gmm_model_path = pkg_path + '/models/grasp_al_prior/gmm_10_sets'
# prior_model_path = pkg_path + '/models/grasp_al_prior/' + \
# 'suc_prior_net_freeze_enc_10_sets.ckpt'
# gmm_model_path = pkg_path + '/models/grasp_al_prior/suc_gmm_10_sets'
self.grasp_voxel_inf = GraspVoxelInference(grasp_net_model_path,
prior_model_path, gmm_model_path,
vis_preshape=vis_preshape,
virtual_hand_parent_tf=virtual_hand_parent_tf)
self.cvtf = gcf.ConfigConvertFunctions()
self.data_proc_lib = DataProcLib()
def handle_grasp_voxel_inf(self, req):
print 'prior:', req.prior_name
obj_world_pose_stamp = PoseStamped()
obj_world_pose_stamp.header.frame_id = req.seg_obj.header.frame_id
obj_world_pose_stamp.pose = req.seg_obj.pose
self.data_proc_lib.update_object_pose_client(obj_world_pose_stamp)
config_init = None
if req.prior_name == 'Constraint':
init_hand_config = copy.deepcopy(req.init_hand_config)
init_hand_config.palm_pose.header.stamp = rospy.Time.now()
init_hand_config.palm_pose = \
self.data_proc_lib.trans_pose_to_obj_tf(init_hand_config.palm_pose,
return_array=False)
config_init = self.cvtf.convert_full_to_preshape_config(init_hand_config)
response = GraspVoxelInferResponse()
# seg_obj_resp = gcf.segment_object_client(self.data_proc_lib.listener)
# if not seg_obj_resp.object_found:
# response.success = False
# print 'No object found for segmentation!'
# return response
sparse_voxel_grid, voxel_size, voxel_grid_dim = self.data_proc_lib.voxel_gen_client(req.seg_obj)
# show_voxel.plot_voxel(sparse_voxel_grid)
voxel_grid = np.zeros(tuple(voxel_grid_dim))
voxel_grid_index = sparse_voxel_grid.astype(int)
voxel_grid[voxel_grid_index[:, 0], voxel_grid_index[:, 1],
voxel_grid_index[:, 2]] = 1
voxel_grid = np.expand_dims(voxel_grid, -1)
obj_size = np.array([req.seg_obj.width, req.seg_obj.height,
req.seg_obj.depth])
comp_mean_config = self.grasp_voxel_inf.mdn_mean_explicit(voxel_grid,
obj_size, req.grasp_type)
print 'comp_mean_config: ', comp_mean_config
full_comp_mean_config = self.cvtf.convert_preshape_to_full_config(
comp_mean_config, 'object_pose')
config_inf, obj_val_inf, config_init, obj_val_init, plan_info = \
self.grasp_voxel_inf.max_grasp_suc_bfgs(voxel_grid, obj_size,
req.prior_name, config_init, req.grasp_type)
full_config_init = self.cvtf.convert_preshape_to_full_config(
config_init, 'object_pose')
full_config_inf = self.cvtf.convert_preshape_to_full_config(
config_inf, 'object_pose')
self.grasp_voxel_inf.preshape_config = full_comp_mean_config
# self.grasp_voxel_inf.preshape_config = full_config_inf
# self.grasp_voxel_inf.pub_preshape_config()
self.data_proc_lib.update_palm_pose_client(full_config_inf.palm_pose)
response.inf_config_array = config_inf
response.full_inf_config = full_config_inf
response.inf_val = obj_val_inf
response.inf_suc_prob = plan_info['inf_suc_prob']
response.inf_log_prior = plan_info['inf_log_prior']
response.init_val = obj_val_init
response.init_suc_prob = plan_info['init_suc_prob']
response.init_log_prior = plan_info['init_log_prior']
response.init_config_array = config_init
response.full_init_config = full_config_init
response.sparse_voxel_grid = sparse_voxel_grid.flatten()
response.voxel_size = voxel_size
response.voxel_grid_dim = voxel_grid_dim
response.object_size = obj_size
response.success = True
return response
def create_voxel_inf_server(self):
'''
Create grasp inference server for the grasp model,
including the voxel classifier net and the MDN/GMM prior.
'''
rospy.Service('grasp_voxel_infer', GraspVoxelInfer,
self.handle_grasp_voxel_inf)
rospy.loginfo('Service grasp_voxel_infer:')
rospy.loginfo('Ready to perform voxel grasp inference.')
class GraspALServer:
def __init__(self):
rospy.init_node('grasp_al_server')
self.vis_preshape = rospy.get_param('~vis_preshape', False)
virtual_hand_parent_tf = rospy.get_param('~virtual_hand_parent_tf', '')
# grasp_net_model_path = pkg_path + '/models/grasp_al_net/' + \
# 'grasp_net_freeze_enc_2_sets.ckpt'
# gmm_model_path = pkg_path + '/models/grasp_al_prior/gmm_2_sets'
# mdn_model_path = pkg_path + '/models/grasp_al_prior/' + \
# 'prior_net_freeze_enc_2_sets.ckpt'
grasp_net_model_path = pkg_path + '/models/grasp_al_net/' + \
'grasp_net_5_sets.ckpt'
gmm_model_path = pkg_path + '/models/grasp_al_prior/gmm_5_sets'
suc_gmm_model_path = pkg_path + '/models/grasp_al_prior/suc_gmm_5_sets'
mdn_model_path = pkg_path + '/models/grasp_al_prior/' + \
'prior_net_5_sets.ckpt'
suc_mdn_model_path = pkg_path + '/models/grasp_al_prior/' + \
'prior_net_suc_5_sets.ckpt'
active_models_path = '/mnt/tars_data/multi_finger_sim_data/active_models/'
# spv_data_path = '/mnt/tars_data/gazebo_al_grasps/train/' + \
# 'merged_grasp_data_6_6_and_6_8.h5'
# active_data_path = '/mnt/tars_data/multi_finger_sim_data/grasp_data.h5'
spv_data_path = '/mnt/tars_data/gazebo_al_grasps/train/' + \
'merged_grasp_data_6_6_and_6_8_and_6_10_and_6_11_and_6_13.h5'
suc_spv_data_path = '/mnt/tars_data/gazebo_al_grasps/train/' + \
'merged_suc_grasp_5_sets.h5'
active_data_path = '/mnt/tars_data/multi_finger_sim_data/grasp_data.h5'
suc_active_data_path = '/mnt/tars_data/multi_finger_sim_data/suc_grasp_data.h5'
al_file_path = '/mnt/tars_data/multi_finger_sim_data/al_data.h5'
self.data_proc_lib = DataProcLib()
self.cvtf = gcf.ConfigConvertFunctions(self.data_proc_lib)
# self.gal = GraspActiveLearner(grasp_net_model_path, prior_model_path,
# al_file_path, active_models_path,
# spv_data_path, active_data_path)
# prior_name = 'GMM'
prior_name = 'MDN'
prior_model_path = None
if prior_name == 'MDN':
prior_model_path = mdn_model_path
suc_prior_path = suc_mdn_model_path
elif prior_name == 'GMM':
prior_model_path = gmm_model_path
suc_prior_path = suc_gmm_model_path
self.gal = GraspActiveLearnerFK(prior_name, grasp_net_model_path,
prior_model_path, suc_prior_path,
al_file_path, active_models_path,
spv_data_path, suc_spv_data_path,
active_data_path, suc_active_data_path,
self.cvtf)
# self.active_method = 'UCB'
self.active_method = 'ALTER_OPT'
# self.active_method = 'ALTER_POOLING'
# self.active_method = 'UCB_POOLING'
def handle_grasp_active_learning(self, req):
response = GraspActiveLearnResponse()
response.success = False
seg_obj_resp = gcf.segment_object_client(self.data_proc_lib.listener)
if not seg_obj_resp.object_found:
print 'No object found for segmentation!'
return response
obj_world_pose_stamp = PoseStamped()
obj_world_pose_stamp.header.frame_id = seg_obj_resp.obj.header.frame_id
obj_world_pose_stamp.pose = seg_obj_resp.obj.pose
self.data_proc_lib.update_object_pose_client(obj_world_pose_stamp)
# sparse_voxel_grid = self.data_proc_lib.voxel_gen_client(seg_obj_resp.obj)
sparse_voxel_grid, voxel_size, voxel_grid_dim = \
self.data_proc_lib.voxel_gen_client(seg_obj_resp.obj)
# show_voxel.plot_voxel(sparse_voxel_grid)
voxel_grid = np.zeros(tuple(voxel_grid_dim))
voxel_grid_index = sparse_voxel_grid.astype(int)
voxel_grid[voxel_grid_index[:, 0], voxel_grid_index[:, 1],
voxel_grid_index[:, 2]] = 1
voxel_grid = np.expand_dims(voxel_grid, -1)
# voxel_grid = np.zeros(tuple(self.data_proc_lib.voxel_grid_full_dim))
# print sparse_voxel_grid
# voxel_grid_index = sparse_voxel_grid.astype(int)
# voxel_grid[voxel_grid_index[:, 0], voxel_grid_index[:, 1],
# voxel_grid_index[:, 2]] = 1
# voxel_grid = np.expand_dims(voxel_grid, -1)
obj_size = np.array([seg_obj_resp.obj.width, seg_obj_resp.obj.height,
seg_obj_resp.obj.depth])
self.gal.cur_act_reward = None
self.gal.cur_action_id = None
if self.active_method == 'UCB':
reward, ik_config_inf, config_inf, obj_val_inf, \
ik_config_init, config_init, obj_val_init, \
res_info = self.gal.ucb(voxel_grid, obj_size)
elif self.active_method == 'ALTER_OPT':
reward, ik_config_inf, config_inf, obj_val_inf, \
ik_config_init, config_init, obj_val_init, \
res_info = self.gal.alternation(voxel_grid, obj_size,
pooling=False)
elif self.active_method == 'ALTER_POOLING':
reward, ik_config_inf, config_inf, obj_val_inf, \
ik_config_init, config_init, obj_val_init, \
res_info = self.gal.alternation(voxel_grid, obj_size,
pooling=True)
elif self.active_method == 'UCB_POOLING':
reward, ik_config_inf, config_inf, obj_val_inf, \
ik_config_init, config_init, obj_val_init, \
res_info = self.gal.ucb(voxel_grid, obj_size,
pooling=True)
if reward is None:
print 'Active learning failed!'
return response
full_config_init = self.cvtf.convert_preshape_to_full_config(
config_init, 'object_pose')
full_config_inf = self.cvtf.convert_preshape_to_full_config(
config_inf, 'object_pose')
self.data_proc_lib.update_palm_pose_client(full_config_inf.palm_pose)
response.reward = reward
response.action = res_info['action']
response.inf_ik_config_array = ik_config_inf
response.inf_config_array = config_inf
response.full_inf_config = full_config_inf
response.inf_val = obj_val_inf
response.inf_suc_prob = res_info['inf_suc_prob']
response.inf_log_prior = res_info['inf_log_prior']
response.inf_uct = res_info['inf_uct']
response.init_val = obj_val_init
response.init_suc_prob = res_info['init_suc_prob']
response.init_log_prior = res_info['init_log_prior']
response.init_uct = res_info['init_uct']
response.init_ik_config_array = ik_config_init
response.init_config_array = config_init
response.full_init_config = full_config_init
response.sparse_voxel_grid = sparse_voxel_grid.flatten()
response.object_size = obj_size
response.success = True
return response
def create_grasps_al_server(self):
'''
Create grasps active learning server.
'''
rospy.Service('grasp_active_learn', GraspActiveLearn,
self.handle_grasp_active_learning)
rospy.loginfo('Service grasp_active_learn:')
rospy.loginfo('Ready to perform grasp active learning.')
def handle_active_model_update(self, req):
response = ActiveModelUpdateResponse()
self.gal.active_model_update(req.batch_id)
response.success = True
return response
def create_al_model_update_server(self):
'''
Create grasps active learning model update server.
'''
rospy.Service('active_model_update', ActiveModelUpdate,
self.handle_active_model_update)
rospy.loginfo('Service active_model_update:')
rospy.loginfo('Ready to perform grasp active learning model update.')
def handle_active_data_update(self, req):
response = ActiveDataUpdateResponse()
if req.grasp_has_plan:
self.gal.active_data_update()
response.success = True
return response
def create_al_data_update_server(self):
'''
Create grasps active learning model update server.
'''
rospy.Service('active_data_update', ActiveDataUpdate,
self.handle_active_data_update)
rospy.loginfo('Service active_data_update:')
rospy.loginfo('Ready to perform grasp active learning data update.')
class ActivePlanServer:
def __init__(self):
rospy.init_node('active_plan_server')
self.vis_preshape = rospy.get_param('~vis_preshape', False)
virtual_hand_parent_tf = rospy.get_param('~virtual_hand_parent_tf', '')
active_models_path = rospy.get_param('~active_models_path', '')
#active_models_path = '/mnt/tars_data/multi_finger_sim_data/active_models/'
# less_data_net_path = pkg_path + '/models/grasp_al_net/' + \
# 'grasp_net_freeze_enc_2_sets.ckpt'
# less_data_prior_path = pkg_path + '/models/grasp_al_prior/gmm_2_sets'
# more_data_net_path = pkg_path + '/models/grasp_al_net/' + \
# 'grasp_net_freeze_enc_10_sets.ckpt'
# more_data_prior_path = pkg_path + '/models/grasp_al_prior/gmm_10_sets'
less_data_net_path = pkg_path + '/models/grasp_al_net/' + \
'grasp_net_5_sets.ckpt'
less_data_prior_path = pkg_path + '/models/grasp_al_prior/prior_net_5_sets.ckpt'
more_data_net_path = pkg_path + '/models/grasp_al_net/' + \
'grasp_net_10_sets.ckpt'
more_data_prior_path = pkg_path + '/models/grasp_al_prior/prior_net_10_sets.ckpt'
self.data_proc_lib = DataProcLib()
self.cvtf = gcf.ConfigConvertFunctions(self.data_proc_lib)
#prior_name = 'GMM'
prior_name = 'MDN'
g1 = tf.Graph()
g2 = tf.Graph()
g3 = tf.Graph()
with g1.as_default():
self.active_planner = GraspActiveLearnerFK(prior_name,
active_models_path=active_models_path,
cvtf=self.cvtf)
with g2.as_default():
self.more_data_spv_planner = GraspActiveLearnerFK(prior_name,
grasp_net_model_path=more_data_net_path,
prior_model_path=more_data_prior_path,
cvtf=self.cvtf)
with g3.as_default():
self.less_data_spv_planner = GraspActiveLearnerFK(prior_name,
grasp_net_model_path=less_data_net_path,
prior_model_path=less_data_prior_path,
cvtf=self.cvtf)
self.action = 'grasp_suc'
def handle_grasp_active_plan(self, req):
response = GraspActiveLearnResponse()
response.success = False
seg_obj_resp = gcf.segment_object_client(self.data_proc_lib.listener)
if not seg_obj_resp.object_found:
print 'No object found for segmentation!'
return response
obj_world_pose_stamp = PoseStamped()
obj_world_pose_stamp.header.frame_id = seg_obj_resp.obj.header.frame_id
obj_world_pose_stamp.pose = seg_obj_resp.obj.pose
self.data_proc_lib.update_object_pose_client(obj_world_pose_stamp)
sparse_voxel_grid, voxel_size, voxel_grid_full_dim = self.data_proc_lib.voxel_gen_client(seg_obj_resp.obj)
# show_voxel.plot_voxel(sparse_voxel_grid)
voxel_grid = np.zeros(tuple(voxel_grid_full_dim))
voxel_grid_index = sparse_voxel_grid.astype(int)
voxel_grid[voxel_grid_index[:, 0], voxel_grid_index[:, 1],
voxel_grid_index[:, 2]] = 1
voxel_grid = np.expand_dims(voxel_grid, -1)
obj_size = np.array([seg_obj_resp.obj.width, seg_obj_resp.obj.height,
seg_obj_resp.obj.depth])
planner = None
print req.planner_name
if req.planner_name == 'active_planner':
planner = self.active_planner
elif req.planner_name == 'more_data_spv_planner':
planner = self.more_data_spv_planner
elif req.planner_name == 'less_data_spv_planner':
planner = self.less_data_spv_planner
reward, ik_config_inf, config_inf, obj_val_inf, \
ik_config_init, config_init, obj_val_init, plan_info, inf_time = \
planner.grasp_strategies(self.action, voxel_grid,
obj_size)
if reward is None:
print 'Active planning failed!'
return response
full_config_init = self.cvtf.convert_preshape_to_full_config(
config_init, 'object_pose')
full_config_inf = self.cvtf.convert_preshape_to_full_config(
config_inf, 'object_pose')
self.data_proc_lib.update_palm_pose_client(full_config_inf.palm_pose)
response.reward = reward
response.action = plan_info['action']
response.inf_ik_config_array = ik_config_inf
response.inf_config_array = config_inf
response.full_inf_config = full_config_inf
response.inf_val = obj_val_inf
response.inf_suc_prob = plan_info['inf_suc_prob']
response.inf_log_prior = plan_info['inf_log_prior']
response.inf_uct = plan_info['inf_uct']
response.init_val = obj_val_init
response.init_suc_prob = plan_info['init_suc_prob']
response.init_log_prior = plan_info['init_log_prior']
response.init_uct = plan_info['init_uct']
response.init_ik_config_array = ik_config_init
response.init_config_array = config_init
response.full_init_config = full_config_init
response.sparse_voxel_grid = sparse_voxel_grid.flatten()
response.voxel_size = voxel_size
response.voxel_grid_dim = voxel_grid_full_dim
response.object_size = obj_size
response.success = True
return response
def create_active_plan_server(self):
'''
Create grasps active learning server.
'''
rospy.Service('grasp_active_plan', GraspActiveLearn,
self.handle_grasp_active_plan)
rospy.loginfo('Service grasp_active_plan:')
rospy.loginfo('Ready to perform grasp active planning.')