def display_grasps(self, object_name, gripper_name, metric_name, config=None): """ Display grasps for an object Parameters ---------- object_name : :obj:`str` Object to display gripper_name : :obj:`str` Gripper for which to display grasps metric_name : :obj:`str` Metric to color/rank grasps with config : :obj:`dict` Configuration dict for visualization. Parameters are in Other parameters. Values from self.default_config are used for keys not provided. Other Parameters ---------------- gripper_dir Directory where the grippers models and parameters are. quality_scale Range to scale quality metric values to show_gripper Whether or not to show the gripper in the visualization min_metric lowest value of metric to show grasps for max_plot_gripper Number of grasps to plot animate Whether or not to animate the displayed object """ self._check_opens() config = self._get_config(config) grippers = os.listdir(config['gripper_dir']) if gripper_name not in grippers: raise ValueError( "{} is not a valid gripper name".format(gripper_name)) gripper = gr.RobotGripper.load(gripper_name, gripper_dir=config['gripper_dir']) objects = self.dataset.object_keys if object_name not in objects: raise ValueError( "{} is not a valid object name".format(object_name)) metrics = self.dataset.available_metrics(object_name, gripper=gripper.name) if metric_name not in metrics: raise ValueError( "{} is not computed for gripper {}, object {}".format( metric_name, gripper.name, object_name)) logger.info('Displaying grasps for gripper %s on object %s' % (gripper.name, object_name)) object = self.dataset[object_name] grasps, metrics = self.dataset.sorted_grasps(object_name, metric_name, gripper=gripper.name) if len(grasps) == 0: raise RuntimeError('No grasps for gripper %s on object %s' % (gripper.name, object_name)) return 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 config['show_gripper']: i = 0 stable_pose = self.dataset.stable_pose(object.key, 'pose_1') for grasp, metric in zip(grasps, metrics): if metric <= config['min_metric']: continue print 'Grasp %d %s=%.5f' % (grasp.id, metric_name, metric) T_obj_world = RigidTransform(from_frame='obj', to_frame='world') color = plt.get_cmap('hsv')(q_to_c(metric))[:-1] T_obj_gripper = grasp.gripper_pose(gripper) grasp = grasp.perpendicular_table(stable_pose) vis.figure() vis.gripper_on_object(gripper, grasp, object, gripper_color=(0.25, 0.25, 0.25), stable_pose=stable_pose, plot_table=False) vis.show(animate=config['animate']) i += 1 if i >= config['max_plot_gripper']: break else: i = 0 vis.figure() vis.mesh(object.mesh, style='surface') for grasp, metric in zip(grasps, metrics): if metric <= config['min_metric']: continue print 'Grasp %d %s=%.5f' % (grasp.id, metric_name, metric) T_obj_world = RigidTransform(from_frame='obj', to_frame='world') color = plt.get_cmap('hsv')(q_to_c(metric))[:-1] T_obj_gripper = grasp.gripper_pose(gripper) vis.grasp(grasp, grasp_axis_color=color, endpoint_color=color) i += 1 if i >= config['max_plot_gripper']: break vis.show(animate=config['animate'])
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
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