def grasps(data): """ Return a list of grasp objects from the data provided in the HDF5 dictionary """ # need to read in a bunch of grasps but also need to know what kind of grasp it is grasps = [] num_grasps = data.attrs[NUM_GRASPS_KEY] for i in range(num_grasps): # get the grasp data y'all grasp_key = GRASP_KEY + '_' + str(i) if grasp_key in list(data.keys()): grasp_id = data[grasp_key].attrs[GRASP_ID_KEY] grasp_type = data[grasp_key].attrs[GRASP_TYPE_KEY] configuration = data[grasp_key].attrs[GRASP_CONFIGURATION_KEY] frame = data[grasp_key].attrs[GRASP_RF_KEY] # create object based on type g = None if grasp_type == 'ParallelJawPtGrasp3D': g = ParallelJawPtGrasp3D(configuration=configuration, frame=frame, grasp_id=grasp_id) grasps.append(g) else: logging.debug('Grasp %s is corrupt. Skipping' %(grasp_key)) return grasps
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, sigma_scale=2.5): """ Returns a list of candidate grasps for graspable object by Gaussian with variance specified by principal dimensions. Parameters ---------- graspable : :obj:`GraspableObject3D` the object to grasp num_grasps : int the number of grasps to generate sigma_scale : float the number of sigmas on the tails of the Gaussian for each dimension Returns ------- :obj:`list` of obj:`ParallelJawPtGrasp3D` list of generated grasps """ # get object principal axes center_of_mass = graspable.mesh.center_of_mass principal_dims = graspable.mesh.principal_dims() sigma_dims = principal_dims / (2 * sigma_scale) # sample centers grasp_centers = stats.multivariate_normal.rvs( mean=center_of_mass, cov=sigma_dims**2, size=num_grasps) # samples angles uniformly from sphere u = stats.uniform.rvs(size=num_grasps) v = stats.uniform.rvs(size=num_grasps) thetas = 2 * np.pi * u phis = np.arccos(2 * v - 1.0) grasp_dirs = np.array([np.sin(phis) * np.cos(thetas), np.sin(phis) * np.sin(thetas), np.cos(phis)]) grasp_dirs = grasp_dirs.T # convert to grasp objects grasps = [] for i in range(num_grasps): grasp = ParallelJawPtGrasp3D(ParallelJawPtGrasp3D.configuration_from_params(grasp_centers[i,:], grasp_dirs[i,:], self.gripper.max_width)) contacts_found, contacts = grasp.close_fingers(graspable) # add grasp if it has valid contacts if contacts_found and np.linalg.norm(contacts[0].point - contacts[1].point) > self.min_contact_dist: grasps.append(grasp) # visualize if vis: for grasp in grasps: plt.clf() h = plt.gcf() plt.ion() grasp.close_fingers(graspable, vis=vis) plt.show(block=False) time.sleep(0.5) grasp_centers_grid = graspable.sdf.transform_pt_obj_to_grid(grasp_centers.T) grasp_centers_grid = grasp_centers_grid.T com_grid = graspable.sdf.transform_pt_obj_to_grid(center_of_mass) plt.clf() ax = plt.gca(projection = '3d') graspable.sdf.scatter() ax.scatter(grasp_centers_grid[:,0], grasp_centers_grid[:,1], grasp_centers_grid[:,2], s=60, c=u'm') ax.scatter(com_grid[0], com_grid[1], com_grid[2], s=120, c=u'y') ax.set_xlim3d(0, graspable.sdf.dims_[0]) ax.set_ylim3d(0, graspable.sdf.dims_[1]) ax.set_zlim3d(0, graspable.sdf.dims_[2]) plt.show() return grasps
def generate_grasps(self, graspable, target_num_grasps=None, grasp_gen_mult=5, max_iter=3, sample_approach_angles=False, vis=False, direction=None, stable_pose=None, **kwargs): """Samples a set of grasps for an object. Parameters ---------- graspable : :obj:`GraspableObject3D` the object to grasp target_num_grasps : int number of grasps to return, defualts to self.target_num_grasps grasp_gen_mult : int number of additional grasps to generate max_iter : int number of attempts to return an exact number of grasps before giving up sample_approach_angles : bool whether or not to sample approach angles Return ------ :obj:`list` of :obj:`ParallelJawPtGrasp3D` list of generated grasps """ # get num grasps if target_num_grasps is None: target_num_grasps = self.target_num_grasps num_grasps_remaining = target_num_grasps grasps = [] k = 1 while num_grasps_remaining > 0 and k <= max_iter: # SAMPLING: generate more than we need num_grasps_generate = grasp_gen_mult * num_grasps_remaining new_grasps = self.sample_grasps(graspable, num_grasps_generate, vis, direction=direction, stable_pose=stable_pose, **kwargs) # COVERAGE REJECTION: prune grasps by distance pruned_grasps = [] for grasp in new_grasps: min_dist = np.inf for cur_grasp in grasps: dist = ParallelJawPtGrasp3D.distance(cur_grasp, grasp) if dist < min_dist: min_dist = dist for cur_grasp in pruned_grasps: dist = ParallelJawPtGrasp3D.distance(cur_grasp, grasp) if dist < min_dist: min_dist = dist if min_dist >= self.grasp_dist_thresh_: pruned_grasps.append(grasp) # ANGLE EXPANSION sample grasp rotations around the axis candidate_grasps = [] if sample_approach_angles: for grasp in pruned_grasps: # construct a set of rotated grasps for i in range(self.num_grasp_rots): rotated_grasp = copy.copy(grasp) rotated_grasp.set_approach_angle(i * delta_theta) candidate_grasps.append(rotated_grasp) else: candidate_grasps = pruned_grasps # add to the current grasp set grasps += candidate_grasps logging.info('%d/%d grasps found after iteration %d.', len(grasps), target_num_grasps, k) grasp_gen_mult *= 2 num_grasps_remaining = target_num_grasps - len(grasps) k += 1 # shuffle computed grasps random.shuffle(grasps) if len(grasps) > target_num_grasps: logging.info('Truncating %d grasps to %d.', len(grasps), target_num_grasps) grasps = grasps[:target_num_grasps] logging.info('Found %d grasps.', 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) # #这个不是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