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)
