def __init__(self):
vae_checkpoint_folder = f'{graspnet_path}/checkpoints/latent_size_2_ngpus_1_gan_1_confidence_weight_0.1_npoints_1024_num_grasps_per_object_256_train_evaluator_0_'
evaluator_checkpoint_folder = f'{graspnet_path}/checkpoints/npoints_1024_train_evaluator_1_allowed_categories__ngpus_8_/'
self.vae_checkpoint_folder = vae_checkpoint_folder
self.evaluator_checkpoint_folder = evaluator_checkpoint_folder
self.grasp_conf_threshold = 0.8
self.gradient_based_refinement = False
cfg = grasp_estimator.joint_config(
self.vae_checkpoint_folder,
self.evaluator_checkpoint_folder,
)
cfg['threshold'] = self.grasp_conf_threshold
cfg['sample_based_improvement'] = 1 - int(
self.gradient_based_refinement)
cfg['num_refine_steps'] = 10 if self.gradient_based_refinement else 20
cfg['training'] = True
cfg['logdir'] = "log"
cfg["vae_checkpoint_folder"] = None
cfg["evaluator_checkpoint_folder"] = None
cfg["npoints"] = 1024
cfg["num_grasps_per_object"] = 64
self.cfg = cfg
self.num_refine_steps = cfg['num_refine_steps']
self.estimator = grasp_estimator.GraspEstimator(cfg)
self.sess = tf.Session()
ph = get_vae_placeholder_dict(cfg)
self.tf_output = self.estimator.build_network(ph)
def main(args):
parser = make_parser()
args = parser.parse_args(args)
cfg = grasp_estimator.joint_config(
args.vae_checkpoint_folder,
args.evaluator_checkpoint_folder,
)
cfg['threshold'] = args.threshold
cfg['sample_based_improvement'] = 1 - int(args.gradient_based_refinement)
cfg['num_refine_steps'] = 10 if args.gradient_based_refinement else 20
estimator = grasp_estimator.GraspEstimator(cfg)
os.environ['CUDA_VISIBLE_DEVICES'] = str(cfg.gpu)
sess = tf.Session()
estimator.build_network()
estimator.load_weights(sess)
for npy_file in glob.glob(os.path.join(args.npy_folder, '*.npy')):
print(npy_file)
# Depending on your numpy version you may need to change allow_pickle
# from True to False.
data = np.load(npy_file, allow_pickle=True).item()
print(data.keys())
for k in data.keys():
print(k, np.shape(data[k]))
depth = data['depth']
image = data['image']
K = data['intrinsics_matrix']
# Removing points that are farther than 1 meter or missing depth
# values.
depth[depth == 0] = np.nan
depth[depth > 1] = np.nan
pc, selection = backproject(depth,
K,
return_finite_depth=True,
return_selection=True)
pc_colors = image.copy()
pc_colors = np.reshape(pc_colors, [-1, 3])
pc_colors = pc_colors[selection, :]
# Smoothed pc comes from averaging the depth for 10 frames and removing
# the pixels with jittery depth between those 10 frames.
object_pc = data['smoothed_object_pc']
print(object_pc)
latents = estimator.sample_latents()
generated_grasps, generated_scores, _ = estimator.predict_grasps(
sess,
object_pc,
latents,
num_refine_steps=cfg.num_refine_steps,
)
mlab.figure(bgcolor=(1, 1, 1))
draw_scene(
pc,
pc_color=pc_colors,
grasps=generated_grasps,
grasp_scores=generated_scores,
)
print('close the window to continue to next object . . .')
mlab.show()
def main(args):
parser = make_parser()
args = parser.parse_args(args)
cfg = grasp_estimator.joint_config(
args.vae_checkpoint_folder,
args.evaluator_checkpoint_folder,
)
cfg['threshold'] = args.threshold
cfg['sample_based_improvement'] = 1 - int(args.gradient_based_refinement)
cfg['num_refine_steps'] = 10 if args.gradient_based_refinement else 20
estimator = grasp_estimator.GraspEstimator(cfg)
os.environ['CUDA_VISIBLE_DEVICES'] = str(cfg.gpu)
#gpu_options = tf.GPUOptions(per_process_gpu_memory_fraction=0.25)
config = tf.ConfigProto()
#config=tf.ConfigProto(gpu_options=gpu_options)
config.gpu_options.allow_growth = True
sess = tf.Session(config=config)
#sess = tf.Session()
estimator.build_network()
estimator.load_weights(sess)
msg = rospy.wait_for_message('/cloud_pcd', PointCloud2)
pc = ros_numpy.numpify(msg)
points = np.zeros((pc.shape[0], 3))
colors = 100 * np.ones((pc.shape[0], 3))
points[:, 0] = pc['x']
points[:, 1] = pc['y']
points[:, 2] = pc['z']
latents = estimator.sample_latents()
generated_grasps, generated_scores, _ = estimator.predict_grasps(
sess,
points,
latents,
num_refine_steps=cfg.num_refine_steps,
)
#print(np.array(generated_grasps).shape)
print(type(generated_grasps))
scores_np = np.asarray(generated_scores)
sorting = np.argsort(-scores_np)
sorted_scores = scores_np[sorting]
grasps_np = np.asarray(generated_grasps)
sorted_grasps = grasps_np[sorting]
mlab.figure(bgcolor=(1, 1, 1))
if len(generated_scores) != 0:
draw_scene(points,
pc_color=colors,
grasps=sorted_grasps[0:20].tolist(),
grasp_scores=sorted_scores[0:20].tolist())
else:
draw_scene(
points,
pc_color=colors,
)
print('close the window to continue to next object . . .')
mlab.show()
def configure(self, cfg: dict):
"""Additional class configuration
Parameters
----------
cfg : dict configuration dict, as sourced from the YAML file
"""
# Create a namespace from the config dict
# Since the GraspNet implementation uses a namespace
self.cfg_ns = SimpleNamespace(**self.cfg)
self.cfg_grasp_estimator = grasp_estimator.joint_config(
self.cfg_ns.vae_checkpoint_folder,
self.cfg_ns.evaluator_checkpoint_folder,
)
self.cfg_grasp_estimator['threshold'] = self.cfg_ns.threshold
self.cfg_grasp_estimator['sample_based_improvement'] = 1 - int(
self.cfg_ns.gradient_based_refinement)
self.cfg_grasp_estimator[
'num_refine_steps'] = 10 if self.cfg_ns.gradient_based_refinement else 20
if return_finite_depth:
selection = np.isfinite(X[:, 0])
X = X[selection, :]
if return_selection:
return X, selection
return X
## The model is loaded as a global object ##
parser = make_parser()
args = parser.parse_args(sys.argv[1:])
cfg = grasp_estimator.joint_config(
args.vae_checkpoint_folder,
args.evaluator_checkpoint_folder,
)
cfg['threshold'] = args.threshold
cfg['sample_based_improvement'] = 1 - int(args.gradient_based_refinement)
cfg['num_refine_steps'] = 10 if args.gradient_based_refinement else 20
estimator = grasp_estimator.GraspEstimator(cfg)
os.environ['CUDA_VISIBLE_DEVICES'] = str(cfg.gpu)
#gpu_options = tf.GPUOptions(per_process_gpu_memory_fraction=0.25)
config = tf.ConfigProto()
#config=tf.ConfigProto(gpu_options=gpu_options)
config.gpu_options.allow_growth = True
sess = tf.Session(config=config)
#sess = tf.Session()
estimator.build_network()
estimator.load_weights(sess)
grasp_success: list of binary numbers. 0 means that the grasp failed,
and 1 means that the grasp succeeded.
"""
raise NotImplementedError(
"The code for grasp evaluation is not released")
def __del__(self):
del self._grasp_reader
if __name__ == '__main__':
args = make_parser(sys.argv)
create_directory(args.output_folder)
cfg = grasp_estimator.joint_config(args.vae_training_folder,
args.evaluator_training_folder,
args.dataset_root_folder,
args.eval_split)
cfg['threshold'] = -1
if args.gradient_based_refinement:
cfg['num_refine_steps'] = 10
cfg['sample_based_improvement'] = 0
else:
cfg['num_refine_steps'] = 20
cfg['sample_based_improvement'] = 1
evaluator = Evaluator(
cfg,
args.generate_data_if_missing,
args.output_folder,
args.eval_data_folder,
args.num_experiments,
def main(args):
sock = su.initialize_client('localhost', 7777)
parser = make_parser()
args = parser.parse_args(args)
cfg = grasp_estimator.joint_config(
args.vae_checkpoint_folder,
args.evaluator_checkpoint_folder,
)
cfg['threshold'] = args.threshold
cfg['sample_based_improvement'] = 1 - int(args.gradient_based_refinement)
cfg['num_refine_steps'] = 10 if args.gradient_based_refinement else 20
estimator = grasp_estimator.GraspEstimator(cfg)
os.environ['CUDA_VISIBLE_DEVICES'] = str(cfg.gpu)
sess = tf.Session()
estimator.build_network()
estimator.load_weights(sess)
while True:
data = su.recvall_pickle(sock)
print(data.keys())
for k in data.keys():
print(k, np.shape(data[k]))
depth = data['depth']
image = data['image']
K = data['intrinsics_matrix']
# Removing points that are farther than 1 meter or missing depth
# values.
depth[depth == 0] = np.nan
depth[depth > 1] = np.nan
pc, selection = backproject(depth, K, return_finite_depth=True, return_selection=True)
pc_colors = image.copy()
pc_colors = np.reshape(pc_colors, [-1, 3])
pc_colors = pc_colors[selection, ::-1]
# down sampling
idx = np.random.choice(pc_colors.shape[0], 100000, replace=False)
pc = pc[idx, :]
pc_colors = pc_colors[idx, :]
# Smoothed pc comes from averaging the depth for 10 frames and removing
# the pixels with jittery depth between those 10 frames.
object_pc = data['smoothed_object_pc']
latents = estimator.sample_latents()
generated_grasps, generated_scores, _ = estimator.predict_grasps(
sess,
object_pc,
latents,
num_refine_steps=cfg.num_refine_steps,
)
print("====>", generated_grasps, generated_scores)
mlab.figure(bgcolor=(1,1,1))
if len(generated_grasps) != 0:
draw_scene(
pc,
pc_color=pc_colors,
grasps=generated_grasps,
grasp_scores=generated_scores,
)
print('close the window to continue to next object . . .')
mlab.show()
su.sendall_pickle(sock, [generated_grasps, generated_scores])
def __init__(self, config: Config):
self.config = config
self.vae_checkpoint_folder = config.vae_checkpoint_folder
self.evaluator_checkpoint_folder = config.evaluator_checkpoint_folder
self.gradient_based_refinement = False
self.grasp_conf_threshold = 0.8
self.cut_off_points = config.cut_off_points
self.output_grasps_dir = "vae_generated_grasps"
self.fix_view = config.fix_view
##### Prepare the 6dof graspnet network for forward pass ######
cfg = grasp_estimator.joint_config(
self.vae_checkpoint_folder,
self.evaluator_checkpoint_folder,
)
cfg['threshold'] = self.grasp_conf_threshold
cfg['sample_based_improvement'] = 1 - int(
self.gradient_based_refinement)
cfg['num_refine_steps'] = 10 if self.gradient_based_refinement else 20
if self.config.data_collection_mode:
if not self.config.data_collection_from_trained_model:
cfg["use_geometry_sampling"] = True
cfg['num_refine_steps'] = 0
cfg['grasp_selection_mode'] = "all"
#cfg['num_refine_steps'] = 0
self.num_refine_steps = cfg['num_refine_steps']
self.estimator = grasp_estimator.GraspEstimator(cfg)
self.sess = tf.Session()
self.estimator.build_network()
self.estimator.load_weights(self.sess)
if not os.path.exists(self.output_grasps_dir):
os.makedirs(self.output_grasps_dir)
# set camera for this:
self.camera_positions, self.camera_quats = pcp_utils.cameras.generate_new_cameras_hemisphere(
radius=self.config.camera_radius,
lookat_point=self.config.camera_lookat_pos,
pitch=self.config.camera_pitch,
yaw=self.config.camera_yaw,
yaw_list=self.config.camera_yaw_list)
self.n_cams = len(self.camera_positions)
# i don't think we need this
mujoco_T_adam = np.array([[1, 0, 0], [0, 0, 1], [0, -1, 0]],
dtype=np.float32)
origin_T_camR_xpos = np.array(config.table_top, np.float32) + np.array(
config.table_T_camR, np.float)
origin_T_adam = np.zeros((4, 4), dtype=np.float32)
origin_T_adam[:3, :3] = mujoco_T_adam
origin_T_adam[:3, 3] = origin_T_camR_xpos
origin_T_adam[3, 3] = 1
self.origin_T_adam = origin_T_adam
self.adam_T_origin = np.linalg.inv(self.origin_T_adam)
gripper_pc = np.squeeze(tf_utils.get_control_point_tensor(1, False), 0)
gripper_pc[2, 2] = 0.059
gripper_pc[3, 2] = 0.059
mid_point = 0.5 * (gripper_pc[2, :] + gripper_pc[3, :])
# modified grasps
modified_gripper_pc = []
modified_gripper_pc.append(np.zeros((3, ), np.float32))
modified_gripper_pc.append(mid_point)
modified_gripper_pc.append(gripper_pc[2])
modified_gripper_pc.append(gripper_pc[4])
modified_gripper_pc.append(gripper_pc[2])
modified_gripper_pc.append(gripper_pc[3])
modified_gripper_pc.append(gripper_pc[5])
self.gripper_pc_ori = np.asarray(modified_gripper_pc)
def main(config_file, task_config_file, output_file):
config = utils.config_from_yaml_file(config_file)
# object configurations
objs_config = utils.config_from_yaml_file(task_config_file)
objects_to_get_grasps_for = list()
for obj_name, obj_config in objs_config['objs'].items():
obj_config_class = MeshObject.Config().update(obj_config)
obj = MeshObject(obj_config_class, obj_name)
objects_to_get_grasps_for.append(obj)
print(f'found {len(objects_to_get_grasps_for)} objects to find grasps for')
updated_config = Grasp6d_graspnet.Config().update(config)
grasper = Grasp6d_graspnet(updated_config)
##### Prepare the 6dof graspnet network for forward pass ######
cfg = grasp_estimator.joint_config(
grasper.vae_checkpoint_folder,
grasper.evaluator_checkpoint_folder,
)
cfg['threshold'] = grasper.grasp_conf_threshold
cfg['sample_based_improvement'] = 1 - int(
grasper.gradient_based_refinement)
cfg['num_refine_steps'] = 10 if grasper.gradient_based_refinement else 20
estimator = grasp_estimator.GraspEstimator(cfg)
sess = tf.Session()
estimator.build_network()
estimator.load_weights(sess)
##### End of 6dof graspnet network preparation ################
nmeshes = len(objects_to_get_grasps_for)
for mesh_id, meshobj in enumerate(objects_to_get_grasps_for):
print(f'---- processing {mesh_id}/{nmeshes} ------')
obj_pc, pc, pc_colors, env_state = grasper.add_object(meshobj)
# the object is added, data is collected, processed and returned
# now sample the grasps, and save them
latents = estimator.sample_latents()
generated_grasps, generated_scores, _ = estimator.predict_grasps(
sess,
obj_pc,
latents,
num_refine_steps=cfg.num_refine_steps,
)
print(
f'------ number of generated grasps are: {len(generated_grasps)} ---------'
)
save_file_path = os.path.join(grasper.output_grasps_dir,
f'grasps_{meshobj.name}.npy')
print(f'---- saving to {save_file_path} -----')
save_dict = {
'generated_grasps': generated_grasps,
'generated_scores': generated_scores,
'pcd': pc,
'pcd_color': pc_colors,
'obj_pcd': obj_pc,
'env_state': env_state,
}
np.save(save_file_path, save_dict)
def main(args):
parser = make_parser()
args = parser.parse_args(args)
cfg = grasp_estimator.joint_config(
args.vae_checkpoint_folder,
args.evaluator_checkpoint_folder,
)
cfg['threshold'] = args.threshold
cfg['sample_based_improvement'] = 1 - int(args.gradient_based_refinement)
cfg['num_refine_steps'] = 10 if args.gradient_based_refinement else 20
cfg['num_samples'] = 20
estimator = grasp_estimator.GraspEstimator(cfg)
os.environ['CUDA_VISIBLE_DEVICES'] = str(cfg.gpu)
#edit by alessandro
os.environ['CUDA_VISIBLE_DEVICES'] ='0'
config = tf.ConfigProto()
config.gpu_options.allow_growth = True
sess = tf.Session(config=config)
#sess = tf.Session()
#
estimator.build_network()
estimator.load_weights(sess)
for npy_file in glob.glob(os.path.join(args.npy_folder, '*.npy')):
print(npy_file)
# Depending on your numpy version you may need to change allow_pickle
# from True to False.
data = np.load(npy_file, allow_pickle=True).item()
print(data.keys())
depth = data['depth']
image = data['image']
K = data['intrinsics_matrix']
# Removing points that are farther than 1 meter or missing depth
# values.
depth[depth == 0] = np.nan
depth[depth > 1] = np.nan
pc, selection = backproject(depth, K, return_finite_depth=True, return_selection=True)
pc_colors = image.copy()
pc_colors = np.reshape(pc_colors, [-1, 3])
pc_colors = pc_colors[selection, :]
ipdb.set_trace()
# Smoothed pc comes from averaging the depth for 10 frames and removing
# the pixels with jittery depth between those 10 frames.
object_pc = data['smoothed_object_pc']
latents = estimator.sample_latents()
###################################
#### Test PCD #####################
#import open3d as o3d
#from matplotlib import pyplot
#from mpl_toolkits.mplot3d import Axes3D
#pcd = o3d.io.read_point_cloud("./demo/data/tomato_soup_0.pcd")
#object_pc = np.asarray(pcd.points)
#color_c1 = 200*np.ones((object_pc.shape[0],1))
#color_c23 = np.zeros((object_pc.shape[0],2))
#pc_colors = np.concatenate((color_c1,color_c23),axis=1)
#pc = object_pc
###################################
print('HERE1')
generated_grasps, generated_scores, _ = estimator.predict_grasps(
sess,
object_pc,
latents,
num_refine_steps=cfg.num_refine_steps,
)
print('HERE2')
mlab.figure(bgcolor=(1,1,1))
draw_scene(
pc,
pc_color=pc_colors,
grasps=generated_grasps,
grasp_scores=generated_scores,
)
print('close the window to continue to next object . . .')
mlab.show()