def write_pickle_file(objcm_name, push_info_list, root=None, file_name='preannotated_push.pickle', append=False): if root is None: root = './' gu.write_pickle_file(objcm_name, push_info_list, root=root, file_name=file_name, append=append)
if __name__ == '__main__': import robot_sim.end_effectors.gripper.yumi_gripper.yumi_gripper as yg import modeling.collision_model as cm import visualization.panda.world as wd base = wd.World(cam_pos=[.5, .5, .3], lookat_pos=[0, 0, 0]) gripper_instance = yg.YumiGripper(enable_cc=True, cdmesh_type='aabb') objcm = cm.CollisionModel('../objects/tubebig.stl', cdmesh_type='convex_hull') objcm.attach_to(base) objcm.show_localframe() grasp_info_list = [] for height in [.08, .095]: for roll_angle in [math.pi*.1, math.pi*.2]: gl_hndz = rm.rotmat_from_axangle(np.array([1,0,0]), roll_angle).dot(np.array([0,0,-1])) grasp_info_list += gutil.define_grasp_with_rotation(gripper_instance, objcm, gl_jaw_center_pos=np.array([0, 0, height]), gl_jaw_center_z=gl_hndz, gl_hndx=np.array([1,0,0]), jaw_width=.025, gl_rotation_ax=np.array([0, 0, 1])) for grasp_info in grasp_info_list: jaw_width, gl_jaw_center, pos, rotmat = grasp_info # gic = gripper_s.copy() gripper_instance.fix_to(pos, rotmat) gripper_instance.jaw_to(jaw_width) gripper_instance.gen_meshmodel().attach_to(base) gutil.write_pickle_file(objcm_name='tubebig', grasp_info_list=grasp_info_list) base.run()