def build_model():
dataset_filename = choose.choose_from(paths.PROCESSED_TRAINING_DATASET_DIR)
model_template = choose.choose_from(paths.MODEL_TEMPLATE_DIR)
model_template_yaml = open(paths.MODEL_TEMPLATE_DIR + model_template + "/model.yaml", 'r').read()
hyper_params_file = open(paths.MODEL_TEMPLATE_DIR + model_template + "/hyper_params.yaml", 'r').read()
dataset = h5py.File(paths.PROCESSED_TRAINING_DATASET_DIR + dataset_filename)
num_labels = dataset['c01b_train_patch_labels'].shape[-1]
hyper_params_dict = yaml_parse.load(hyper_params_file)
hyper_params_dict['save_path'] = get_save_path(model_template, dataset_filename)
hyper_params_dict['dataset'] = paths.PROCESSED_TRAINING_DATASET_DIR + dataset_filename
hyper_params_dict['num_output_channels'] = num_labels
return model_template_yaml, hyper_params_dict
def main():
conv_model_name = choose.choose_from(paths.MODEL_DIR)
conv_model_filepath = paths.MODEL_DIR + conv_model_name + "/cnn_model.pkl"
dataset_file = choose.choose_from(paths.RAW_TRAINING_DATASET_DIR)
raw_rgbd_filepath = paths.RAW_TRAINING_DATASET_DIR + dataset_file
save_filepath = init_save_file(dataset_file, conv_model_name)
pipelines = [("grasp_rgbd", GraspClassificationPipeline),
("grasp_depth", GraspClassificationPipeline),
#("constrained_grasp", ConstrainedGraspClassificationPipeline),
("garmet", GarmetClassificationPipeline),
("barrett", BarrettGraspClassificationPipeline)
]
pipeline = choose.choose(pipelines, 'pipeline')
pipeline = pipeline(save_filepath, raw_rgbd_filepath, conv_model_filepath, input_key="rgbd")
#pipeline = pipeline(save_filepath, raw_rgbd_filepath, conv_model_filepath, input_key="depth_data")
pipeline.run()
def main():
dataset_file = choose.choose_from(paths.HEATMAPS_DATASET_DIR)
dataset = h5py.File(paths.HEATMAPS_DATASET_DIR + dataset_file)
for i in range(dataset['rgbd_data'].shape[0]):
rgbd_img = dataset['rgbd_data'][i]
heatmaps = dataset['heatmaps'][i]
convolved_heatmaps = dataset['convolved_heatmaps'][i]
palm_conv = dataset["p_convolved_heatmaps"][i]
l_conv = dataset["l_convolved_heatmaps"][i]
r_conv = dataset["r_convolved_heatmaps"][i]
plotter2 = Plotter(2)
plotter2.add_subplot('l_obs', heatmaps[:, :, 0]/heatmaps[:,:,0].max())
plotter2.add_subplot('l_g_p * palm', palm_conv[0, :, :])
plotter2.add_subplot('l_g_r * r_grip', r_conv[1, :, :])
plotter2.add_subplot('l_convolved', convolved_heatmaps[:, :, 0])
plotter2.add_subplot('p_obs', heatmaps[:, :, 1]/heatmaps[:,:,1].max())
plotter2.add_subplot('p_g_l * l_grip', l_conv[2, :, :])
plotter2.add_subplot('p_g_r * r_grip', r_conv[3, :, :])
plotter2.add_subplot('p_convolved', convolved_heatmaps[:, :, 1])
plotter2.add_subplot('r_obs', heatmaps[:, :, 2]/heatmaps[:,:,2].max())
plotter2.add_subplot('r_g_p * palm', palm_conv[5, :, :])
plotter2.add_subplot('r_g_l * l_grip', l_conv[4, :, :])
plotter2.add_subplot('r_convolved', convolved_heatmaps[:, :, 2])
palm_out = convolved_heatmaps[:, :, 1]
palm_min = np.argmin(palm_out)
palm_out_x, palm_out_y = (palm_min / palm_out.shape[1], palm_min % palm_out.shape[1])
l_out = convolved_heatmaps[:, :, 0]
l_min = np.argmin(l_out)
l_out_x, l_out_y = (l_min / l_out.shape[1], l_min % l_out.shape[1])
r_out = convolved_heatmaps[:, :, 2]
r_min = np.argmin(r_out)
r_out_x, r_out_y = (r_min / r_out.shape[1], r_min % r_out.shape[1])
rgbd_overlay = np.copy(rgbd_img)
x_offset = (rgbd_overlay.shape[0]-palm_out.shape[0])/2
y_offset = (rgbd_overlay.shape[1]-palm_out.shape[1])/2
rgbd_overlay[x_offset+ l_out_x-10:x_offset+l_out_x+10,y_offset+ l_out_y-10:y_offset+l_out_y+10, :] = 0
rgbd_overlay[x_offset+ palm_out_x-10:x_offset+palm_out_x+10,y_offset+ palm_out_y-10:y_offset+palm_out_y+10, :] = 0
rgbd_overlay[x_offset+ r_out_x-10:x_offset+r_out_x+10,y_offset+ r_out_y-10:y_offset+r_out_y+10, :] = 0
plotter1 = Plotter(1)
if rgbd_img.shape[-1] == 4:
plotter2.add_subplot('rgb', rgbd_img[:, :, 0:3])
plotter2.add_subplot('d', rgbd_img[:, :, 3])
else:
plotter2.add_subplot('d', rgbd_img[:,:,0])
if rgbd_img.shape[-1] == 4:
plotter2.add_subplot("convolved_overlay", rgbd_overlay[:, :, :-1])
else:
plotter2.add_subplot("convolved_overlay", rgbd_overlay[:, :, 0])
#plotter1.show()
plotter2.show()
plt.show()
grasp_prior = GraspPrior()
grasp_prior.wrist_roll = avg_wrist_roll[i]
grasp_prior.uvd = avg_uvd[i]
grasp_prior.joint_values = avg_joint_values[i]
grasp_priors_list.add_grasp_prior(grasp_prior)
f = open(self.out_filepath, 'w')
pickle.dump(grasp_priors_list, f)
f.close()
if __name__ == "__main__":
# Choose in and out files.
in_file = choose_from(CONDENSED_GAZEBO_DIR)
in_filepath = CONDENSED_GAZEBO_DIR + in_file
out_path = GRASP_PRIORS_DIR + in_file[:-3] + "/"
try:
os.stat(out_path)
except:
os.mkdir(out_path)
out_filepath = out_path + "grasp_priors_list.pkl"
gpl_generator = GraspPriorListGen(in_filepath=in_filepath,
out_filepath=out_filepath)
gpl_generator.run()
model_manager = GazeboModelManager(models_dir=GDL_MODEL_PATH)
model_manager.pause_physics()
model_manager.clear_world()
sleep(0.5)
kinect_manager = GazeboKinectManager()
if kinect_manager.get_model_state().status_message == 'GetModelState: model does not exist':
kinect_manager.spawn_kinect()
sleep(0.5)
model_names = os.listdir(GDL_MODEL_PATH)
graspit_agg_h5 = choose_from(AGG_GRASPIT_DIR)
graspit_agg_name = graspit_agg_h5[:3]
graspit_grasp_dataset = GraspDataset(AGG_GRASPIT_DIR + graspit_agg_h5,
DATASET_TEMPLATE_PATH + "/dataset_configs/graspit_grasps_dataset.yaml")
gazebo_grasp_path = choose_from_or_none(RAW_GAZEBO_DIR)
if not gazebo_grasp_path:
# make a new dset
gazebo_grasp_path = RAW_GAZEBO_DIR + graspit_agg_name + "-" + get_date_string() + ".h5"
num_iter = 0
else:
num_iter = 1
gazebo_grasp_dataset = GraspDataset(gazebo_grasp_path,
DATASET_TEMPLATE_PATH + "/dataset_configs/gazebo_capture_config.yaml")
if num_iter:
gazebo_grasp_dataset.get_current_index()
from choose import choose_from
from grasp import get_model_grasps
from grasp_dataset import GraspDataset
def get_date_string():
t = time.localtime()
minute = str(t.tm_min)
if len(minute) == 1:
minute = '0' + minute
t_string = str(t.tm_mon) + "_" + str(t.tm_mday) + "_" + str(t.tm_hour) + "_" + minute
return t_string
if __name__ == "__main__":
graspit_dataset_dir = os.path.expanduser("~/grasp_deep_learning/data/grasp_datasets/0_raw_graspit/")
graspit_dir = choose_from(graspit_dataset_dir)
graspit_grasps_dir = (graspit_dataset_dir + graspit_dir) + '/'
graspit_agg_dir = os.path.expanduser("~/grasp_deep_learning/data/grasp_datasets/1_agg_graspit/")
rospack = rospkg.RosPack()
DATASET_TEMPLATE_PATH = rospack.get_path('grasp_dataset')
import IPython
IPython.embed()
assert False
grasp_dataset = GraspDataset(graspit_agg_dir + graspit_dir + "-" + get_date_string() + ".h5",
DATASET_TEMPLATE_PATH + "/dataset_configs/graspit_grasps_dataset.yaml")
grasps = []
from choose import choose_from
PATCH_SIZE = 72
VC_INDICES = [1, 8, 12, 16]
NUM_PATCHES = len(VC_INDICES)
def extract_patch(vc_uvd, rgbd_image):
vc_u, vc_v, vc_d = vc_uvd
return rgbd_image[vc_u-PATCH_SIZE/2:vc_u+PATCH_SIZE/2, vc_v-PATCH_SIZE/2:vc_v+PATCH_SIZE/2, :]
if __name__ == "__main__":
gazebo_condensed_file = choose_from(CONDENSED_GAZEBO_DIR)[:-3]
condensed_gazebo_grasp_dataset = GraspDataset(CONDENSED_GAZEBO_DIR + gazebo_condensed_file + ".h5",
DATASET_TEMPLATE_PATH + "/dataset_configs/gazebo_condensed_config.yaml")
dset = h5py.File(RAW_PYLEARN_DIR + gazebo_condensed_file + str(PATCH_SIZE) + ".h5")
num_grasps = condensed_gazebo_grasp_dataset.get_current_index()
num_grasp_types = condensed_gazebo_grasp_dataset.dset['grasp_type'][:].max() + 1
num_labels = num_grasp_types*NUM_PATCHES
dset.create_dataset("rgbd_patches", shape=(num_grasps*NUM_PATCHES, PATCH_SIZE, PATCH_SIZE, 4), chunks=(10, PATCH_SIZE, PATCH_SIZE, 4))
dset.create_dataset("rgbd_patch_labels", shape=(num_grasps*NUM_PATCHES, num_labels), chunks=(10, num_labels))
for i in range(num_grasps):
grasp = condensed_gazebo_grasp_dataset.get_grasp(i)
model_manager.pause_physics()
model_manager.clear_world()
sleep(0.5)
kinect_manager = GazeboKinectManager()
if kinect_manager.get_model_state(
).status_message == 'GetModelState: model does not exist':
kinect_manager.spawn_kinect()
sleep(0.5)
model_names = os.listdir(GDL_MODEL_PATH)
graspit_agg_h5 = choose_from(AGG_GRASPIT_DIR)
graspit_agg_name = graspit_agg_h5[:3]
graspit_grasp_dataset = GraspDataset(
AGG_GRASPIT_DIR + graspit_agg_h5,
DATASET_TEMPLATE_PATH + "/dataset_configs/graspit_grasps_dataset.yaml")
gazebo_grasp_path = choose_from_or_none(RAW_GAZEBO_DIR)
if not gazebo_grasp_path:
# make a new dset
gazebo_grasp_path = RAW_GAZEBO_DIR + graspit_agg_name + "-" + get_date_string(
) + ".h5"
num_iter = 0
else:
num_iter = 1
gazebo_grasp_dataset = GraspDataset(
gazebo_grasp_path,
def main():
dataset_file = choose.choose_from(paths.HEATMAPS_DATASET_DIR)
dataset = h5py.File(paths.HEATMAPS_DATASET_DIR + dataset_file)
print
print "dataset.keys(): " + str(dataset.keys())
print
for i in range(dataset['rgbd_data'].shape[0]):
rgbd_img = dataset['rgbd_data'][i]
heatmaps = dataset['heatmaps'][i]
convolved_priors = dataset['convolved_heatmaps'][i]
independent_x_priors = dataset['independent_x_priors'][i]
plotter2 = Plotter(2)
for j in range(dataset['heatmaps'].shape[-1]):
plotter2.add_subplot('heatmap_' + str(j), heatmaps[:, :, j]/heatmaps[:, :, j].max())
# plotter2.add_subplot('l_obs', heatmaps[:, :, 0]/heatmaps[:,:,0].max())
# plotter2.add_subplot('l_g_p * palm', palm_conv[0, :, :])
# plotter2.add_subplot('l_g_r * r_grip', r_conv[1, :, :])
# plotter2.add_subplot('l_convolved', convolved_heatmaps[:, :, 0])
# plotter2.add_subplot('p_obs', heatmaps[:, :, 1]/heatmaps[:,:,1].max())
# plotter2.add_subplot('p_g_l * l_grip', l_conv[2, :, :])
# plotter2.add_subplot('p_g_r * r_grip', r_conv[3, :, :])
# plotter2.add_subplot('p_convolved', convolved_heatmaps[:, :, 1])
# plotter2.add_subplot('r_obs', heatmaps[:, :, 2]/heatmaps[:,:,2].max())
# plotter2.add_subplot('r_g_p * palm', palm_conv[5, :, :])
# plotter2.add_subplot('r_g_l * l_grip', l_conv[4, :, :])
# plotter2.add_subplot('r_convolved', convolved_heatmaps[:, :, 2])
# palm_out = convolved_heatmaps[:, :, 1]
# palm_min = np.argmin(palm_out)
# palm_out_x, palm_out_y = (palm_min / palm_out.shape[1], palm_min % palm_out.shape[1])
#
# l_out = convolved_heatmaps[:, :, 0]
# l_min = np.argmin(l_out)
# l_out_x, l_out_y = (l_min / l_out.shape[1], l_min % l_out.shape[1])
#
# r_out = convolved_heatmaps[:, :, 2]
# r_min = np.argmin(r_out)
# r_out_x, r_out_y = (r_min / r_out.shape[1], r_min % r_out.shape[1])
#
# rgbd_overlay = np.copy(rgbd_img)
# x_offset = (rgbd_overlay.shape[0]-palm_out.shape[0])/2
# y_offset = (rgbd_overlay.shape[1]-palm_out.shape[1])/2
#
# rgbd_overlay[x_offset+ l_out_x-10:x_offset+l_out_x+10,y_offset+ l_out_y-10:y_offset+l_out_y+10, :] = 0
# rgbd_overlay[x_offset+ palm_out_x-10:x_offset+palm_out_x+10,y_offset+ palm_out_y-10:y_offset+palm_out_y+10, :] = 0
# rgbd_overlay[x_offset+ r_out_x-10:x_offset+r_out_x+10,y_offset+ r_out_y-10:y_offset+r_out_y+10, :] = 0
#
#
# plotter1 = Plotter(1)
#
# if rgbd_img.shape[-1] == 4:
# plotter2.add_subplot('rgb', rgbd_img[:, :, 0:3])
# plotter2.add_subplot('d', rgbd_img[:, :, 3])
# else:
# plotter2.add_subplot('d', rgbd_img[:,:,0])
# if rgbd_img.shape[-1] == 4:
# plotter2.add_subplot("convolved_overlay", rgbd_overlay[:, :, :-1])
# else:
# plotter2.add_subplot("convolved_overlay", rgbd_overlay[:, :, 0])
#plotter1.show()
plotter2.show()
plt.show()
slop = .01
bin_ranges = [(0, math.pi + slop), (0, 2.44 + slop), (0, .84 + slop),
(0, math.pi + slop), (0, 2.44 + slop), (0, .84 + slop),
(0, 2.44 + slop), (0, .84 + slop),
(-math.pi - slop, math.pi + slop)]
num_entries_per_bin = NUM_BINS_PER_JOINT
num_grasp_types = math.pow(num_entries_per_bin, len(bin_ranges))
bin_edges = []
for bin_range in bin_ranges:
hist, edges = np.histogram([], num_entries_per_bin, bin_range)
bin_edges.append(edges)
gazebo_raw_file = choose_from(RAW_GAZEBO_DIR)
gazebo_grasp_dataset = GraspDataset(
RAW_GAZEBO_DIR + gazebo_raw_file,
DATASET_TEMPLATE_PATH + "/dataset_configs/gazebo_capture_config.yaml")
count = 0
grasp_types = []
print "Iterating through raw grasps"
for grasp in gazebo_grasp_dataset.iterator():
if count % 500 == 0:
print str(count) + '/' + str(
gazebo_grasp_dataset.get_current_index())
bins = list(grasp.joint_values)
import paths
import choose
import matplotlib.pyplot as plt
import h5py
dataset_file = choose.choose_from(paths.PROCESSED_TRAINING_DATASET_DIR)
processed_rgbd_filepath = paths.PROCESSED_TRAINING_DATASET_DIR + dataset_file
dataset = h5py.File(processed_rgbd_filepath)
print dataset.keys()
train_patches = dataset['train_patches']
train_patch_labels = dataset['train_patch_labels']
i = 0
while True:
plt.figure(0)
for index in range(9):
plt.subplot(3, 3, index + 1)
plt.imshow(train_patches[i+index, :, :, 0:3])
plt.title(str(train_patch_labels[i+index]))
i += 9
plt.show()
output_keys=(
("train_patches", "train_patch_labels"),
("valid_patches", "valid_patch_labels"),
("test_patches", "test_patch_labels")
),
output_weights=(.8, .1, .1),
source_keys=("normalized_patches", "patch_labels")))
#now we swap around the axis so the data fits nicely onto the gpu
# C01B rather than B01C
pipeline.items.append(hdf5_data_preprocessors.MakeC01B())
#now lets actually make a new dataset and run it through the pipeline
if not os.path.exists(paths.PROCESSED_TRAINING_DATASET_DIR):
os.makedirs(paths.PROCESSED_TRAINING_DATASET_DIR)
hd5f_dataset = h5py.File(attribs["output_filepath"])
pipeline.apply(hd5f_dataset)
if __name__ == "__main__":
raw_data_filename = choose.choose_from(paths.RAW_TRAINING_DATASET_DIR)
raw_data_filepath = paths.RAW_TRAINING_DATASET_DIR + raw_data_filename
preprocess_attribs = dict(sets=("train", "test", "valid"),
raw_filepath=raw_data_filepath,
output_filepath=paths.PROCESSED_TRAINING_DATASET_DIR + 'processed_' + raw_data_filename)
preprocess_grasp_dataset(preprocess_attribs)
def main():
dataset_file = choose.choose_from(paths.HEATMAPS_DATASET_DIR)
dataset = h5py.File(paths.HEATMAPS_DATASET_DIR + dataset_file)
for i in range(dataset['rgbd_data'].shape[0]):
rgbd_img = dataset['rgbd_data'][i]
heatmaps = dataset['rescaled_heatmaps'][i]
#indepent_grasp_points = dataset["independent_grasp_points"][i]
#convolved_heatmaps = dataset['convolved_heatmaps'][i]
#dependent_grasp_points = dataset["dependent_grasp_points"][i]
#palm_conv = dataset["p_convolved_heatmaps"][i]
#l_conv = dataset["l_convolved_heatmaps"][i]
#r_conv = dataset["r_convolved_heatmaps"][i]
#best_grasp = dataset["best_grasp"][i]
plotter1 = Plotter(1)
plotter1.add_subplot('grey_scale', rgbd_img[:, :, 0])
#plotter1.add_subplot('output', heatmaps[:,:,0])
#plotter1.add_subplot('best_grasp', best_grasp)
# p_to_f_dist = 10
# l = heatmaps[:, p_to_f_dist*2:, 0]
# p = heatmaps[:, p_to_f_dist:-p_to_f_dist, 1]
# r = heatmaps[:, :-p_to_f_dist*2, 2]
#
# out = l * p * r
#
# plotter1.add_subplot("out", out)
#
titles = ['Right ankle',
'Right knee',
'Right hip',
'Left hip',
'Left knee',
'Left ankle',
'Right wrist',
'Right elbow',
'Right shoulder',
'Left shoulder',
'Left elbow',
'Left wrist',
'Neck',
'Head top']
x_offset = 32
y_offset = 32
for i in range(14):
out = heatmaps[:, :, i]
out_min = np.argmin(out)
out_x, out_y = (out_min / out.shape[1], out_min % out.shape[1])
out_min_plot = np.copy(rgbd_img)
marker_size = 10
out_min_plot[x_offset + out_x-marker_size : x_offset + out_x+marker_size, y_offset + out_y-marker_size : y_offset + out_y+marker_size] = 0
#plotter1.add_subplot(titles[i], out_min_plot)
plotter1.add_subplot(titles[i], out)
plotter1.show()
plt.show()
def get_date_string():
t = time.localtime()
minute = str(t.tm_min)
if len(minute) == 1:
minute = '0' + minute
t_string = str(t.tm_mon) + "_" + str(t.tm_mday) + "_" + str(
t.tm_hour) + "_" + minute
return t_string
if __name__ == "__main__":
graspit_dataset_dir = os.path.expanduser(
"~/grasp_deep_learning/data/grasp_datasets/0_raw_graspit/")
graspit_dir = choose_from(graspit_dataset_dir)
graspit_grasps_dir = (graspit_dataset_dir + graspit_dir) + '/'
graspit_agg_dir = os.path.expanduser(
"~/grasp_deep_learning/data/grasp_datasets/1_agg_graspit/")
rospack = rospkg.RosPack()
DATASET_TEMPLATE_PATH = rospack.get_path('grasp_dataset')
import IPython
IPython.embed()
assert False
grasp_dataset = GraspDataset(
graspit_agg_dir + graspit_dir + "-" + get_date_string() + ".h5",
DATASET_TEMPLATE_PATH + "/dataset_configs/graspit_grasps_dataset.yaml")
PATCH_SIZE = 72
VC_INDICES = [1, 8, 12, 16]
NUM_PATCHES = len(VC_INDICES)
def extract_patch(vc_uvd, rgbd_image):
vc_u, vc_v, vc_d = vc_uvd
return rgbd_image[vc_u - PATCH_SIZE / 2:vc_u + PATCH_SIZE / 2,
vc_v - PATCH_SIZE / 2:vc_v + PATCH_SIZE / 2, :]
if __name__ == "__main__":
gazebo_condensed_file = choose_from(CONDENSED_GAZEBO_DIR)[:-3]
condensed_gazebo_grasp_dataset = GraspDataset(
CONDENSED_GAZEBO_DIR + gazebo_condensed_file + ".h5",
DATASET_TEMPLATE_PATH +
"/dataset_configs/gazebo_condensed_config.yaml")
dset = h5py.File(RAW_PYLEARN_DIR + gazebo_condensed_file +
str(PATCH_SIZE) + ".h5")
num_grasps = condensed_gazebo_grasp_dataset.get_current_index()
num_grasp_types = condensed_gazebo_grasp_dataset.dset['grasp_type'][:].max(
) + 1
num_labels = num_grasp_types * NUM_PATCHES
dset.create_dataset("rgbd_patches",
(0, 2.44 + slop),
(0, .84 + slop),
(0, 2.44 + slop),
(0, .84 + slop),
(-math.pi - slop, math.pi + slop)]
num_entries_per_bin = NUM_BINS_PER_JOINT
num_grasp_types = math.pow(num_entries_per_bin, len(bin_ranges))
bin_edges = []
for bin_range in bin_ranges:
hist, edges = np.histogram([], num_entries_per_bin, bin_range)
bin_edges.append(edges)
gazebo_raw_file = choose_from(RAW_GAZEBO_DIR)
gazebo_grasp_dataset = GraspDataset(RAW_GAZEBO_DIR + gazebo_raw_file,
DATASET_TEMPLATE_PATH + "/dataset_configs/gazebo_capture_config.yaml")
count = 0
grasp_types = []
print "Iterating through raw grasps"
for grasp in gazebo_grasp_dataset.iterator():
if count % 500 == 0:
print str(count) + '/' + str(gazebo_grasp_dataset.get_current_index())
bins = list(grasp.joint_values)
bins.append(grasp.wrist_roll[0])