def test_grasp_quality_functions(self): num_grasps = NUM_TEST_CASES of = ObjFile(OBJ_FILENAME) sf = SdfFile(SDF_FILENAME) mesh = of.read() sdf = sf.read() obj = GraspableObject3D(sdf, mesh) gripper = RobotGripper.load(GRIPPER_NAME) ags = UniformGraspSampler(gripper, CONFIG) grasps = ags.generate_grasps(obj, target_num_grasps=num_grasps) # test with grasp quality function quality_config = GraspQualityConfigFactory.create_config( CONFIG['metrics']['force_closure']) quality_fn = GraspQualityFunctionFactory.create_quality_function( obj, quality_config) for i, grasp in enumerate(grasps): success, c = grasp.close_fingers(obj) if success: c1, c2 = c fn_fc = quality_fn(grasp).quality true_fc = PointGraspMetrics3D.force_closure( c1, c2, quality_config.friction_coef) self.assertEqual(fn_fc, true_fc)
def test_contacts(self): num_samples = 128 mu = 0.5 num_faces = 8 num_grasps = NUM_TEST_CASES of = ObjFile(OBJ_FILENAME) sf = SdfFile(SDF_FILENAME) mesh = of.read() sdf = sf.read() obj = GraspableObject3D(sdf, mesh) gripper = RobotGripper.load(GRIPPER_NAME) ags = UniformGraspSampler(gripper, CONFIG) grasps = ags.generate_grasps(obj, target_num_grasps=num_grasps) for grasp in grasps: success, c = grasp.close_fingers(obj) if success: c1, c2 = c # test friction cones fc1_exists, fc1, n1 = c1.friction_cone(num_cone_faces=num_faces, friction_coef=mu) if fc1_exists: self.assertEqual(fc1.shape[1], num_faces) alpha = np.tile(fc1.T.dot(c1.normal), [3,1]).T w = np.tile(c1.normal.reshape(3,1), [1,8]).T tan_vecs = fc1.T - alpha * w self.assertTrue(np.allclose(np.linalg.norm(tan_vecs, axis=1), mu)) fc2_exists, fc2, n2 = c2.friction_cone(num_cone_faces=num_faces, friction_coef=mu) if fc2_exists: self.assertEqual(fc2.shape[1], num_faces) alpha = np.tile(fc2.T.dot(c2.normal), [3,1]).T w = np.tile(c2.normal.reshape(3,1), [1,8]).T tan_vecs = fc2.T - alpha * w self.assertTrue(np.allclose(np.linalg.norm(tan_vecs, axis=1), mu)) # test reference frames T_contact1_obj = c1.reference_frame(align_axes=True) self.assertTrue(np.allclose(T_contact1_obj.z_axis, c1.in_direction)) self.assertTrue(np.allclose(T_contact1_obj.translation, c1.point)) for i in range(num_samples): theta = 2 * i * np.pi / num_samples v = np.cos(theta) * T_contact1_obj.x_axis + np.sin(theta) * T_contact1_obj.y_axis self.assertLessEqual(v[0], T_contact1_obj.x_axis[0]) T_contact2_obj = c2.reference_frame(align_axes=True) self.assertTrue(np.allclose(T_contact2_obj.z_axis, c2.in_direction)) self.assertTrue(np.allclose(T_contact2_obj.translation, c2.point)) for i in range(num_samples): theta = 2 * i * np.pi / num_samples v = np.cos(theta) * T_contact2_obj.x_axis + np.sin(theta) * T_contact2_obj.y_axis self.assertLessEqual(v[0], T_contact2_obj.x_axis[0])
def antipodal_grasp_sampler_for_storing(obj_graspable): gripper = RobotGripper.load( GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers') ags = AntipodalGraspSampler(gripper, CONFIG) grasps = ags.generate_grasps(obj_graspable, target_num_grasps=100, max_iter=4) return grasps
def database_delete_grasps(self, object_name): # load gripper gripper = RobotGripper.load(GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers') # open Dex-Net API dexnet_handle = DexNet() dexnet_handle.open_database(self.database_path) dexnet_handle.open_dataset(self.dataset_name) dexnet_handle.dataset.delete_grasps(object_name, gripper=gripper.name) dexnet_handle.close_database()
def get_object_mesh(self, object_name): # load gripper gripper = RobotGripper.load( GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers') # open Dex-Net API dexnet_handle = DexNet() dexnet_handle.open_database(self.database_path) dexnet_handle.open_dataset(self.dataset_name) object_mesh = dexnet_handle.dataset.mesh(object_name) return object_mesh
def get_stable_pose_transform(self, object_name, stable_pose_id): # load gripper gripper = RobotGripper.load( GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers') # open Dex-Net API dexnet_handle = DexNet() dexnet_handle.open_database(self.database_path) dexnet_handle.open_dataset(self.dataset_name) stable_pose = dexnet_handle.dataset.stable_pose( object_name, stable_pose_id=('pose_' + str(stable_pose_id))) dexnet_handle.close_database() return stable_pose
def database_save(self, object_name, filepath, stable_poses_n, force_overwrite=False): # load gripper gripper = RobotGripper.load(GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers') # open Dex-Net API dexnet_handle = DexNet() dexnet_handle.open_database(self.database_path) dexnet_handle.open_dataset(self.dataset_name) mass = CONFIG['default_mass'] if object_name in dexnet_handle.dataset.object_keys: graspable = dexnet_handle.dataset[object_name] mesh = graspable.mesh sdf = graspable.sdf stable_poses = dexnet_handle.dataset.stable_poses(object_name) else: # Create temp dir if cache dir is not provided mp_cache = CONFIG['cache_dir'] del_cache = False if mp_cache is None: mp_cache = tempfile.mkdtemp() del_cache = True # open mesh preprocessor mesh_processor = MeshProcessor(filepath, mp_cache) mesh_processor.generate_graspable(CONFIG) mesh = mesh_processor.mesh sdf = mesh_processor.sdf stable_poses = mesh_processor.stable_poses[:stable_poses_n] # write graspable to database dexnet_handle.dataset.create_graspable(object_name, mesh, sdf, stable_poses, mass=mass) # write grasps to database grasps, metrics = antipodal_grasp_sampler_for_storing(mesh, sdf, stable_poses) id = 0 for grasps_for_pose, metrics_for_pose in zip(grasps, metrics): if (grasps_for_pose == None): continue dexnet_handle.dataset.store_grasps_and_metrics(object_name, grasps_for_pose, metrics_for_pose, gripper=gripper.name, stable_pose_id=('pose_'+str(id)), force_overwrite=force_overwrite) id = id + 1 dexnet_handle.close_database()
def get_stable_pose(self, object_name, stable_pose_id): # load gripper gripper = RobotGripper.load(GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers') # open Dex-Net API dexnet_handle = DexNet() dexnet_handle.open_database(self.database_path) dexnet_handle.open_dataset(self.dataset_name) # read the most robust grasp stable_pose = dexnet_handle.dataset.stable_pose(object_name, stable_pose_id=('pose_'+str(stable_pose_id))) dexnet_handle.close_database() pose_matrix = np.eye(4,4) pose_matrix[:3,:3] = stable_pose.T_obj_world.rotation pose_matrix[:3, 3] = stable_pose.T_obj_world.translation return pose_matrix
def test_antipodal_grasp_sampler(self): of = ObjFile(OBJ_FILENAME) sf = SdfFile(SDF_FILENAME) mesh = of.read() sdf = sf.read() obj = GraspableObject3D(sdf, mesh) gripper = RobotGripper.load(GRIPPER_NAME) ags = AntipodalGraspSampler(gripper, CONFIG) grasps = ags.generate_grasps(obj, target_num_grasps=NUM_TEST_CASES) # test with raw force closure function for i, grasp in enumerate(grasps): success, c = grasp.close_fingers(obj) if success: c1, c2 = c self.assertTrue(PointGraspMetrics3D.force_closure(c1, c2, CONFIG['sampling_friction_coef']))
def antipodal_grasp_sampler(self): of = ObjFile(OBJ_FILENAME) sf = SdfFile(SDF_FILENAME) mesh = of.read() sdf = sf.read() obj = GraspableObject3D(sdf, mesh) gripper = RobotGripper.load(GRIPPER_NAME) ags = AntipodalGraspSampler(gripper, CONFIG) grasps = ags.generate_grasps(obj, target_num_grasps=10) quality_config = GraspQualityConfigFactory.create_config( CONFIG['metrics']['force_closure']) quality_fn = GraspQualityFunctionFactory.create_quality_function( obj, quality_config) q_to_c = lambda quality: CONFIG['quality_scale'] i = 0 vis.figure() vis.mesh(obj.mesh.trimesh, style='surface') for grasp in grasps: print(grasp.frame) success, c = grasp.close_fingers(obj) if success: c1, c2 = c fn_fc = quality_fn(grasp).quality true_fc = PointGraspMetrics3D.force_closure( c1, c2, quality_config.friction_coef) #print(grasp) T_obj_world = RigidTransform(from_frame='obj', to_frame='world') color = plt.get_cmap('hsv')(q_to_c(CONFIG['metrics']))[:-1] T_obj_gripper = grasp.gripper_pose(gripper) #vis.grasp(grasp,grasp_axis_color=color,endpoint_color=color) i += 1 #T_obj_world = RigidTransform(from_frame='obj',to_frame='world') vis.show(False)
def antipodal_grasp_sampler_for_storing_stable_poses(mesh, sdf, stable_poses): mass = 1.0 CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE obj = GraspableObject3D(sdf, mesh) gripper = RobotGripper.load( GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers') ags = AntipodalGraspSampler(gripper, CONFIG) quality_config = GraspQualityConfigFactory.create_config( CONFIG['metrics']['force_closure']) quality_function = GraspQualityFunctionFactory.create_quality_function( obj, quality_config) max_poses = len(stable_poses) grasps = [None] * max_poses metrics = [None] * max_poses all_grasps = ags.generate_grasps(obj, target_num_grasps=200, max_iter=4) for id, stable_pose in enumerate(stable_poses): print('sampling for stable pose: ', id) if id == max_poses: break grasps_pose = filter( lambda x: grasp_is_parallel_to_table(x.axis, stable_pose.r), all_grasps) #grasps_pose = ags.generate_grasps(obj,target_num_grasps=20, max_iter=5, stable_pose=stable_pose.r) grasps[id] = [] metrics[id] = [] for grasp in grasps_pose: quality = quality_function.quality(grasp) #quality = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config) grasps[id].append(copy.deepcopy(grasp)) metrics[id].append(copy.deepcopy(quality.quality)) return grasps, metrics
def test_init_gripper(self): gripper = RobotGripper.load(GRIPPER_NAME)
# def camera_intrinsics_callback(data): # """ Callback for Camera Intrinsics """ # global ir_intrinsics # ir_intrinsics = per.CameraIntrinsics('primesense_overhead', data.K[0], data.K[4], data.K[2], data.K[5], data.K[1], data.height, data.width) if __name__ == '__main__': # initialize the ROS node rospy.init_node('Yumi_Control_Node') rospy.loginfo('Initializing YuMi') config = YamlConfig( '/home/autolab/Workspace/vishal_working/catkin_ws/src/gqcnn/cfg/ros_nodes/yumi_control_node.yaml' ) robot, subscriber, arm, home_pose = init_robot(config) rospy.loginfo('Loading Gripper') gripper = RobotGripper.load('yumi_metal_spline') rospy.loginfo('Loading T_camera_world') T_camera_world = RigidTransform.load( '/home/autolab/Public/alan/calib/primesense_overhead/primesense_overhead_to_world.tf' ) # create a subscriber to get GQCNN Grasp rospy.loginfo('Subscribing to GQCNN Grasp Topic') rospy.Subscriber('/gqcnn_grasp', GQCNNGrasp, grasp_callback) rospy.spin()
def _set_gripper(self): return RobotGripper.load(self.config['gripper'])
T_camera_robot = RigidTransform( rotation=RigidTransform.random_rotation(), translation=RigidTransform.random_translation(), from_frame='camera', to_frame='robot') # fake transformation from a known 3D object model frame of reference to the camera frame of reference # in practice this could be computed using point cloud registration T_obj_camera = RigidTransform( rotation=RigidTransform.random_rotation(), translation=RigidTransform.random_translation(), from_frame='obj', to_frame='camera') # load gripper gripper = RobotGripper.load(gripper_name) # open Dex-Net API dexnet_handle = DexNet() dexnet_handle.open_database(database_path) dexnet_handle.open_dataset(dataset_name) # read the most robust grasp sorted_grasps = dexnet_handle.dataset.sorted_grasps(object_name, metric_name, gripper=gripper_name) most_robust_grasp = sorted_grasps[0][ 0] # Note: in general this step will require collision checking # transform into the robot reference frame for control T_gripper_obj = most_robust_grasp.gripper_pose(gripper)
def worker(i, sample_nums, grasp_amount, good_grasp): #主要是抓取采样器以及打分 100 20 """ brief: 对制定的模型,利用随机采样算法,进行抓取姿态的检测和打分 param [in] i 处理第i个mesh模型 param [in] sample_nums 每个对象模型返回的目标抓取数量 param [in] grasp_amount """ #截取目标对象名称 object_name = file_list_all[i][len(home_dir) + 35:] print('a worker of task {} start'.format(object_name)) #读取初始配置文件,读取并在内存中复制了一份 yaml_config = YamlConfig(home_dir + "/code/dex-net/test/config.yaml") #设置夹名称 gripper_name = 'panda' #根据设置的夹爪名称加载夹爪配置 gripper = RobotGripper.load(gripper_name, home_dir + "/code/dex-net/data/grippers") #设置抓取采样的方法 grasp_sample_method = "antipodal" if grasp_sample_method == "uniform": ags = UniformGraspSampler(gripper, yaml_config) elif grasp_sample_method == "gaussian": ags = GaussianGraspSampler(gripper, yaml_config) elif grasp_sample_method == "antipodal": #使用对映点抓取,输入夹爪与配置文件 ags = AntipodalGraspSampler(gripper, yaml_config) elif grasp_sample_method == "gpg": ags = GpgGraspSampler(gripper, yaml_config) elif grasp_sample_method == "point": ags = PointGraspSampler(gripper, yaml_config) else: raise NameError("Can't support this sampler") print("Log: do job", i) #设置obj模型文件与sdf文件路径 if os.path.exists(str(file_list_all[i]) + "/google_512k/nontextured.obj") and os.path.exists( str(file_list_all[i]) + "/google_512k/nontextured.sdf"): of = ObjFile(str(file_list_all[i]) + "/google_512k/nontextured.obj") sf = SdfFile(str(file_list_all[i]) + "/google_512k/nontextured.sdf") else: print("can't find any obj or sdf file!") raise NameError("can't find any obj or sdf file!") #根据路径读取模型与sdf文件 mesh = of.read() sdf = sf.read() #构建抓取模型类 obj = GraspableObject3D(sdf, mesh) print("Log: opened object", i + 1, object_name) ######################################### #设置 force_closure_quality_config = {} #设置力闭合 字典 canny_quality_config = {} #生成一个起点是2.0终点是0.75 步长为-0.4 (递减)的等距数列fc_list_sub1 (2.0, 0.75, -0.4) fc_list_sub1 = np.arange(2.0, 0.75, -0.3) #生成一个起点是0.5终点是0.36 步长为-0.05的等距数列fc_list_sub2 (0.5, 0.36, -0.05) fc_list_sub2 = np.arange(0.5, 0.36, -0.1) #将上面两个向量接起来,变成一个长条向量,使用不同的步长,目的是为了在更小摩擦力的时候,有更多的分辨率 fc_list = np.concatenate([fc_list_sub1, fc_list_sub2]) print("判断摩擦系数") print(fc_list) for value_fc in fc_list: #对value_fc保留2位小数,四舍五入 value_fc = round(value_fc, 2) #更改内存中配置中的摩擦系数,而没有修改硬盘中的yaml文件 yaml_config['metrics']['force_closure']['friction_coef'] = value_fc yaml_config['metrics']['robust_ferrari_canny'][ 'friction_coef'] = value_fc #把每个摩擦力值当成键, force_closure_quality_config[ value_fc] = GraspQualityConfigFactory.create_config( yaml_config['metrics']['force_closure']) canny_quality_config[ value_fc] = GraspQualityConfigFactory.create_config( yaml_config['metrics']['robust_ferrari_canny']) #####################准备开始采样############################ #填充一个与摩擦数量相同的数组,每个对应的元素都是0 good_count_perfect = np.zeros(len(fc_list)) count = 0 #设置每个摩擦值需要计算的最少抓取数量 (根据指定输入值20) minimum_grasp_per_fc = grasp_amount #如果每个摩擦系数下,有效的抓取(满足力闭合或者其他判断标准)小于要求值,就一直循环查找,直到所有摩擦系数条件下至少都存在20个有效抓取 while np.sum(good_count_perfect < minimum_grasp_per_fc) != 0: #开始使用antipodes sample获得对映随机抓取,此时并不判断是否满足力闭合,只是先采集满足夹爪条件的抓取 #如果一轮多次随机采样之后,发现无法获得指定数量的随机抓取,就会重复迭代计算3次,之后放弃,并把已经找到的抓取返回来 grasps = ags.generate_grasps(obj, target_num_grasps=sample_nums, grasp_gen_mult=10, max_iter=10, vis=False, random_approach_angle=True) count += len(grasps) #循环对每个采样抓取进行判断 for j in grasps: tmp, is_force_closure = False, False #循环对某个采样抓取应用不同的抓取摩擦系数,判断是否是力闭合 for ind_, value_fc in enumerate(fc_list): value_fc = round(value_fc, 2) tmp = is_force_closure #判断在当前给定的摩擦系数下,抓取是否是力闭合的 is_force_closure = PointGraspMetrics3D.grasp_quality( j, obj, force_closure_quality_config[value_fc], vis=False) #假设当前,1号摩擦力为1.6 抓取不是力闭合的,但是上一个0号摩擦系数2.0 条件下抓取是力闭合的 if tmp and not is_force_closure: #当0号2.0摩擦系数条件下采样的good抓取数量还不足指定的最低数量20 if good_count_perfect[ind_ - 1] < minimum_grasp_per_fc: #以0号摩擦系数作为边界 canny_quality = PointGraspMetrics3D.grasp_quality( j, obj, canny_quality_config[round(fc_list[ind_ - 1], 2)], vis=False) good_grasp.append((j, round(fc_list[ind_ - 1], 2), canny_quality)) #在0号系数的good抓取下计数加1 good_count_perfect[ind_ - 1] += 1 #当前抓取j的边界摩擦系数找到了,退出摩擦循环,判断下一个抓取 break #如果当前1号摩擦系数1.6条件下,该抓取j本身就是力闭合的,且摩擦系数是列表中的最后一个(所有的摩擦系数都判断完了) elif is_force_closure and value_fc == fc_list[-1]: if good_count_perfect[ind_] < minimum_grasp_per_fc: #以当前摩擦系数作为边界 canny_quality = PointGraspMetrics3D.grasp_quality( j, obj, canny_quality_config[value_fc], vis=False) good_grasp.append((j, value_fc, canny_quality)) good_count_perfect[ind_] += 1 #当前抓取j关于当前摩擦系数1.6判断完毕,而且满足所有的摩擦系数,就换到下一个摩擦系数 break print('Object:{} GoodGrasp:{}'.format(object_name, good_count_perfect)) #判断 object_name_len = len(object_name) object_name_ = str(object_name) + " " * (25 - object_name_len) if count == 0: good_grasp_rate = 0 else: good_grasp_rate = len(good_grasp) / count print('Gripper:{} Object:{} Rate:{:.4f} {}/{}'.format( gripper_name, object_name_, good_grasp_rate, len(good_grasp), count))
def worker(i, sample_nums, grasp_amount, good_grasp): object_name = file_list_all[i][len(home_dir) + 35:] print('a worker of task {} start'.format(object_name)) yaml_config = YamlConfig(home_dir + "/code/grasp-pointnet/dex-net/test/config.yaml") gripper_name = 'robotiq_85' gripper = RobotGripper.load( gripper_name, home_dir + "/code/grasp-pointnet/dex-net/data/grippers") grasp_sample_method = "antipodal" if grasp_sample_method == "uniform": ags = UniformGraspSampler(gripper, yaml_config) elif grasp_sample_method == "gaussian": ags = GaussianGraspSampler(gripper, yaml_config) elif grasp_sample_method == "antipodal": ags = AntipodalGraspSampler(gripper, yaml_config) elif grasp_sample_method == "gpg": ags = GpgGraspSampler(gripper, yaml_config) elif grasp_sample_method == "point": ags = PointGraspSampler(gripper, yaml_config) else: raise NameError("Can't support this sampler") print("Log: do job", i) if os.path.exists(str(file_list_all[i]) + "/google_512k/nontextured.obj"): of = ObjFile(str(file_list_all[i]) + "/google_512k/nontextured.obj") sf = SdfFile(str(file_list_all[i]) + "/google_512k/nontextured.sdf") else: print("can't find any obj or sdf file!") raise NameError("can't find any obj or sdf file!") mesh = of.read() sdf = sf.read() obj = GraspableObject3D(sdf, mesh) print("Log: opened object", i + 1, object_name) force_closure_quality_config = {} canny_quality_config = {} fc_list_sub1 = np.arange(2.0, 0.75, -0.4) fc_list_sub2 = np.arange(0.5, 0.36, -0.05) fc_list = np.concatenate([fc_list_sub1, fc_list_sub2]) for value_fc in fc_list: value_fc = round(value_fc, 2) yaml_config['metrics']['force_closure']['friction_coef'] = value_fc yaml_config['metrics']['robust_ferrari_canny'][ 'friction_coef'] = value_fc force_closure_quality_config[ value_fc] = GraspQualityConfigFactory.create_config( yaml_config['metrics']['force_closure']) canny_quality_config[ value_fc] = GraspQualityConfigFactory.create_config( yaml_config['metrics']['robust_ferrari_canny']) good_count_perfect = np.zeros(len(fc_list)) count = 0 minimum_grasp_per_fc = grasp_amount while np.sum(good_count_perfect < minimum_grasp_per_fc) != 0: grasps = ags.generate_grasps(obj, target_num_grasps=sample_nums, grasp_gen_mult=10, vis=False, random_approach_angle=True) count += len(grasps) for j in grasps: tmp, is_force_closure = False, False for ind_, value_fc in enumerate(fc_list): value_fc = round(value_fc, 2) tmp = is_force_closure is_force_closure = PointGraspMetrics3D.grasp_quality( j, obj, force_closure_quality_config[value_fc], vis=False) if tmp and not is_force_closure: if good_count_perfect[ind_ - 1] < minimum_grasp_per_fc: canny_quality = PointGraspMetrics3D.grasp_quality( j, obj, canny_quality_config[round(fc_list[ind_ - 1], 2)], vis=False) good_grasp.append((j, round(fc_list[ind_ - 1], 2), canny_quality)) good_count_perfect[ind_ - 1] += 1 break elif is_force_closure and value_fc == fc_list[-1]: if good_count_perfect[ind_] < minimum_grasp_per_fc: canny_quality = PointGraspMetrics3D.grasp_quality( j, obj, canny_quality_config[value_fc], vis=False) good_grasp.append((j, value_fc, canny_quality)) good_count_perfect[ind_] += 1 break print('Object:{} GoodGrasp:{}'.format(object_name, good_count_perfect)) object_name_len = len(object_name) object_name_ = str(object_name) + " " * (25 - object_name_len) if count == 0: good_grasp_rate = 0 else: good_grasp_rate = len(good_grasp) / count print('Gripper:{} Object:{} Rate:{:.4f} {}/{}'.format( gripper_name, object_name_, good_grasp_rate, len(good_grasp), count))
from autolab_core import YamlConfig try: from mayavi import mlab except ImportError: print("can not import mayavi") mlab = None from dexnet.grasping import GpgGraspSampler # temporary way for show 3D gripper using mayavi import pcl import glob # global configurations: home_dir = os.environ['HOME'] yaml_config = YamlConfig(home_dir + "/code/grasp-pointnet/dex-net/test/config.yaml") gripper_name = 'robotiq_85' gripper = RobotGripper.load( gripper_name, home_dir + "/code/grasp-pointnet/dex-net/data/grippers") ags = GpgGraspSampler(gripper, yaml_config) save_fig = True # save fig as png file show_fig = False # show the mayavi figure generate_new_file = False # whether generate new file for collision free grasps check_pcd_grasp_points = False if not show_fig: mlab.options.offscreen = True def get_file_name(file_dir_): file_list = [] for root, dirs, files in os.walk(file_dir_): # print(root) # current path if root.count('/') == file_dir_.count('/') + 1:
from meshpy.obj_file import ObjFile from meshpy.sdf_file import SdfFile from dexnet.grasping import GraspableObject3D import numpy as np from dexnet.visualization.visualizer3d import DexNetVisualizer3D as Vis from dexnet.grasping import RobotGripper from autolab_core import YamlConfig from mayavi import mlab from dexnet.grasping import GpgGraspSampler # temporary way for show 3D gripper using mayavi import glob # global configurations: home_dir = os.environ["HOME"] + "/code/PointNetGPD" yaml_config = YamlConfig(home_dir + "/dex-net/test/config.yaml") gripper_name = "robotiq_85" gripper = RobotGripper.load(gripper_name, home_dir + "/dex-net/data/grippers") ags = GpgGraspSampler(gripper, yaml_config) save_fig = False # save fig as png file show_fig = True # show the mayavi figure generate_new_file = False # whether generate new file for collision free grasps check_pcd_grasp_points = False def open_npy_and_obj(name_to_open_): npy_m_ = np.load(name_to_open_) file_dir = home_dir + "/PointNetGPD/data/ycb-tools/models/ycb/" object_name_ = name_to_open_.split("/")[-1][:-4] ply_name_ = file_dir + object_name_ + "/google_512k/nontextured.ply" if not check_pcd_grasp_points: of = ObjFile(file_dir + object_name_ + "/google_512k/nontextured.obj") sf = SdfFile(file_dir + object_name_ + "/google_512k/nontextured.sdf")
def read_grasps(self, object_name, stable_pose_id, max_grasps=10, visualize=False): # load gripper gripper = RobotGripper.load( GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers') # open Dex-Net API dexnet_handle = DexNet() dexnet_handle.open_database(self.database_path) dexnet_handle.open_dataset(self.dataset_name) # read the most robust grasp sorted_grasps, metrics = dexnet_handle.dataset.sorted_grasps( object_name, stable_pose_id=('pose_' + str(stable_pose_id)), metric='force_closure', gripper=gripper.name) if (len(sorted_grasps)) == 0: print('no grasps for this stable pose') if visualize: stable_pose = dexnet_handle.dataset.stable_pose( object_name, stable_pose_id=('pose_' + str(stable_pose_id))) obj = dexnet_handle.dataset.graspable(object_name) vis.figure() T_table_world = RigidTransform(from_frame='table', to_frame='world') T_obj_world = Visualizer3D.mesh_stable_pose( obj.mesh.trimesh, stable_pose.T_obj_world, T_table_world=T_table_world, color=(0.5, 0.5, 0.5), style='surface', plot_table=True, dim=0.15) vis.show(False) return None, None contact_points = map(get_contact_points, sorted_grasps) # ------------------------ VISUALIZATION CODE ------------------------ if visualize: low = np.min(metrics) high = np.max(metrics) if low == high: q_to_c = lambda quality: CONFIG['quality_scale'] else: q_to_c = lambda quality: CONFIG['quality_scale'] * ( quality - low) / (high - low) stable_pose = dexnet_handle.dataset.stable_pose( object_name, stable_pose_id=('pose_' + str(stable_pose_id))) obj = dexnet_handle.dataset.graspable(object_name) vis.figure() T_table_world = RigidTransform(from_frame='table', to_frame='world') T_obj_world = Visualizer3D.mesh_stable_pose( obj.mesh.trimesh, stable_pose.T_obj_world, T_table_world=T_table_world, color=(0.5, 0.5, 0.5), style='surface', plot_table=True, dim=0.15) for grasp, metric in zip(sorted_grasps, metrics): color = plt.get_cmap('hsv')((q_to_c(metric)))[:-1] vis.grasp(grasp, T_obj_world=stable_pose.T_obj_world, grasp_axis_color=color, endpoint_color=color) vis.show(False) # ------------------------ END VISUALIZATION CODE --------------------------- # ------------------------ START COLLISION CHECKING --------------------------- #stable_pose = dexnet_handle.dataset.stable_pose(object_name, stable_pose_id=('pose_'+str(stable_pose_id))) #graspable = dexnet_handle.dataset.graspable(object_name) #cc = GraspCollisionChecker(gripper).set_graspable_object(graspable, stable_pose.T_obj_world) stable_pose_matrix = self.get_stable_pose(object_name, stable_pose_id) # CLOSE DATABASE dexnet_handle.close_database() gripper_poses = [] for grasp in sorted_grasps[:max_grasps]: gripper_pose_matrix = np.eye(4, 4) center_world = np.matmul( stable_pose_matrix, [grasp.center[0], grasp.center[1], grasp.center[2], 1]) axis_world = np.matmul( stable_pose_matrix, [grasp.axis_[0], grasp.axis_[1], grasp.axis_[2], 1]) gripper_angle = math.atan2(axis_world[1], axis_world[0]) gripper_pose_matrix[:3, 3] = center_world[:3] gripper_pose_matrix[0, 0] = math.cos(gripper_angle) gripper_pose_matrix[0, 1] = -math.sin(gripper_angle) gripper_pose_matrix[1, 0] = math.sin(gripper_angle) gripper_pose_matrix[1, 1] = math.cos(gripper_angle) #if visualize: # vis.figure() # vis.gripper_on_object(gripper, grasp, obj, stable_pose=stable_pose.T_obj_world) # vis.show(False) gripper_poses.append(gripper_pose_matrix) return contact_points, gripper_poses
import numpy as np import os from autolab_core import Point, YamlConfig, RigidTransform from dexnet.constants import READ_ONLY_ACCESS from dexnet.grasping import RobotGripper from dexnet.database import Hdf5Database config_dir = 'cfg/tools/generate_oblique_xyzq.yaml' save_dir = '/data/hdf5_stats/grasp_axes_2/' config = YamlConfig(config_dir) gripper = RobotGripper.load(config['gripper']) table_offset = config['collision_checking']['table_offset'] if not os.path.exists(save_dir): os.mkdir(save_dir) database = Hdf5Database(config['database_name'], access_level=READ_ONLY_ACCESS) target_object_keys = config['target_objects'] dataset_names = target_object_keys.keys() datasets = [database.dataset(dn) for dn in dataset_names] labels = [] beta = [] orig_beta = [] alpha = [] cnt = 0 obj_cnt = 0 for dataset in datasets: object_keys = dataset.object_keys
def antipodal_grasp_sampler(self): #of = ObjFile(OBJ_FILENAME) #sf = SdfFile(SDF_FILENAME) #mesh = of.read() #sdf = sf.read() #obj = GraspableObject3D(sdf, mesh) mass = 1.0 CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE mesh_processor = MeshProcessor(OBJ_FILENAME, CONFIG['cache_dir']) mesh_processor.generate_graspable(CONFIG) mesh = mesh_processor.mesh sdf = mesh_processor.sdf obj = GraspableObject3D(sdf, mesh) gripper = RobotGripper.load(GRIPPER_NAME) ags = AntipodalGraspSampler(gripper, CONFIG) grasps = ags.sample_grasps(obj, num_grasps=100) print(len(grasps)) quality_config = GraspQualityConfigFactory.create_config(CONFIG['metrics']['robust_ferrari_canny']) #quality_config = GraspQualityConfigFactory.create_config(CONFIG['metrics']['force_closure']) quality_fn = GraspQualityFunctionFactory.create_quality_function(obj, quality_config) vis.figure() #vis.points(PointCloud(nparraypoints), scale=0.001, color=np.array([0.5,0.5,0.5])) #vis.plot3d(nparraypoints) metrics = [] vis.mesh(obj.mesh.trimesh, style='surface') for grasp in grasps: success, c = grasp.close_fingers(obj) if success: c1, c2 = c #fn_fc = quality_fn(grasp).quality true_fc = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config) true_fc = true_fc metrics.append(true_fc) #color = quality_fn(grasp).quality #if (true_fc > 1.0): # true_fc = 1.0 #color = (1.0-true_fc, true_fc, 0.0) low = np.min(metrics) high = np.max(metrics) print(low) print(high) if low == high: q_to_c = lambda quality: CONFIG['quality_scale'] else: q_to_c = lambda quality: CONFIG['quality_scale'] * (quality - low) / (high - low) print(len(metrics)) for grasp, metric in zip(grasps, metrics): color = plt.get_cmap('hsv')(q_to_c(metric))[:-1] print(color) vis.grasp(grasp, grasp_axis_color=color,endpoint_color=color) vis.show(False)
def generate_gqcnn_dataset(dataset_path, database, target_object_keys, env_rv_params, gripper_name, config): """ Generates a GQ-CNN TensorDataset for training models with new grippers, quality metrics, objects, and cameras. Parameters ---------- dataset_path : str path to save the dataset to database : :obj:`Hdf5Database` Dex-Net database containing the 3D meshes, grasps, and grasp metrics target_object_keys : :obj:`OrderedDict` dictionary mapping dataset names to target objects (either 'all' or a list of specific object keys) env_rv_params : :obj:`OrderedDict` parameters of the camera and object random variables used in sampling (see meshpy_berkeley.UniformPlanarWorksurfaceImageRandomVariable for more info) gripper_name : str name of the gripper to use config : :obj:`autolab_core.YamlConfig` other parameters for dataset generation Notes ----- Required parameters of config are specified in Other Parameters Other Parameters ---------------- images_per_stable_pose : int number of object and camera poses to sample for each stable pose stable_pose_min_p : float minimum probability of occurrence for a stable pose to be used in data generation (used to prune bad stable poses gqcnn/crop_width : int width, in pixels, of crop region around each grasp center, before resize (changes the size of the region seen by the GQ-CNN) gqcnn/crop_height : int height, in pixels, of crop region around each grasp center, before resize (changes the size of the region seen by the GQ-CNN) gqcnn/final_width : int width, in pixels, of final transformed grasp image for input to the GQ-CNN (defaults to 32) gqcnn/final_height : int height, in pixels, of final transformed grasp image for input to the GQ-CNN (defaults to 32) table_alignment/max_approach_table_angle : float max angle between the grasp axis and the table normal when the grasp approach is maximally aligned with the table normal table_alignment/max_approach_offset : float max deviation from perpendicular approach direction to use in grasp collision checking table_alignment/num_approach_offset_samples : int number of approach samples to use in collision checking collision_checking/table_offset : float max allowable interpenetration between the gripper and table to be considered collision free collision_checking/table_mesh_filename : str path to a table mesh for collision checking (default data/meshes/table.obj) collision_checking/approach_dist : float distance, in meters, between the approach pose and final grasp pose along the grasp axis collision_checking/delta_approach : float amount, in meters, to discretize the straight-line path from the gripper approach pose to the final grasp pose tensors/datapoints_per_file : int number of datapoints to store in each unique tensor file on disk tensors/fields : :obj:`dict` dictionary mapping field names to dictionaries specifying the data type, height, width, and number of channels for each tensor debug : bool True (or 1) if the random seed should be set to enforce deterministic behavior, False (0) otherwise vis/candidate_grasps : bool True (or 1) if the collision free candidate grasps should be displayed in 3D (for debugging) vis/rendered_images : bool True (or 1) if the rendered images for each stable pose should be displayed (for debugging) vis/grasp_images : bool True (or 1) if the transformed grasp images should be displayed (for debugging) """ # read data gen params output_dir = dataset_path gripper = RobotGripper.load(gripper_name) image_samples_per_stable_pose = config['images_per_stable_pose'] stable_pose_min_p = config['stable_pose_min_p'] # read gqcnn params gqcnn_params = config['gqcnn'] im_crop_height = gqcnn_params['crop_height'] im_crop_width = gqcnn_params['crop_width'] im_final_height = gqcnn_params['final_height'] im_final_width = gqcnn_params['final_width'] cx_crop = float(im_crop_width) / 2 cy_crop = float(im_crop_height) / 2 # open database dataset_names = target_object_keys.keys() datasets = [database.dataset(dn) for dn in dataset_names] # set target objects for dataset in datasets: if target_object_keys[dataset.name] == 'all': target_object_keys[dataset.name] = dataset.object_keys # setup grasp params table_alignment_params = config['table_alignment'] min_grasp_approach_offset = -np.deg2rad( table_alignment_params['max_approach_offset']) max_grasp_approach_offset = np.deg2rad( table_alignment_params['max_approach_offset']) max_grasp_approach_table_angle = np.deg2rad( table_alignment_params['max_approach_table_angle']) num_grasp_approach_samples = table_alignment_params[ 'num_approach_offset_samples'] phi_offsets = [] if max_grasp_approach_offset == min_grasp_approach_offset: phi_inc = 1 elif num_grasp_approach_samples == 1: phi_inc = max_grasp_approach_offset - min_grasp_approach_offset + 1 else: phi_inc = (max_grasp_approach_offset - min_grasp_approach_offset) / ( num_grasp_approach_samples - 1) phi = min_grasp_approach_offset while phi <= max_grasp_approach_offset: phi_offsets.append(phi) phi += phi_inc # setup collision checking coll_check_params = config['collision_checking'] approach_dist = coll_check_params['approach_dist'] delta_approach = coll_check_params['delta_approach'] table_offset = coll_check_params['table_offset'] table_mesh_filename = coll_check_params['table_mesh_filename'] if not os.path.isabs(table_mesh_filename): table_mesh_filename = os.path.join( os.path.dirname(os.path.realpath(__file__)), '..', table_mesh_filename) table_mesh = ObjFile(table_mesh_filename).read() # set tensor dataset config tensor_config = config['tensors'] tensor_config['fields']['depth_ims_tf_table']['height'] = im_final_height tensor_config['fields']['depth_ims_tf_table']['width'] = im_final_width tensor_config['fields']['obj_masks']['height'] = im_final_height tensor_config['fields']['obj_masks']['width'] = im_final_width # add available metrics (assuming same are computed for all objects) metric_names = [] dataset = datasets[0] obj_keys = dataset.object_keys if len(obj_keys) == 0: raise ValueError('No valid objects in dataset %s' % (dataset.name)) obj = dataset[obj_keys[0]] grasps = dataset.grasps(obj.key, gripper=gripper.name) grasp_metrics = dataset.grasp_metrics(obj.key, grasps, gripper=gripper.name) metric_names = grasp_metrics[grasp_metrics.keys()[0]].keys() for metric_name in metric_names: tensor_config['fields'][metric_name] = {} tensor_config['fields'][metric_name]['dtype'] = 'float32' # init tensor dataset tensor_dataset = TensorDataset(output_dir, tensor_config) tensor_datapoint = tensor_dataset.datapoint_template # setup log file experiment_log_filename = os.path.join(output_dir, 'dataset_generation.log') formatter = logging.Formatter('%(asctime)s %(levelname)s: %(message)s') hdlr = logging.FileHandler(experiment_log_filename) hdlr.setFormatter(formatter) logging.getLogger().addHandler(hdlr) root_logger = logging.getLogger() # copy config out_config_filename = os.path.join(output_dir, 'dataset_generation.json') ordered_dict_config = collections.OrderedDict() for key in config.keys(): ordered_dict_config[key] = config[key] with open(out_config_filename, 'w') as outfile: json.dump(ordered_dict_config, outfile) # 1. Precompute the set of valid grasps for each stable pose: # i) Perpendicular to the table # ii) Collision-free along the approach direction # load grasps if they already exist grasp_cache_filename = os.path.join(output_dir, CACHE_FILENAME) if os.path.exists(grasp_cache_filename): logging.info('Loading grasp candidates from file') candidate_grasps_dict = pkl.load(open(grasp_cache_filename, 'rb')) # otherwise re-compute by reading from the database and enforcing constraints else: # create grasps dict candidate_grasps_dict = {} # loop through datasets and objects for dataset in datasets: logging.info('Reading dataset %s' % (dataset.name)) for obj in dataset: if obj.key not in target_object_keys[dataset.name]: continue # init candidate grasp storage candidate_grasps_dict[obj.key] = {} # setup collision checker collision_checker = GraspCollisionChecker(gripper) collision_checker.set_graspable_object(obj) # read in the stable poses of the mesh stable_poses = dataset.stable_poses(obj.key) for i, stable_pose in enumerate(stable_poses): # render images if stable pose is valid if stable_pose.p > stable_pose_min_p: candidate_grasps_dict[obj.key][stable_pose.id] = [] # setup table in collision checker T_obj_stp = stable_pose.T_obj_table.as_frames( 'obj', 'stp') T_obj_table = obj.mesh.get_T_surface_obj( T_obj_stp, delta=table_offset).as_frames('obj', 'table') T_table_obj = T_obj_table.inverse() collision_checker.set_table(table_mesh_filename, T_table_obj) # read grasp and metrics grasps = dataset.grasps(obj.key, gripper=gripper.name) logging.info( 'Aligning %d grasps for object %s in stable %s' % (len(grasps), obj.key, stable_pose.id)) # align grasps with the table aligned_grasps = [ grasp.perpendicular_table(stable_pose) for grasp in grasps ] # check grasp validity logging.info( 'Checking collisions for %d grasps for object %s in stable %s' % (len(grasps), obj.key, stable_pose.id)) for aligned_grasp in aligned_grasps: # check angle with table plane and skip unaligned grasps _, grasp_approach_table_angle, _ = aligned_grasp.grasp_angles_from_stp_z( stable_pose) perpendicular_table = ( np.abs(grasp_approach_table_angle) < max_grasp_approach_table_angle) if not perpendicular_table: continue # check whether any valid approach directions are collision free collision_free = False for phi_offset in phi_offsets: rotated_grasp = aligned_grasp.grasp_y_axis_offset( phi_offset) collides = collision_checker.collides_along_approach( rotated_grasp, approach_dist, delta_approach) if not collides: collision_free = True break # store if aligned to table candidate_grasps_dict[obj.key][ stable_pose.id].append( GraspInfo(aligned_grasp, collision_free)) # visualize if specified if collision_free and config['vis'][ 'candidate_grasps']: logging.info('Grasp %d' % (aligned_grasp.id)) vis.figure() vis.gripper_on_object(gripper, aligned_grasp, obj, stable_pose.T_obj_world) vis.show() # save to file logging.info('Saving to file') pkl.dump(candidate_grasps_dict, open(grasp_cache_filename, 'wb')) # 2. Render a dataset of images and associate the gripper pose with image coordinates for each grasp in the Dex-Net database # setup variables obj_category_map = {} pose_category_map = {} cur_pose_label = 0 cur_obj_label = 0 cur_image_label = 0 # render images for each stable pose of each object in the dataset render_modes = [RenderMode.SEGMASK, RenderMode.DEPTH_SCENE] for dataset in datasets: logging.info('Generating data for dataset %s' % (dataset.name)) # iterate through all object keys object_keys = dataset.object_keys for obj_key in object_keys: obj = dataset[obj_key] if obj.key not in target_object_keys[dataset.name]: continue # read in the stable poses of the mesh stable_poses = dataset.stable_poses(obj.key) for i, stable_pose in enumerate(stable_poses): # render images if stable pose is valid if stable_pose.p > stable_pose_min_p: # log progress logging.info('Rendering images for object %s in %s' % (obj.key, stable_pose.id)) # add to category maps if obj.key not in obj_category_map.keys(): obj_category_map[obj.key] = cur_obj_label pose_category_map['%s_%s' % (obj.key, stable_pose.id)] = cur_pose_label # read in candidate grasps and metrics candidate_grasp_info = candidate_grasps_dict[obj.key][ stable_pose.id] candidate_grasps = [g.grasp for g in candidate_grasp_info] grasp_metrics = dataset.grasp_metrics(obj.key, candidate_grasps, gripper=gripper.name) # compute object pose relative to the table T_obj_stp = stable_pose.T_obj_table.as_frames('obj', 'stp') T_obj_stp = obj.mesh.get_T_surface_obj(T_obj_stp) # sample images from random variable T_table_obj = RigidTransform(from_frame='table', to_frame='obj') scene_objs = { 'table': SceneObject(table_mesh, T_table_obj) } urv = UniformPlanarWorksurfaceImageRandomVariable( obj.mesh, render_modes, 'camera', env_rv_params, stable_pose=stable_pose, scene_objs=scene_objs) render_start = time.time() render_samples = urv.rvs( size=image_samples_per_stable_pose) render_stop = time.time() logging.info('Rendering images took %.3f sec' % (render_stop - render_start)) # visualize if config['vis']['rendered_images']: d = int(np.ceil( np.sqrt(image_samples_per_stable_pose))) # binary vis2d.figure() for j, render_sample in enumerate(render_samples): vis2d.subplot(d, d, j + 1) vis2d.imshow(render_sample.renders[ RenderMode.SEGMASK].image) # depth table vis2d.figure() for j, render_sample in enumerate(render_samples): vis2d.subplot(d, d, j + 1) vis2d.imshow(render_sample.renders[ RenderMode.DEPTH_SCENE].image) vis2d.show() # tally total amount of data num_grasps = len(candidate_grasps) num_images = image_samples_per_stable_pose num_save = num_images * num_grasps logging.info('Saving %d datapoints' % (num_save)) # for each candidate grasp on the object compute the projection # of the grasp into image space for render_sample in render_samples: # read images binary_im = render_sample.renders[ RenderMode.SEGMASK].image depth_im_table = render_sample.renders[ RenderMode.DEPTH_SCENE].image # read camera params T_stp_camera = render_sample.camera.object_to_camera_pose shifted_camera_intr = render_sample.camera.camera_intr # read pixel offsets cx = depth_im_table.center[1] cy = depth_im_table.center[0] # compute intrinsics for virtual camera of the final # cropped and rescaled images camera_intr_scale = float(im_final_height) / float( im_crop_height) cropped_camera_intr = shifted_camera_intr.crop( im_crop_height, im_crop_width, cy, cx) final_camera_intr = cropped_camera_intr.resize( camera_intr_scale) # create a thumbnail for each grasp for grasp_info in candidate_grasp_info: # read info grasp = grasp_info.grasp collision_free = grasp_info.collision_free # get the gripper pose T_obj_camera = T_stp_camera * T_obj_stp.as_frames( 'obj', T_stp_camera.from_frame) grasp_2d = grasp.project_camera( T_obj_camera, shifted_camera_intr) # center images on the grasp, rotate to image x axis dx = cx - grasp_2d.center.x dy = cy - grasp_2d.center.y translation = np.array([dy, dx]) binary_im_tf = binary_im.transform( translation, grasp_2d.angle) depth_im_tf_table = depth_im_table.transform( translation, grasp_2d.angle) # crop to image size binary_im_tf = binary_im_tf.crop( im_crop_height, im_crop_width) depth_im_tf_table = depth_im_tf_table.crop( im_crop_height, im_crop_width) # resize to image size binary_im_tf = binary_im_tf.resize( (im_final_height, im_final_width), interp='nearest') depth_im_tf_table = depth_im_tf_table.resize( (im_final_height, im_final_width)) # visualize the transformed images if config['vis']['grasp_images']: grasp_center = Point( depth_im_tf_table.center, frame=final_camera_intr.frame) tf_grasp_2d = Grasp2D( grasp_center, 0, grasp_2d.depth, width=gripper.max_width, camera_intr=final_camera_intr) # plot 2D grasp image vis2d.figure() vis2d.subplot(2, 2, 1) vis2d.imshow(binary_im) vis2d.grasp(grasp_2d) vis2d.subplot(2, 2, 2) vis2d.imshow(depth_im_table) vis2d.grasp(grasp_2d) vis2d.subplot(2, 2, 3) vis2d.imshow(binary_im_tf) vis2d.grasp(tf_grasp_2d) vis2d.subplot(2, 2, 4) vis2d.imshow(depth_im_tf_table) vis2d.grasp(tf_grasp_2d) vis2d.title('Coll Free? %d' % (grasp_info.collision_free)) vis2d.show() # plot 3D visualization vis.figure() T_obj_world = vis.mesh_stable_pose( obj.mesh, stable_pose.T_obj_world, style='surface', dim=0.5) vis.gripper(gripper, grasp, T_obj_world, color=(0.3, 0.3, 0.3)) vis.show() # form hand pose array hand_pose = np.r_[grasp_2d.center.y, grasp_2d.center.x, grasp_2d.depth, grasp_2d.angle, grasp_2d.center.y - shifted_camera_intr.cy, grasp_2d.center.x - shifted_camera_intr.cx, grasp_2d.width_px] # store to data buffers tensor_datapoint[ 'depth_ims_tf_table'] = depth_im_tf_table.raw_data tensor_datapoint[ 'obj_masks'] = binary_im_tf.raw_data tensor_datapoint['hand_poses'] = hand_pose tensor_datapoint['collision_free'] = collision_free tensor_datapoint['obj_labels'] = cur_obj_label tensor_datapoint['pose_labels'] = cur_pose_label tensor_datapoint['image_labels'] = cur_image_label for metric_name, metric_val in grasp_metrics[ grasp.id].iteritems(): coll_free_metric = ( 1 * collision_free) * metric_val tensor_datapoint[ metric_name] = coll_free_metric tensor_dataset.add(tensor_datapoint) # update image label cur_image_label += 1 # update pose label cur_pose_label += 1 # force clean up gc.collect() # update object label cur_obj_label += 1 # force clean up gc.collect() # save last file tensor_dataset.flush() # save category mappings obj_cat_filename = os.path.join(output_dir, 'object_category_map.json') json.dump(obj_category_map, open(obj_cat_filename, 'w')) pose_cat_filename = os.path.join(output_dir, 'pose_category_map.json') json.dump(pose_category_map, open(pose_cat_filename, 'w'))
try: from mayavi import mlab except ImportError: print("can not import mayavi") mlab = None from dexnet.grasping import GpgGraspSampler # temporary way for show 3D gripper using mayavi import pcl import glob import argparse # global configurations: home_dir = os.environ['HOME'] dexnet_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '..')) yaml_config = YamlConfig(dexnet_dir + "/test/config.yaml") gripper_name = 'robotiq_85' gripper = RobotGripper.load(gripper_name, dexnet_dir + "/data/grippers") ags = GpgGraspSampler(gripper, yaml_config) save_fig = False # save fig as png file show_fig = True # show the mayavi figure generate_new_file = False # whether generate new file for collision free grasps check_pcd_grasp_points = False def get_file_name(file_dir_): file_list = [] for root, dirs, files in os.walk(file_dir_): # print(root) # current path if root.count('/') == file_dir_.count('/') + 1: file_list.append(root) # print(dirs) # all the directories in current path # print(files) # all the files in current path
def worker(i, sample_nums, grasp_amount, good_grasp): object_name = file_list_all[i][len(home_dir) + 48:-12] print('a worker of task {} start'.format(object_name)) yaml_config = YamlConfig(home_dir + "/Projects/PointNetGPD/dex-net/test/config.yaml") gripper_name = 'robotiq_85' gripper = RobotGripper.load( gripper_name, home_dir + "/Projects/PointNetGPD/dex-net/data/grippers") grasp_sample_method = "antipodal" if grasp_sample_method == "uniform": ags = UniformGraspSampler(gripper, yaml_config) elif grasp_sample_method == "gaussian": ags = GaussianGraspSampler(gripper, yaml_config) elif grasp_sample_method == "antipodal": ags = AntipodalGraspSampler(gripper, yaml_config) elif grasp_sample_method == "gpg": ags = GpgGraspSampler(gripper, yaml_config) elif grasp_sample_method == "point": ags = PointGraspSampler(gripper, yaml_config) else: raise NameError("Can't support this sampler") print("Log: do job", i) if os.path.exists(str(file_list_all[i]) + "/nontextured.obj"): of = ObjFile(str(file_list_all[i]) + "/nontextured.obj") sf = SdfFile(str(file_list_all[i]) + "/nontextured.sdf") else: print("can't find any obj or sdf file!") raise NameError("can't find any obj or sdf file!") mesh = of.read() sdf = sf.read() obj = GraspableObject3D(sdf, mesh) print("Log: opened object", i + 1, object_name) force_closure_quality_config = {} canny_quality_config = {} fc_list_sub1 = np.arange(2.0, 0.75, -0.4) fc_list_sub2 = np.arange(0.5, 0.36, -0.05) fc_list = np.concatenate( [fc_list_sub1, fc_list_sub2]) # 摩擦系数列表 fc_list [2. 1.6 1.2 0.8 0.5 0.45 0.4 ] print("fc_list", fc_list, "fc_list[-1]", fc_list[-1]) for value_fc in fc_list: value_fc = round(value_fc, 2) yaml_config['metrics']['force_closure']['friction_coef'] = value_fc yaml_config['metrics']['robust_ferrari_canny'][ 'friction_coef'] = value_fc force_closure_quality_config[ value_fc] = GraspQualityConfigFactory.create_config( yaml_config['metrics']['force_closure']) canny_quality_config[ value_fc] = GraspQualityConfigFactory.create_config( yaml_config['metrics']['robust_ferrari_canny']) good_count_perfect = np.zeros(len(fc_list)) count = 0 minimum_grasp_per_fc = grasp_amount # 各个摩擦系数生成一些抓取 while np.sum(good_count_perfect < minimum_grasp_per_fc ) != 0 and good_count_perfect[-1] < minimum_grasp_per_fc: print("[INFO]:good | mini", good_count_perfect, minimum_grasp_per_fc) print("[INFO]:good < mini", good_count_perfect < minimum_grasp_per_fc, np.sum(good_count_perfect < minimum_grasp_per_fc)) grasps = ags.generate_grasps( obj, target_num_grasps=sample_nums, grasp_gen_mult=10, # 生成抓取姿态 vis=False, random_approach_angle=True) # 随机调整抓取方向 print("\033[0;32m%s\033[0m" % "[INFO] Worker{} generate {} grasps.".format(i, len(grasps))) count += len(grasps) for j in grasps: # 遍历生成的抓取姿态, 判断是否为力闭合, 及其对应的摩擦系数 tmp, is_force_closure = False, False for ind_, value_fc in enumerate(fc_list): # 为每个摩擦系数分配抓取姿态 value_fc = round(value_fc, 2) tmp = is_force_closure is_force_closure = PointGraspMetrics3D.grasp_quality( j, obj, # 依据摩擦系数 value_fc 评估抓取姿态 force_closure_quality_config[value_fc], vis=False) print("[INFO] is_force_closure:", is_force_closure, "value_fc:", value_fc, "tmp:", tmp) if tmp and not is_force_closure: # 前一个摩擦系数下为力闭合, 当前摩擦系数下非力闭合, 即找到此抓取对应的最小摩擦系数 print("[debug] tmp and not is_force_closure") if good_count_perfect[ind_ - 1] < minimum_grasp_per_fc: canny_quality = PointGraspMetrics3D.grasp_quality( j, obj, canny_quality_config[round(fc_list[ind_ - 1], 2)], vis=False) good_grasp.append( (j, round(fc_list[ind_ - 1], 2), canny_quality)) # 保存前一个抓取 good_count_perfect[ind_ - 1] += 1 break elif is_force_closure and value_fc == fc_list[ -1]: # 力闭合并且摩擦系数最小 print( "[debug] is_force_closure and value_fc == fc_list[-1]") if good_count_perfect[ind_] < minimum_grasp_per_fc: canny_quality = PointGraspMetrics3D.grasp_quality( j, obj, canny_quality_config[value_fc], vis=False) good_grasp.append((j, value_fc, canny_quality)) good_count_perfect[ind_] += 1 break print('Worker:', i, 'Object:{} GoodGrasp:{}'.format(object_name, good_count_perfect)) object_name_len = len(object_name) object_name_ = str(object_name) + " " * (25 - object_name_len) if count == 0: good_grasp_rate = 0 else: good_grasp_rate = len(good_grasp) / count print( 'Worker:', i, 'Gripper:{} Object:{} Rate:{:.4f} {}/{}'.format( gripper_name, object_name_, good_grasp_rate, len(good_grasp), count))
try: from mayavi import mlab except ImportError: print("Can not import mayavi") mlab = None sys.path.append( path.dirname(path.dirname(path.dirname(path.abspath("__file__"))))) sys.path.append(os.environ['HOME'] + "/code/grasp-pointnet/PointNetGPD") # global config: yaml_config = YamlConfig( os.environ['HOME'] + "/Projects/GPD_PointNet/dex-net/test/sample_config.yaml") gripper_name = 'robotiq_85' gripper = RobotGripper.load( gripper_name, os.environ['HOME'] + "/Projects/GPD_PointNet/dex-net/data/grippers") ags = GpgGraspSamplerPclPcd(gripper, yaml_config) # ags = AntipodalGraspSampler(gripper, yaml_config) value_fc = 0.4 # no use, set a random number num_grasps = 40 num_workers = 20 max_num_samples = 150 n_voxel = 500 minimal_points_send_to_point_net = 20 marker_life_time = 8 show_bad_grasp = False save_grasp_related_file = False
print("Please install grasp msgs from https://github.com/TAMS-Group/gpd_grasp_msgs in your ROS workspace") exit() try: from mayavi import mlab except ImportError: print("Can not import mayavi") mlab = None sys.path.append(path.dirname(path.dirname(path.dirname(path.abspath("__file__"))))) sys.path.append(os.environ['HOME'] + "/code/PointNetGPD/PointNetGPD") from main_test import test_network, model, args # global config: yaml_config = YamlConfig(os.environ['HOME'] + "/code/PointNetGPD/dex-net/test/config.yaml") gripper_name = 'robotiq_85' gripper = RobotGripper.load(gripper_name, os.environ['HOME'] + "/code/PointNetGPD/dex-net/data/grippers") ags = GpgGraspSamplerPcl(gripper, yaml_config) value_fc = 0.4 # no use, set a random number num_grasps = 40 num_workers = 20 max_num_samples = 150 n_voxel = 500 minimal_points_send_to_point_net = 20 marker_life_time = 8 show_bad_grasp = False save_grasp_related_file = False show_final_grasp = args.show_final_grasp tray_grasp = args.tray_grasp
def save_dexnet_objects(output_path, database, target_object_keys, config, pointers, num): slice_dataset = False files = [] if not os.path.exists(output_path): os.mkdir(output_path) for each_file in os.listdir(output_path): os.remove(output_path + '/' + each_file) gripper = RobotGripper.load(config['gripper']) # Setup grasp params: table_alignment_params = config['table_alignment'] min_grasp_approach_offset = -np.deg2rad( table_alignment_params['max_approach_offset']) max_grasp_approach_offset = np.deg2rad( table_alignment_params['max_approach_offset']) max_grasp_approach_table_angle = np.deg2rad( table_alignment_params['max_approach_table_angle']) num_grasp_approach_samples = table_alignment_params[ 'num_approach_offset_samples'] phi_offsets = [] if max_grasp_approach_offset == min_grasp_approach_offset: phi_inc = 1 elif num_grasp_approach_samples == 1: phi_inc = max_grasp_approach_offset - min_grasp_approach_offset + 1 else: phi_inc = (max_grasp_approach_offset - min_grasp_approach_offset) / ( num_grasp_approach_samples - 1) phi = min_grasp_approach_offset while phi <= max_grasp_approach_offset: phi_offsets.append(phi) phi += phi_inc # Setup collision checking coll_check_params = config['collision_checking'] approach_dist = coll_check_params['approach_dist'] delta_approach = coll_check_params['delta_approach'] table_offset = coll_check_params['table_offset'] stable_pose_min_p = config['stable_pose_min_p'] table_mesh_filename = coll_check_params['table_mesh_filename'] if not os.path.isabs(table_mesh_filename): table_mesh_filename = os.path.join(DATA_DIR, table_mesh_filename) # #table_mesh_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', table_mesh_filename) # table_mesh = ObjFile(table_mesh_filename).read() dataset_names = target_object_keys.keys() datasets = [database.dataset(dn) for dn in dataset_names] if slice_dataset: datasets = [dataset.subset(0, 100) for dataset in datasets] start = 0 for dataset in datasets: target_object_keys[dataset.name] = [] end = start + len(dataset.object_keys) for cnt, _id in enumerate(pointers.obj_ids): if _id >= end or _id < start: continue target_object_keys[dataset.name].append(dataset.object_keys[_id - start]) files.append( tuple([ dataset.object_keys[_id - start], pointers.tensor[cnt], pointers.array[cnt], pointers.depths[cnt] ])) start += end print(target_object_keys) print("target object keys:", len(target_object_keys['3dnet']), len(target_object_keys['kit'])) files = np.array(files, dtype=[('Object_id', (np.str_, 40)), ('Tensor', int), ('Array', int), ('Depth', float)]) # Precompute set of valid grasps candidate_grasps_dict = {} counter = 0 for dataset in datasets: for obj in dataset: if obj.key not in target_object_keys[dataset.name]: continue print("Object in subset") # Initiate candidate grasp storage candidate_grasps_dict[obj.key] = {} # Setup collision checker collision_checker = GraspCollisionChecker(gripper) collision_checker.set_graspable_object(obj) # Read in the stable poses of the mesh stable_poses = dataset.stable_poses(obj.key) try: stable_pose = stable_poses[pointers.pose_num[counter]] except IndexError: print( "Problems with reading pose. Tensor %d, Array %d, Pose %d" % (pointers.tensor[counter], pointers.array[counter], pointers.pose_num[counter])) print("Stable poses:", stable_poses) print("Pointers pose:", pointers.pose_num[counter]) counter += 1 print("Continue.") continue print("Read in stable pose") candidate_grasps_dict[obj.key][stable_pose.id] = [] # Setup table in collision checker T_obj_stp = stable_pose.T_obj_table.as_frames('obj', 'stp') T_obj_table = obj.mesh.get_T_surface_obj( T_obj_stp, delta=table_offset).as_frames('obj', 'table') T_table_obj = T_obj_table.inverse() collision_checker.set_table(table_mesh_filename, T_table_obj) # read grasp and metrics grasps = dataset.grasps(obj.key, gripper=gripper.name) # align grasps with the table aligned_grasps = [ grasp.perpendicular_table(stable_pose) for grasp in grasps ] i = 0 found = False if len(aligned_grasps) < pointers.grasp_num[counter]: raise IndexError print("pointers grasp num", pointers.grasp_num[counter]) print("tensor", pointers.tensor[counter]) print("array", pointers.array[counter]) # Check grasp validity for aligned_grasp in aligned_grasps: # Check angle with table plane and skip unaligned grasps _, grasp_approach_table_angle, _ = aligned_grasp.grasp_angles_from_stp_z( stable_pose) perpendicular_table = (np.abs(grasp_approach_table_angle) < max_grasp_approach_table_angle) if not perpendicular_table: continue # Check whether any valid approach directions are collision free collision_free = False for phi_offset in phi_offsets: rotated_grasp = aligned_grasp.grasp_y_axis_offset( phi_offset) collides = collision_checker.collides_along_approach( rotated_grasp, approach_dist, delta_approach) if not collides: collision_free = True break # Store if aligned to table if i == pointers.grasp_num[counter]: found, contact_points = aligned_grasp.close_fingers(obj) print("Contact points", contact_points) if not found: print("Could not find contact points. continue.") break else: print("Original metric: ", pointers.metrics[counter]) print( "Metrics mapped point: ", dataset.grasp_metrics(obj.key, [aligned_grasp], gripper=gripper.name)) candidate_grasps_dict[obj.key][stable_pose.id].append( GraspInfo(aligned_grasp, collision_free, [ contact_points[0].point, contact_points[1].point ])) # logging.info('Grasp %d' % (aligned_grasp.id)) # vis.figure() # vis.gripper_on_object(gripper, aligned_grasp, obj, stable_pose.T_obj_world, plot_table=False) # vis.show() break i += 1 if found: # Save files print("Saving file: ", obj.key) savefile = ObjFile(DATA_DIR + "/meshes/dexnet/" + obj.key + ".obj") savefile.write(obj.mesh) # print("Vertices:", obj.mesh.vertices.shape) # print("Triangles:", obj.mesh.triangles.shape) mesh = o3d.geometry.TriangleMesh( o3d.utility.Vector3dVector(obj.mesh.vertices), o3d.utility.Vector3iVector(obj.mesh.triangles)) o3d.io.write_triangle_mesh( DATA_DIR + '/PerfectPredictions/3D_meshes/' + "%05d_%03d.ply" % (pointers.tensor[counter], pointers.array[counter]), mesh) # Save stable poses save_stp = StablePoseFile(DATA_DIR + "/meshes/dexnet/" + obj.key + ".stp") save_stp.write(stable_poses) # Save candidate grasp info pkl.dump( candidate_grasps_dict[obj.key], open(DATA_DIR + "/meshes/dexnet/" + obj.key + ".pkl", 'wb')) # Save grasp metrics candidate_grasp_info = candidate_grasps_dict[obj.key][ stable_pose.id] candidate_grasps = [g.grasp for g in candidate_grasp_info] grasp_metrics = dataset.grasp_metrics(obj.key, candidate_grasps, gripper=gripper.name) write_metrics = json.dumps(grasp_metrics) f = open(DATA_DIR + "/meshes/dexnet/" + obj.key + ".json", "w") f.write(write_metrics) f.close() counter += 1 if num is not None and counter >= num: break with open(DATA_DIR + '/meshes/dexnet/files.csv', 'w') as csv_file: csv_writer = csv.writer(csv_file, delimiter=',') for point in files: csv_writer.writerow(point)
try: from mayavi import mlab except ImportError: print("Can not import mayavi") mlab = None sys.path.append( path.dirname(path.dirname(path.dirname(path.abspath("__file__"))))) sys.path.append(os.environ['HOME'] + "/code/grasp-pointnet/PointNetGPD") from main_test import test_network, model, args # global config: yaml_config = YamlConfig(os.environ['HOME'] + "/code/grasp-pointnet/dex-net/test/config.yaml") gripper_name = 'robotiq_85' gripper = RobotGripper.load( gripper_name, os.environ['HOME'] + "/code/grasp-pointnet/dex-net/data/grippers") ags = GpgGraspSamplerPcl(gripper, yaml_config) value_fc = 0.4 # no use, set a random number num_grasps = 40 num_workers = 20 max_num_samples = 150 n_voxel = 500 minimal_points_send_to_point_net = 20 marker_life_time = 8 show_bad_grasp = False save_grasp_related_file = False show_final_grasp = args.show_final_grasp
else: good_grasp_rate = len(good_grasp) / count print("Gripper:{} Object:{} Rate:{:.4f} {}/{}". format(gripper_name, object_name_, good_grasp_rate, len(good_grasp), count)) if __name__ == "__main__": if len(sys.argv) > 1: filename_prefix = sys.argv[1] else: filename_prefix = "default" home_dir = os.environ["HOME"] file_dir = home_dir + "/code/PointNetGPD/PointNetGPD/data/ycb-tools/models/ycb" yaml_config = YamlConfig(home_dir + "/code/PointNetGPD/dex-net/test/config.yaml") gripper_name = "robotiq_85" gripper = RobotGripper.load(gripper_name, home_dir + "/code/PointNetGPD/dex-net/data/grippers") grasp_sample_method = "antipodal" file_list_all = get_file_name(file_dir) object_numbers = file_list_all.__len__() job_list = np.arange(object_numbers) job_list = list(job_list) pool_size = 1 # number of jobs did at same time assert (pool_size <= len(job_list)) # Initialize pool pool = [] for _ in range(pool_size): job_i = job_list.pop(0) pool.append(multiprocessing.Process(target=do_job, args=(job_i,))) [p.start() for p in pool] # refill