def open_npy_and_obj(name_to_open_): """从路径指定的npy文件中读取并返回 npy_m_: 抓取序列(只是针对某单个物体的抓取序列) obj_: 待抓取物体的模型mesh(单个模型) ply_name_: 待抓取物体的路径 object_name_: 待抓取物体的名称 """ npy_m_ = np.load(name_to_open_) file_dir = home_dir + "/PointNetGPD/data/ycb_meshes_google/" object_name_ = name_to_open_.split("/")[-1][:-4] #object_name_=object_name_.split("_")[-1][:-3] print(object_name_) 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") mesh = of.read() sdf = sf.read() obj_ = GraspableObject3D(sdf, mesh) else: cloud_path = home_dir + "/dataset/ycb_rgbd/" + object_name_ + "/clouds/" pcd_files = glob.glob(cloud_path + "*.pcd") obj_ = pcd_files obj_.sort() return npy_m_, obj_, ply_name_, object_name_
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 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") mesh = of.read() sdf = sf.read() obj_ = GraspableObject3D(sdf, mesh) else: cloud_path = home_dir + "/dataset/ycb_rgbd/" + object_name_ + "/clouds/" pcd_files = glob.glob(cloud_path + "*.pcd") obj_ = pcd_files obj_.sort() return npy_m_, obj_, ply_name_, object_name_
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 test_find_contacts(self): of = ObjFile(OBJ_FILENAME) sf = SdfFile(SDF_FILENAME) mesh = of.read() sdf = sf.read() obj = GraspableObject3D(sdf, mesh) surface_points, _ = obj.sdf.surface_points(grid_basis=False) indices = np.arange(surface_points.shape[0]) for i in range(NUM_TEST_CASES): np.random.shuffle(indices) c1 = surface_points[0,:] c2 = surface_points[1,:] w = np.linalg.norm(c1 - c2) + 1e-2 g = ParallelJawPtGrasp3D.grasp_from_endpoints(c1, c2, width=w) success, c = g.close_fingers(obj) if success: c1_est, c2_est = c np.assertTrue(np.allclose(c1, c1_est, atol=1e-3, rtol=0.1)) np.assertTrue(np.allclose(c2, c2_est, atol=1e-3, rtol=0.1))
def test_force_closure(self): of = ObjFile(OBJ_FILENAME) sf = SdfFile(SDF_FILENAME) mesh = of.read() sdf = sf.read() obj = GraspableObject3D(sdf, mesh) for i in range(NUM_TEST_CASES): contacts, normals, _, mu, _ = random_force_closure_test_case(antipodal=True) c1 = Contact3D(obj, contacts[:,0]) c1.normal = normals[:,0] c2 = Contact3D(obj, contacts[:,1]) c2.normal = normals[:,1] self.assertTrue(PointGraspMetrics3D.force_closure(c1, c2, mu, use_abs_value=False)) for i in range(NUM_TEST_CASES): contacts, normals, _, mu, _ = random_force_closure_test_case(antipodal=False) c1 = Contact3D(obj, contacts[:,0]) c1.normal = normals[:,0] c2 = Contact3D(obj, contacts[:,1]) c2.normal = normals[:,1] self.assertFalse(PointGraspMetrics3D.force_closure(c1, c2, mu, use_abs_value=False))
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 open_pickle_and_obj(name_to_open_): pickle_names_ = get_pickle_file_name( home_dir + "/code/grasp-pointnet/dex-net/apps/generated_grasps") suggestion_pickle = fuzzy_finder(name_to_open_, pickle_names_) if len(suggestion_pickle) != 1: print("Pickle file suggestions:", suggestion_pickle) exit("Name error for pickle file!") pickle_m_ = pickle.load(open(suggestion_pickle[0], 'rb')) file_dir = home_dir + "/dataset/ycb_meshes_google/objects" file_list_all = get_file_name(file_dir) new_sug = re.findall(r'_\d+', suggestion_pickle[0], flags=0) new_sug = new_sug[0].split('_') new_sug = new_sug[1] suggestion = fuzzy_finder(new_sug, file_list_all) # very dirty way to support name with "-a, -b etc." if len(suggestion) != 1: new_sug = re.findall(r'_\d+\W\w', suggestion_pickle[0], flags=0) new_sug = new_sug[0].split('_') new_sug = new_sug[1] suggestion = fuzzy_finder(new_sug, file_list_all) if len(suggestion) != 1: exit("Name error for obj file!") object_name_ = suggestion[0][len(file_dir) + 1:] ply_name_ = suggestion[0] + "/google_512k/nontextured.ply" if not check_pcd_grasp_points: of = ObjFile(suggestion[0] + "/google_512k/nontextured.obj") sf = SdfFile(suggestion[0] + "/google_512k/nontextured.sdf") mesh = of.read() sdf = sf.read() obj_ = GraspableObject3D(sdf, mesh) else: cloud_path = home_dir + "/code/grasp-pointnet/pointGPD/data/ycb_rgbd/" + object_name_ + "/clouds/" pcd_files = glob.glob(cloud_path + "*.pcd") obj_ = pcd_files obj_.sort() return pickle_m_, obj_, ply_name_, object_name_
def open_pickle_and_obj(object_to_display): pickle_names_ = get_pickle_file_name(grasps_dir) suggestion_pickle = fuzzy_finder(object_to_display['object_name'], pickle_names_) if len(suggestion_pickle) != 1: print("Pickle file suggestions:", suggestion_pickle) exit("Name error for pickle file!") pickle_m_ = pickle.load(open(suggestion_pickle[0], 'rb')) object_name_ = object_to_display['object_name'] ply_name_ = object_to_display['obj_file'].replace('.obj', '.stl') if not check_pcd_grasp_points: of = ObjFile(object_to_display['obj_file']) sf = SdfFile(object_to_display['sdf_file']) mesh = of.read() sdf = sf.read() obj_ = GraspableObject3D(sdf, mesh) else: cloud_path = home_dir + "/code/grasp-pointnet/pointGPD/data/ycb_rgbd/" + object_name_ + "/clouds/" pcd_files = glob.glob(cloud_path + "*.pcd") obj_ = pcd_files obj_.sort() return pickle_m_, obj_, ply_name_, object_name_
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 do_job(job_i): ii = np.random.choice(all_p.shape[0]) show_grasp_norm(all_p[ii], surface_normal[ii]) print("done job", job_i, ii) if __name__ == '__main__': home_dir = os.environ['HOME'] file_dir = home_dir + "/dataset/ycb_meshes_google/objects" mlab.figure(bgcolor=(1.0, 1.0, 1.0), fgcolor=(0.7, 0.7, 0.7)) file_list_all = get_file_name(file_dir) object_numbers = file_list_all.__len__() i = 0 # index of objects to define which object to show 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() graspable = GraspableObject3D(sdf, mesh) print("Log: opened object") begin_time = time.time() surface_points, _ = graspable.sdf.surface_points(grid_basis=False) all_p = surface_points method = "voxel" if method == "random": surface_points = surface_points[np.random.choice(
def do_job(job_i): ii = np.random.choice(all_p.shape[0]) show_grasp_norm(all_p[ii], surface_normal[ii]) print("done job", job_i, ii) if __name__ == '__main__': home_dir = os.environ['HOME'] file_dir = home_dir + "/Projects/PointNetGPD/dataset/ycb_meshes_google/" # for google ycb mlab.figure(bgcolor=(1, 1, 1), fgcolor=(0.7, 0.7, 0.7)) file_list_all = get_file_name(file_dir) object_numbers = file_list_all.__len__() i = 0 # index of objects to define which object to show 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() graspable = GraspableObject3D(sdf, mesh) print("Log: opened object") begin_time = time.time() surface_points, _ = graspable.sdf.surface_points(grid_basis=False) all_p = surface_points method = "voxel" if method == "random": surface_points = surface_points[np.random.choice(surface_points.shape[0], 1000, replace=False)]
def test_new_database_and_graspable(self): # new database database = Hdf5Database(TEST_DB_NAME, access_level=READ_WRITE_ACCESS) database.close() self.assertTrue(database is not None) # new dataset database = Hdf5Database(TEST_DB_NAME, access_level=READ_WRITE_ACCESS) database.create_dataset(TEST_DS_NAME) database.close() self.assertTrue(database is not None) # read existing dataset database = Hdf5Database(TEST_DB_NAME, access_level=READ_WRITE_ACCESS) dataset = database.dataset(TEST_DS_NAME) self.assertTrue(database is not None and dataset is not None) # create graspable mass = 1.0 CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE mesh_processor = MeshProcessor(OBJ_FILENAME, CONFIG['cache_dir']) mesh_processor.generate_graspable(CONFIG) dataset.create_graspable(mesh_processor.key, mesh_processor.mesh, mesh_processor.sdf, mesh_processor.stable_poses, mass=mass) # read graspable and ensure data integrity obj = dataset[mesh_processor.key] self.assertTrue(obj.key == mesh_processor.key) write_vertices = mesh_processor.mesh.vertices write_triangles = mesh_processor.mesh.triangles write_sdf_data = mesh_processor.sdf.data write_stable_poses = mesh_processor.stable_poses load_vertices = obj.mesh.vertices load_triangles = obj.mesh.triangles load_sdf_data = obj.sdf.data load_stable_poses = dataset.stable_poses(obj.key) self.assertTrue(np.allclose(write_vertices, load_vertices)) self.assertTrue(np.allclose(write_triangles, load_triangles)) self.assertTrue(np.allclose(write_sdf_data, load_sdf_data)) self.assertTrue(obj.mass == mass) for wsp, lsp in zip(write_stable_poses, load_stable_poses): self.assertTrue(np.allclose(wsp.r, lsp.r)) self.assertTrue(np.allclose(wsp.p, lsp.p)) self.assertTrue(database is not None and dataset is not None) # test loop access for obj in dataset: key = obj.key # test direct access obj = dataset[key] self.assertTrue(obj.key == key) # read / write meshing obj = dataset[dataset.object_keys[0]] mesh_filename = dataset.obj_mesh_filename(obj.key, overwrite=True) f = ObjFile(mesh_filename) load_mesh = f.read() write_vertices = np.array(obj.mesh.vertices) write_triangles = np.array(obj.mesh.triangles) load_vertices = np.array(load_mesh.vertices) load_triangles = np.array(load_mesh.triangles) self.assertTrue(np.allclose(write_vertices, load_vertices, atol=1e-5)) self.assertTrue(np.allclose(write_triangles, load_triangles, atol=1e-5)) # test rendering images stable_poses = dataset.stable_poses(obj.key) stable_pose = stable_poses[0] # setup virtual camera width = CONFIG['width'] height = CONFIG['height'] f = CONFIG['focal'] cx = float(width) / 2 cy = float(height) / 2 ci = CameraIntrinsics('camera', fx=f, fy=f, cx=cx, cy=cy, height=height, width=width) vp = ViewsphereDiscretizer(min_radius=CONFIG['min_radius'], max_radius=CONFIG['max_radius'], num_radii=CONFIG['num_radii'], min_elev=CONFIG['min_elev']*np.pi, max_elev=CONFIG['max_elev']*np.pi, num_elev=CONFIG['num_elev'], num_az=CONFIG['num_az'], num_roll=CONFIG['num_roll']) vc = VirtualCamera(ci) # render segmasks and depth render_modes = [RenderMode.SEGMASK, RenderMode.DEPTH, RenderMode.SCALED_DEPTH] for render_mode in render_modes: rendered_images = vc.wrapped_images_viewsphere(obj.mesh, vp, render_mode, stable_pose) pre_store_num_images = len(rendered_images) dataset.store_rendered_images(obj.key, rendered_images, stable_pose_id=stable_pose.id, render_mode=render_mode) rendered_images = dataset.rendered_images(obj.key, stable_pose_id=stable_pose.id, render_mode=render_mode) post_store_num_images = len(rendered_images) self.assertTrue(pre_store_num_images == post_store_num_images) # test read / write grasp metrics metric_name = CONFIG['metrics'].keys()[0] metric_config = CONFIG['metrics'][metric_name] dataset.create_metric(metric_name, metric_config) load_metric_config = dataset.metric(metric_name) self.assertTrue(dataset.has_metric(metric_name)) for key, value in metric_config.iteritems(): if isinstance(value, dict): for k, v in value.iteritems(): self.assertTrue(load_metric_config[key][k] == v) else: self.assertTrue(load_metric_config[key] == value) dataset.delete_metric(metric_name) self.assertFalse(dataset.has_metric(metric_name)) # test read / write grasps num_grasps = NUM_DB_GRASPS database = Hdf5Database(TEST_DB_NAME, access_level=READ_WRITE_ACCESS) dataset = database.dataset(TEST_DS_NAME) key = dataset.object_keys[0] grasps = [] grasp_metrics = {} for i in range(num_grasps): configuration = np.random.rand(9) configuration[3:6] = configuration[3:6] / np.linalg.norm(configuration[3:6]) random_grasp = ParallelJawPtGrasp3D(configuration) grasps.append(random_grasp) dataset.store_grasps(key, grasps) loaded_grasps = dataset.grasps(key) for g in loaded_grasps: grasp_metrics[g.id] = {} grasp_metrics[g.id]['force_closure'] = 1 * (np.random.rand() > 0.5) for g1, g2 in zip(grasps, loaded_grasps): self.assertTrue(np.allclose(g1.configuration, g2.configuration)) self.assertTrue(dataset.has_grasps(key)) dataset.store_grasp_metrics(key, grasp_metrics) loaded_grasp_metrics = dataset.grasp_metrics(key, loaded_grasps) for i, metrics in loaded_grasp_metrics.iteritems(): self.assertTrue(metrics['force_closure'] == grasp_metrics[i]['force_closure']) # remove grasps dataset.delete_grasps(key) self.assertFalse(dataset.has_grasps(key)) # remove rendered images render_modes = [RenderMode.SEGMASK, RenderMode.DEPTH, RenderMode.SCALED_DEPTH] for render_mode in render_modes: dataset.delete_rendered_images(key, stable_pose_id=stable_pose.id, render_mode=render_mode) rendered_images = dataset.rendered_images(key, stable_pose_id=stable_pose.id, render_mode=render_mode) self.assertTrue(len(rendered_images) == 0) # remove graspable database = Hdf5Database(TEST_DB_NAME, access_level=READ_WRITE_ACCESS) dataset = database.dataset(TEST_DS_NAME) key = dataset.object_keys[0] dataset.delete_graspable(key) obj_deleted = False try: obj = dataset[key] except: obj_deleted = True self.assertTrue(obj_deleted) database.close()
import os import copy import numpy as np import open3d as o3d from meshpy.obj_file import ObjFile from meshpy.sdf_file import SdfFile home_dir = os.environ['HOME'] file_dir = home_dir + "/Projects/GPD_PointNet/extract_normals" of = ObjFile(file_dir + "/mesh_ascii_with_normal_crop.obj") obj = of.read() center_of_mass = obj.center_of_mass print(center_of_mass) cloud = o3d.io.read_point_cloud(file_dir + "/mesh_ascii_with_normal.ply") mesh = o3d.io.read_triangle_mesh(file_dir + "/mesh_ascii_with_normal.ply") mesh_sphere = o3d.geometry.create_mesh_sphere(radius=0.005) mesh_sphere.compute_vertex_normals() mesh_sphere.paint_uniform_color([1.0, 0.0, 0.0]) transformation = np.identity(4) transformation[:3, 3] = center_of_mass mesh_sphere.transform(transformation) o3d.draw_geometries([cloud, mesh_sphere])
def test_init_graspable(self): of = ObjFile(OBJ_FILENAME) sf = SdfFile(SDF_FILENAME) mesh = of.read() sdf = sf.read() obj = GraspableObject3D(sdf, mesh)
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))
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))
if __name__ == '__main__': home_dir = os.environ['HOME'] file_dir = home_dir + "/dataset/ycb_meshes_google/objects" #sf = SdfFile(home_dir + "/dataset/ycb_meshes_google/003_cracker_box/google_512k/nontextured.sdf") #of = ObjFile(home_dir + "/dataset/ycb_meshes_google/003_cracker_box/google_512k/nontextured.obj") #mesh = of.read() #sdf = sf.read() #obj = GraspableObject3D(sdf, mesh) #render_sdf(obj, "learn0") print("done") if len(sys.argv) > 1: sdf_filename = sys.argv[1] else: sdf_filename = DEF_SDF_FILE # sdf_file = read_sdf(sdf_filename) # render_sdf(sdf_file, 1) obj_files = get_file_name(file_dir) obj_files = obj_files[1:4] for items in obj_files: object_name = items[len(home_dir) + 35:] sf = SdfFile(items + "/google_512k/nontextured.sdf") of = ObjFile(items + "/google_512k/nontextured.obj") mesh = of.read() sdf = sf.read() obj = GraspableObject3D(sdf, mesh) render_sdf(obj, object_name)
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))