grasp_indices, best_metric_indices = sorted_contacts( vertices, normals, T_ar_object) for indices in best_metric_indices[0:5]: a = grasp_indices[indices][0] b = grasp_indices[indices][1] normal1, normal2 = normals[a], normals[b] contact1, contact2 = vertices[a], vertices[b] # visualize the mesh and contacts vis.figure() vis.mesh(mesh) vis.normals( NormalCloud(np.hstack( (normal1.reshape(-1, 1), normal2.reshape(-1, 1))), frame='test'), PointCloud(np.hstack( (contact1.reshape(-1, 1), contact2.reshape(-1, 1))), frame='test')) # vis.pose(T_obj_gripper, alpha=0.05) vis.show() if BAXTER_CONNECTED: repeat = True while repeat: # come in from the top... T_obj_gripper = contacts_to_baxter_hand_pose( contact1, contact2, np.array([0, 0, -1])) execute_grasp(T_obj_gripper, T_ar_object, ar_tag) repeat = bool(raw_input("repeat?")) exit()
# read data mesh_filename = args.mesh_filename _, mesh_ext = os.path.splitext(mesh_filename) if mesh_ext != '.obj': raise ValueError('Must provide mesh in Wavefront .OBJ format!') orig_mesh = ObjFile(mesh_filename).read() mesh = orig_mesh.subdivide(min_tri_length=0.01) mesh.compute_vertex_normals() stable_poses = mesh.stable_poses() if vis_normals: vis3d.figure() vis3d.mesh(mesh) vis3d.normals(NormalCloud(mesh.normals.T), PointCloud(mesh.vertices.T), subsample=10) vis3d.show() d = utils.sqrt_ceil(len(stable_poses)) vis.figure(size=(16, 16)) table_mesh = ObjFile('data/meshes/table.obj').read() table_mesh = table_mesh.subdivide() table_mesh.compute_vertex_normals() table_mat_props = MaterialProperties(color=(0, 255, 0), ambient=0.5, diffuse=1.0, specular=1, shininess=0)