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,