def open_database(self): """ Open a database """ if self.database is not None: self.database.close() # get user input invalid_db = True while invalid_db: database_name = raw_input('Enter database name: ') tokens = database_name.split() if len(tokens) > 1: print 'Please provide only a single database name' if database_name.lower() == 'q': return True # create new db database_root, database_ext = os.path.splitext(database_name) if database_ext == '': database_name = database_name + db.HDF5_EXT database_root, database_ext = os.path.splitext(database_name) if database_ext != db.HDF5_EXT: print 'Database must have extension %s' %(db.HDF5_EXT) # create new db if asked if not os.path.exists(database_name): print 'Database %s does not exist' %(database_name) create_new = raw_input('Create new db? [y/n] ') while create_new.lower() != 'n' and create_new.lower() != 'y': print 'Did not understand input. Please answer \'y\' or \'n\'' create_new = raw_input('Create new db? [y/n] ') if create_new.lower() == 'n': print 'Aborting database creation' return True # open db self.database = db.Hdf5Database(database_name, access_level=db.READ_WRITE_ACCESS) print 'Opened database %s' %(database_name) print invalid_db = False return True
if __name__ == '__main__': # parse args parser = argparse.ArgumentParser() parser.add_argument('output_path') args = parser.parse_args() # open config cfg_file = 'cfg/examples/tabletop_registration.yaml' cfg = ec.ExperimentConfig(cfg_file) logging.getLogger().setLevel(logging.INFO) object_key = cfg['object_key'] stp_id = cfg['stp_id'] # open db db_filename = os.path.join(cfg['database_dir'], cfg['database_name']) database = db.Hdf5Database(db_filename, cfg) dataset = database.datasets[0] obj = dataset[object_key] stable_pose = dataset.stable_pose(object_key, stp_id) # get images depth_images, ir_intrinsics = load_depth_images(cfg) depth_im = Image.median_images(depth_images) point_cloud_cam = ir_intrinsics.deproject(depth_im) # for debug only T_camera_world = RigidTransform.load( os.path.join(cfg['calib_dir'], '%s_registration.tf' % (ir_intrinsics.frame))) point_cloud_world = T_camera_world * point_cloud_cam # create registration solver registration_solver = KnownObjectStablePoseTabletopRegistrationSolver(
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2 x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2 y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2 z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2 return w, x, y, z if __name__ == '__main__': config_filename = 'cfg/examples/read_grasps.yaml' logging.getLogger().setLevel(logging.INFO) # read config file and open database config = ec.ExperimentConfig(config_filename) database_filename = os.path.join(config['database_dir'], config['database_name']) database = db.Hdf5Database(database_filename, access_level=db.READ_ONLY_ACCESS) # read gripper gripper = gr.RobotGripper.load(config['gripper_name']) # loop through available datasets dataset_name = config['dataset'] dataset = database.dataset(dataset_name) # loop through objects in dataset object_key = config['object_name'] obj = dataset[object_key] logging.info('Reading grasps for object {}'.format(object_key)) dataset.obj_mesh_filename("WizRook", output_dir="data/meshes/chess_pieces/")
def get_rook_grasp(self, position, quaternion): config_filename = self.dexnet_path + '/cfg/examples/read_grasps.yaml' logging.getLogger().setLevel(logging.INFO) # read config file and open database config = ec.ExperimentConfig(config_filename) database_filename = os.path.join(config['database_dir'], config['database_name']) database = db.Hdf5Database(database_filename, access_level=db.READ_ONLY_ACCESS) # read gripper gripper = gr.RobotGripper.load(config['gripper_name'], self.dexnet_path + "/data/grippers") # loop through available datasets dataset_name = config['dataset'] dataset = database.dataset(dataset_name) # loop through objects in dataset object_key = config['object_name'] obj = dataset[object_key] logging.info('Reading grasps for object {}'.format(object_key)) # dataset.obj_mesh_filename("WizRook", output_dir="data/meshes/chess_pieces/") # read grasps and grasp metadata grasps = dataset.grasps(object_key, gripper=gripper.name) grasp_features = dataset.grasp_features(object_key, grasps, gripper=gripper.name) grasp_metrics = dataset.grasp_metrics(object_key, grasps, gripper=gripper.name) # read in object stable poses stable_pose = dataset.stable_pose(object_key, config['stable_pose_id']) #65 # 78 grasp = grasps[78] rot = quat_to_rot_mat(quaternion) arr = np.array([[-1, 0, 0, position[0]], [0, -1, 0, position[1]], [0, 0, 1, position[2]], [0, 0, 0, 1]]) for i in range(3): for j in range(3): arr[i, j] = rot[i, j] T_world_obj = stf.SimilarityTransform3D(from_frame='obj', to_frame='world', pose=tfx.pose(arr)) # for displaying the fake gripper CAD file in the scene (for visualization purposes) T_mesh_gripper = stf.SimilarityTransform3D.load( self.dexnet_path + '/data/grippers/yumi/T_mesh_gripper.stf') T_obj_gripper = grasp.gripper_pose(gripper) T_gripper_mesh = T_mesh_gripper.inverse() T_obj_mesh = T_obj_gripper.dot(T_gripper_mesh) T_world_mesh = T_world_obj.dot(T_obj_mesh) # for actual planning # this file is only for use with the yumi_kinematics ik solver T_tip_gripper = stf.SimilarityTransform3D.load( self.dexnet_path + '/data/grippers/yumi/T_tip_gripper.stf') T_gripper_tip = T_tip_gripper.inverse() T_obj_tip = T_obj_gripper.dot(T_gripper_tip) T_world_tip = T_world_obj.dot(T_obj_tip) return T_world_mesh, T_world_tip
return False return rendered_images if __name__ == '__main__': config_filename = sys.argv[1] config = ec.ExperimentConfig(config_filename) render_mode = config['maya']['render_mode'] save_images = config['maya']['save_images'] renderer = MayaRenderer(config) database_name = os.path.join(config['database_dir'], config['database_name']) database = db.Hdf5Database(database_name, config, access_level=db.READ_WRITE_ACCESS) for dataset_name in config['datasets'].keys(): dataset = database.dataset(dataset_name) for obj in dataset: stable_poses = dataset.stable_poses(obj.key) for i, stable_pose in enumerate(stable_poses): if stable_pose.p > config['maya']['min_prob']: if dataset.has_rendered_images( obj.key, stable_pose_id=stable_pose.id, image_type=render_mode): dataset.delete_rendered_images( obj.key, stable_pose_id=stable_pose.id,