def gripper_on_object(gripper, grasp, obj, stable_pose=None, T_table_world=RigidTransform(from_frame='table', to_frame='world'), gripper_color=(0.5, 0.5, 0.5), object_color=(0.5, 0.5, 0.5), style='surface', plot_table=True, table_dim=0.15): """ Visualize a gripper on an object. Parameters ---------- gripper : :obj:`dexnet.grasping.RobotGripper` gripper to plot grasp : :obj:`dexnet.grasping.Grasp` grasp to plot the gripper in obj : :obj:`dexnet.grasping.GraspableObject3D` 3D object to plot the gripper on stable_pose : :obj:`meshpy.StablePose` or :obj:`autolab_core.RigidTransform` stable pose of the object on a planar worksurface T_table_world : :obj:`autolab_core.RigidTransform` pose of table, specified as a transformation from mesh frame to world frame gripper_color : 3-tuple color of the gripper mesh object_color : 3-tuple color of the object mesh style : :obj:`str` color of the object mesh plot_table : bool whether or not to plot the table table_dim : float dimension of the table """ if stable_pose is None: Visualizer3D.mesh(obj.mesh, color=object_color, style=style) T_obj_world = RigidTransform(from_frame='obj', to_frame='world') elif isinstance(stable_pose, StablePose): T_obj_world = Visualizer3D.mesh_stable_pose( obj.mesh, stable_pose, T_table_world=T_table_world, color=object_color, style=style, plot_table=plot_table, dim=table_dim) else: T_obj_world = Visualizer3D.mesh_table(obj.mesh, stable_pose, T_table_world=T_table_world, color=object_color, style=style, plot_table=plot_table, dim=table_dim) DexNetVisualizer3D.gripper(gripper, grasp, T_obj_world, color=gripper_color)
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
stable_pose = mesh.resting_pose(T_obj_table) #print stable_pose.r table_dim = 0.3 T_obj_table_plot = mesh.get_T_surface_obj(T_obj_table) T_obj_table_plot.translation[0] += 0.1 vis.figure() vis.mesh(mesh, T_obj_table_plot, color=(1, 0, 0), style='wireframe') vis.points(Point(mesh.center_of_mass, 'obj'), T_obj_table_plot, color=(1, 0, 1), scale=0.01) vis.pose(T_obj_table_plot, alpha=0.1) vis.mesh_stable_pose(mesh, stable_pose, dim=table_dim, color=(0, 1, 0), style='surface') vis.pose(stable_pose.T_obj_table, alpha=0.1) vis.show() exit(0) # compute stable poses vis.figure() vis.mesh(mesh, color=(1, 1, 0), style='surface') vis.mesh(mesh.convex_hull(), color=(1, 0, 0)) stable_poses = mesh.stable_poses() vis.show()
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