def test_wrench_in_positive_span(self): # simple test for in positive span wrench_basis = np.eye(6) force_limit = 1000 num_fingers = 1 # truly in span, with force limits for i in range(NUM_TEST_CASES): target_wrench = np.random.rand(6) in_span, norm = PointGraspMetrics3D.wrench_in_positive_span(wrench_basis, target_wrench, force_limit, num_fingers) self.assertTrue(in_span) # not in span, but within force limits for i in range(NUM_TEST_CASES): target_wrench = -np.random.rand(6) in_span, norm = PointGraspMetrics3D.wrench_in_positive_span(wrench_basis, target_wrench, force_limit, num_fingers) self.assertFalse(in_span) # truly in span, but not with force limits force_limit = 0.1 for i in range(NUM_TEST_CASES): target_wrench = np.random.rand(6) target_wrench[0] = 1000 in_span, norm = PointGraspMetrics3D.wrench_in_positive_span(wrench_basis, target_wrench, force_limit, num_fingers) self.assertFalse(in_span)
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 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 sample(self, size=1): """ Samples deterministic quasi-static point grasp quality metrics. Parameters ---------- size : int number of samples to take """ # sample grasp cur_time = time.time() grasp_sample = self.grasp_rv_.rvs(size=1, iteration=self.sample_count_) grasp_time = time.time() # sample object obj_sample = self.obj_rv_.rvs(size=1, iteration=self.sample_count_) obj_time = time.time() # sample params params_sample = None if self.params_rv_ is not None: params_sample = self.params_rv_.rvs(size=1, iteration=self.sample_count_) params_time = time.time() logging.debug('Sampling took %.3f sec' % (params_time - cur_time)) # compute deterministic quality start = time.time() q = PointGraspMetrics3D.grasp_quality(grasp_sample, obj_sample, params_sample) quality_time = time.time() logging.debug('Quality comp took %.3f sec' % (quality_time - start)) self.sample_count_ = self.sample_count_ + 1 return q
def test_min_norm_vector_in_facet(self): # zero in facet facet = np.c_[np.eye(6), -np.eye(6)] min_norm, _ = PointGraspMetrics3D.min_norm_vector_in_facet(facet) self.assertLess(min_norm, 1e-5) # simplex facet = np.c_[np.eye(6)] min_norm, v = PointGraspMetrics3D.min_norm_vector_in_facet(facet) true_v = (1.0/6.0)*np.ones(6) self.assertTrue(np.allclose(min_norm, np.linalg.norm(true_v))) self.assertTrue(np.allclose(v, true_v)) # single data point, edge case facet = np.ones([6,1]) min_norm, v = PointGraspMetrics3D.min_norm_vector_in_facet(facet) self.assertTrue(np.allclose(min_norm, np.linalg.norm(facet))) self.assertTrue(np.allclose(v, facet))
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 quality(self, grasp): """ Compute grasp quality using a quasistatic method. Parameters ---------- grasp : :obj:`GraspableObject3D` grasp to quality quality on Returns ------- :obj:`GraspQualityResult` result of quality computation """ if not isinstance(grasp, Grasp): raise ValueError('Must provide Grasp object to compute quality') quality = PointGraspMetrics3D.grasp_quality(grasp, self.graspable_, self.quality_config_) return GraspQualityResult(quality, quality_config=self.quality_config_)
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 sample_grasps(self, graspable, num_grasps, vis=False, direction=None, stable_pose=None): """Returns a list of candidate grasps for graspable object. Parameters ---------- graspable : :obj:`GraspableObject3D` the object to grasp num_grasps : int number of grasps to sample vis : bool whether or not to visualize progress, for debugging Returns ------- :obj:`list` of :obj:`ParallelJawPtGrasp3D` the sampled grasps """ # get surface points grasps = [] surface_points, _ = graspable.sdf.surface_points(grid_basis=False) np.random.shuffle(surface_points) shuffled_surface_points = surface_points[:min(self.max_num_surface_points_, len(surface_points))] logging.info('Num surface: %d' %(len(surface_points))) for k, x_surf in enumerate(shuffled_surface_points): start_time = time.clock() # perturb grasp for num samples for i in range(self.num_samples): # perturb contact (TODO: sample in tangent plane to surface) x1 = self.perturb_point(x_surf, graspable.sdf.resolution) # compute friction cone faces c1 = Contact3D(graspable, x1) _, tx1, ty1 = c1.tangents() cone_succeeded, cone1, n1 = c1.friction_cone(self.num_cone_faces, self.friction_coef) if not cone_succeeded: continue cone_time = time.clock() # sample grasp axes from friction cone v_samples = self.sample_from_cone(n1, tx1, ty1, num_samples=1) sample_time = time.clock() # ------------ CHECK GRASP IS PARALLEL TO TABLE ---------------------- if stable_pose != None: angle = np.dot(np.matmul(stable_pose, v_samples[0]), np.array([0,0,1])) # don't save if angle greater than 36 degrees if abs(angle) > 0.2: continue # -------------------------------------------------------------------- for v in v_samples: # random axis flips since we don't have guarantees on surface normal directoins if random.random() > 0.5: v = -v # start searching for contacts grasp, c1, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid(graspable, x1, v, self.gripper.max_width, min_grasp_width_world=self.gripper.min_width, vis=vis) if grasp is None or c2 is None: continue # get true contacts (previous is subject to variation) success, c = grasp.close_fingers(graspable) if not success: continue c1 = c[0] c2 = c[1] # make sure grasp is wide enough x2 = c2.point if np.linalg.norm(x1 - x2) < self.min_contact_dist: continue v_true = grasp.axis # compute friction cone for contact 2 cone_succeeded, cone2, n2 = c2.friction_cone(self.num_cone_faces, self.friction_coef) if not cone_succeeded: continue # check friction cone if PointGraspMetrics3D.force_closure(c1, c2, self.friction_coef): grasps.append(grasp) # randomly sample max num grasps from total list random.shuffle(grasps) return grasps
def sample_grasps(self, graspable, num_grasps, vis=False): """Returns a list of candidate grasps for graspable object. Parameters ---------- graspable : :obj:`GraspableObject3D` the object to grasp num_grasps : int number of grasps to sample vis : bool whether or not to visualize progress, for debugging Returns ------- :obj:`list` of :obj:`ParallelJawPtGrasp3D` the sampled grasps """ # get surface points grasps = [] surface_points, _ = graspable.sdf.surface_points(grid_basis=False) np.random.shuffle(surface_points) shuffled_surface_points = surface_points[:min( self.max_num_surface_points_, len(surface_points))] logging.info('Num surface: %d' % (len(surface_points))) for k, x_surf in enumerate(shuffled_surface_points): start_time = time.clock() # perturb grasp for num samples for i in range(self.num_samples): # perturb contact (TODO: sample in tangent plane to surface) x1 = self.perturb_point(x_surf, graspable.sdf.resolution) # compute friction cone faces c1 = Contact3D(graspable, x1, in_direction=None) _, tx1, ty1 = c1.tangents() cone_succeeded, cone1, n1 = c1.friction_cone( self.num_cone_faces, self.friction_coef) if not cone_succeeded: continue cone_time = time.clock() # sample grasp axes from friction cone v_samples = self.sample_from_cone( n1, tx1, ty1, num_samples=1) # #这个不是approaching vector而是抓取的闭合方向 sample_time = time.clock() for v in v_samples: if vis: x1_grid = graspable.sdf.transform_pt_obj_to_grid(x1) cone1_grid = graspable.sdf.transform_pt_obj_to_grid( cone1, direction=True) plt.clf() h = plt.gcf() plt.ion() ax = plt.gca(projection='3d') for i in range(cone1.shape[1]): ax.scatter(x1_grid[0] - cone1_grid[0], x1_grid[1] - cone1_grid[1], x1_grid[2] - cone1_grid[2], s=50, c=u'm') # random axis flips since we don't have guarantees on surface normal directoins if random.random() > 0.5: v = -v # start searching for contacts grasp, c1, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid( graspable, x1, v, self.gripper.max_width, min_grasp_width_world=0.0, #self.gripper.min_width, vis=vis) if grasp is None or c2 is None: continue # get true contacts (previous is subject to variation) success, c = grasp.close_fingers(graspable) if not success: continue c1 = c[0] c2 = c[1] # make sure grasp is wide enough x2 = c2.point # if np.linalg.norm(x1 - x2) < self.min_contact_dist: # continue v_true = grasp.axis #这个不是approaching vector而是抓取的闭合方向 # compute friction cone for contact 2 cone_succeeded, cone2, n2 = c2.friction_cone( self.num_cone_faces, self.friction_coef) if not cone_succeeded: continue if vis: plt.figure() ax = plt.gca(projection='3d') c1_proxy = c1.plot_friction_cone(color='m') c2_proxy = c2.plot_friction_cone(color='y') ax.view_init(elev=5.0, azim=0) plt.show(block=False) time.sleep(0.5) plt.close() # lol # check friction cone if PointGraspMetrics3D.force_closure( c1, c2, self.friction_coef): print 'c2.point', c2.point grasp.touch_pointc1_ = c1.point grasp.touch_pointc2_ = c2.point grasps.append(grasp) #需要在这里加入一个点,也就是根据grasp.center,grasp.axis,copy.deepcopy(grasp), theta = self._angle_aligned_with_table(table_normal) #生成目标angle #中心也要变 ##########################################new added #先求点法式方程: # edge_margin=0.004 #设定为边界余量 # common_perpendicular_threshold=0.0001 #设定为公垂线距离阈值,阈值越小,精度越高,但得到的点也越少 # gn= c1.point-c2.point # unit_gn=gn/np.linalg.norm(gn) # gcenterpoint=(c1.point+c2.point)/2.0 # gA=gn[0] # gB=gn[1] # gC=gn[2] # gD=-gA*gcenterpoint[0]-gB*gcenterpoint[1]-gC*gcenterpoint[2] # # if (np.linalg.norm(c1.point-c2.point)/2.0<edge_margin):#太窄 # #则这时候限制一个edge_margin意义不大 # uplimit=np.linalg.norm(c1.point-c2.point)/2.0 # else: # uplimit=np.linalg.norm(c1.point-c2.point)/2.0-edge_margin # # # # print 'new grasp contact' # for gi in range(0,len(surface_points)): # #每个表面的点求点到面的距离 # distance= math.fabs(surface_points[gi][0]*gA+surface_points[gi][1]*gB+surface_points[gi][2]*gC+gD)/(math.sqrt(gA*gA+gB*gB+gC*gC)) # if distance > uplimit: # continue # else:#在这个uplimit内,则可以开始考虑其法线了 # #获得这个点的法线 # normalvalue=Contact3D(graspable, surface_points[gi], in_direction=None).normal # if len(normalvalue)==0: # print 'no normal value' # continue # #获得过grasp.axis且平行于normalvalue的平面(以便求公垂线的距离) # #先求这个面的法线: # unit_normalvalue=normalvalue/np.linalg.norm(normalvalue) # planenorm=np.cross(unit_normalvalue,unit_gn) # unit_planenorm=planenorm/np.linalg.norm(planenorm) # # #故有点法式面方程:法线:unit_planenorm,点:gcenterpoint # ggA=unit_planenorm[0] # ggB=unit_planenorm[1] # ggC=unit_planenorm[2] # ggD=-ggA*gcenterpoint[0]-ggB*gcenterpoint[1]-ggC*gcenterpoint[2] # distance_common_perpendicular= math.fabs(surface_points[gi][0]*ggA+surface_points[gi][1]*ggB+surface_points[gi][2]*ggC+ggD)/(math.sqrt(ggA*ggA+ggB*ggB+ggC*ggC)) # if (distance_common_perpendicular>common_perpendicular_threshold): # continue # else:#否则,这个点surface_points[gi]则可被视为approaching点,下面可以进一步计算其法线对应的approaching angle # # #加入碰撞检测!!!!!!!!!! # # # newgrasp=copy.deepcopy(grasp) # print 'surface_points[gi]',surface_points[gi] # newgrasp.approach_point_=surface_points[gi] # #print 'approach_point_[gi]',newgrasp.approach_point_ # #这里需要重新求grasp.center: 也就是grasp.axis # # newgrasp.approach_angle_=grasp._angle_aligned_with_table(unit_normalvalue)#注意,这个angle未必会过grasp.center # print 'approach_angle_',newgrasp.approach_angle_ # grasps.append(newgrasp) # print len(grasps),' in ',self.target_num_grasps # if len(grasps) >self.target_num_grasps: # break # if len(grasps) >self.target_num_grasps: # break # # # if len(grasps) >self.target_num_grasps: # break #对每个点,判断该点到grasp.center的向量(设为approaching vector)与grasp.axis是否垂直 #再看这个点的法线和approaching vector是否重合 # randomly sample max num grasps from total list random.shuffle(grasps) print 'the number:', len(grasps) return grasps
def sample_grasps(self, graspable, num_grasps, vis=False): """Returns a list of candidate grasps for graspable object. Parameters ---------- graspable : :obj:`GraspableObject3D` the object to grasp num_grasps : int number of grasps to sample vis : bool whether or not to visualize progress, for debugging Returns ------- :obj:`list` of :obj:`ParallelJawPtGrasp3D` the sampled grasps """ # get surface points grasps = [] surface_points, _ = graspable.sdf.surface_points(grid_basis=False) np.random.shuffle(surface_points) shuffled_surface_points = surface_points[:min( self.max_num_surface_points_, len(surface_points))] logging.info('Num surface: %d' % (len(surface_points))) for k, x_surf in enumerate(shuffled_surface_points): start_time = time.clock() # perturb grasp for num samples for i in range(self.num_samples): # perturb contact (TODO: sample in tangent plane to surface) x1 = self.perturb_point(x_surf, graspable.sdf.resolution) # compute friction cone faces c1 = Contact3D(graspable, x1, in_direction=None) _, tx1, ty1 = c1.tangents() cone_succeeded, cone1, n1 = c1.friction_cone( self.num_cone_faces, self.friction_coef) if not cone_succeeded: continue cone_time = time.clock() # sample grasp axes from friction cone v_samples = self.sample_from_cone(n1, tx1, ty1, num_samples=1) sample_time = time.clock() for v in v_samples: if vis: x1_grid = graspable.sdf.transform_pt_obj_to_grid(x1) cone1_grid = graspable.sdf.transform_pt_obj_to_grid( cone1, direction=True) plt.clf() h = plt.gcf() plt.ion() ax = plt.gca(projection='3d') for i in range(cone1.shape[1]): ax.scatter(x1_grid[0] - cone1_grid[0], x1_grid[1] - cone1_grid[1], x1_grid[2] - cone1_grid[2], s=50, c=u'm') # random axis flips since we don't have guarantees on surface normal directoins if random.random() > 0.5: v = -v # start searching for contacts grasp, c1, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid( graspable, x1, v, self.gripper.max_width, min_grasp_width_world=self.gripper.min_width, vis=vis) if grasp is None or c2 is None: continue # get true contacts (previous is subject to variation) success, c = grasp.close_fingers(graspable) if not success: continue c1 = c[0] c2 = c[1] # make sure grasp is wide enough x2 = c2.point if np.linalg.norm(x1 - x2) < self.min_contact_dist: continue v_true = grasp.axis # compute friction cone for contact 2 cone_succeeded, cone2, n2 = c2.friction_cone( self.num_cone_faces, self.friction_coef) if not cone_succeeded: continue if vis: plt.figure() ax = plt.gca(projection='3d') c1_proxy = c1.plot_friction_cone(color='m') c2_proxy = c2.plot_friction_cone(color='y') ax.view_init(elev=5.0, azim=0) plt.show(block=False) time.sleep(0.5) plt.close() # lol # check friction cone if PointGraspMetrics3D.force_closure( c1, c2, self.friction_coef): grasps.append(grasp) # randomly sample max num grasps from total list random.shuffle(grasps) return grasps
def sample_grasps(self, graspable, openning_ratio_id, openning_ratios, vis=False): """Returns a list of candidate grasps for graspable object. Parameters ---------- graspable : :obj:`GraspableObject3D` the object to grasp openning_ratio_id : int initial gripper openning ratio for sampling; not actual grasp openning ratio openning_ratios : list all possible opening ratios vis : bool whether or not to visualize progress, for debugging Returns ------- :obj:`list` of :obj:`ParallelJawPtGrasp3D` the sampled grasps """ # get surface points grasps = [] surface_points, _ = graspable.sdf.surface_points(grid_basis=False) np.random.shuffle(surface_points) shuffled_surface_points = surface_points[:min(self.max_num_surface_points_, len(surface_points))] logging.info('Num surface: %d' %(len(surface_points))) for k, x_surf in enumerate(shuffled_surface_points): start_time = time.clock() # perturb grasp for num samples for i in range(self.num_samples): # perturb contact (TODO: sample in tangent plane to surface) x1 = self.perturb_point(x_surf, graspable.sdf.resolution) # compute friction cone faces c1 = Contact3D(graspable, x1, in_direction=None) _, tx1, ty1 = c1.tangents() cone_succeeded, cone1, n1 = c1.friction_cone(self.num_cone_faces, self.friction_coef) if not cone_succeeded: continue cone_time = time.clock() # sample grasp axes from friction cone v_samples = self.sample_from_cone(n1, tx1, ty1, num_samples=1) sample_time = time.clock() for v in v_samples: if vis: x1_grid = graspable.sdf.transform_pt_obj_to_grid(x1) cone1_grid = graspable.sdf.transform_pt_obj_to_grid(cone1, direction=True) plt.clf() h = plt.gcf() plt.ion() ax = plt.gca(projection = '3d') for i in range(cone1.shape[1]): ax.scatter(x1_grid[0] - cone1_grid[0], x1_grid[1] - cone1_grid[1], x1_grid[2] - cone1_grid[2], s = 50, c = u'm') # # random axis flips since we don't have guarantees on surface normal directoins # if random.random() > 0.5: # v = -v # randomly pick grasp width & angle grasp_width = openning_ratios[openning_ratio_id] * self.gripper.max_width grasp_angle = np.random.rand() * np.pi * 2 # start searching for contacts grasp, c1, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid(graspable, x1, v, grasp_width, grasp_angle=grasp_angle, min_grasp_width_world=self.gripper.min_width, vis=vis) if grasp is None or c2 is None: continue # get true contacts (previous is subject to variation) success, c = grasp.close_fingers(graspable) if not success: continue c1 = c[0] c2 = c[1] # make sure grasp is wide enough if np.linalg.norm(c1.point - c2.point) < self.min_contact_dist: continue # update grasp center grasp.center = ParallelJawPtGrasp3D.center_from_endpoints(c1.point, c2.point) # compute friction cone for new contacts cone_succeeded, cone1, n1 = c1.friction_cone(self.num_cone_faces, self.friction_coef) if not cone_succeeded: continue cone_succeeded, cone2, n2 = c2.friction_cone(self.num_cone_faces, self.friction_coef) if not cone_succeeded: continue # check friction cone if PointGraspMetrics3D.force_closure(c1, c2, self.friction_coef): # try to find minimum possible openning width original_max_width = grasp.max_grasp_width_ for index in range(openning_ratio_id): grasp.max_grasp_width_ = openning_ratios[index] * self.gripper.max_width success, _ = grasp.close_fingers(graspable) if success: break else: grasp.max_grasp_width_ = original_max_width grasps.append(grasp) # randomly sample max num grasps from total list random.shuffle(grasps) return grasps
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 antipodal_grasp_sampler(visual=False, debug=False): 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 stable_poses = mesh_processor.stable_poses obj = GraspableObject3D(sdf, mesh) stable_pose = stable_poses[0] if visual: 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) if debug: print(len(stable_poses)) #for stable_pose in stable_poses: # print(stable_pose.p) # print(stable_pose.r) # print(stable_pose.x0) # print(stable_pose.face) # print(stable_pose.id) # glass = 22 is standing straight if debug: print(stable_pose.r) print(stable_pose.T_obj_world) gripper = RobotGripper.load( GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers') ags = AntipodalGraspSampler(gripper, CONFIG) stable_pose.id = 0 grasps = ags.generate_grasps(obj, target_num_grasps=20, max_iter=5, stable_pose=stable_pose.r) quality_config = GraspQualityConfigFactory.create_config( CONFIG['metrics']['robust_ferrari_canny']) metrics = [] result = [] #grasps = map(lambda g : g.perpendicular_table(stable_pose), grasps) for grasp in grasps: c1, c2 = grasp.endpoints true_fc = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config) metrics.append(true_fc) result.append((c1, c2)) if debug: success, c = grasp.close_fingers(obj) if success: c1, c2 = c print("Grasp:") print(c1.point) print(c2.point) print(true_fc) 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) if visual: for grasp, metric in zip(grasps, metrics): #grasp2 = grasp.perpendicular_table(stable_pose) #c1, c2 = grasp.endpoints #axis = ParallelJawPtGrasp3D.axis_from_endpoints(c1, c2) #angle = np.dot(np.matmul(stable_pose.r, axis), [1,0,0]) #angle = math.tan(axis[1]/axis[0]) #angle = math.degrees(angle)%360 #print(angle) #print(angle/360.0) color = plt.get_cmap('hsv')(q_to_c(metric))[:-1] vis.grasp(grasp, T_obj_world=T_obj_world, grasp_axis_color=color, endpoint_color=color) #axis = np.array([[0,0,0], point]) #points = [(x[0], x[1], x[2]) for x in axis] #Visualizer3D.plot3d(points, color=(0,0,1), tube_radius=0.002) vis.show(False) pose_matrix = np.eye(4, 4) pose_matrix[:3, :3] = T_obj_world.rotation pose_matrix[:3, 3] = T_obj_world.translation return pose_matrix, result