def get_new_demo_name(verb, items):
verb_data_accessor = multi_item_verbs.VerbDataAccessor()
all_demo_info = verb_data_accessor.get_all_demo_info()
for i in xrange(1000):
demo_name = "%s-%s%i"%(verb, "-".join(items), i)
if demo_name not in all_demo_info:
return demo_name
def make_verb_library_multi(data_dir, verb_lib):
from lfd import multi_item_verbs
link_names = ["r_gripper_tool_frame", "l_gripper_tool_frame"]
verb_data_accessor = multi_item_verbs.VerbDataAccessor()
for (verb_name,
verb_info) in verb_data_accessor.get_all_demo_info().items():
print colorize("processing demo: %s" % verb_name, "red")
for stage_num, stage_name in enumerate(verb_info["stages"]):
bag_file_name = "bags/%s.bag" % (stage_name)
seg_file_name = "images/%s.seg.h5" % (stage_name)
bag = rosbag.Bag(osp.join(data_dir, bag_file_name))
segs = bag_proc.create_segment_without_look(bag, link_names)
if len(segs) > 1: print "warning: more than one segment found"
kinematics_data = segs[0]
stage_data = copy(kinematics_data)
seg_file = h5py.File(osp.join(data_dir, seg_file_name), "r")
bag_proc.dict_to_hdf(verb_lib, stage_data, stage_name)
seg_file.copy("/", verb_lib[stage_name], "object_cloud")
# is the following needed, since it is stored in the yaml file?
verb_lib[stage_name]["arms_used"] = verb_info["arms_used"][
stage_num]
def save_simple_pc(demo_dir, demo_name, stage_num, file_name):
verb_data_accessor = multi_item_verbs.VerbDataAccessor(
test_info_dir="test/multi_item/empty_move_data/%s" % demo_dir)
stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num)
stage_data = verb_data_accessor.get_demo_stage_data(stage_info.stage_name)
stage_pc = stage_data["object_cloud"][stage_info.item]["xyz"]
pc_down = voxel_downsample(stage_pc, .02)
os.chdir(osp.join(osp.dirname(__file__), "test_pcs"))
np.savetxt("%s.pc" % (stage_info.item if file_name is None else file_name),
pc_down)
def get_test_demos():
abs_test_dir = osp.join(osp.dirname(osp.abspath(__file__)), TEST_DATA_DIR)
test_demo_dirs = [entry for entry in os.listdir(abs_test_dir) if osp.isdir(osp.join(abs_test_dir, entry))]
test_demos = {}
for test_demo_dir in test_demo_dirs:
verb_data_accessor = multi_item_verbs.VerbDataAccessor(osp.join(abs_test_dir, test_demo_dir))
all_demo_info = verb_data_accessor.get_all_demo_info()
verb = all_demo_info[all_demo_info.keys()[0]]["verb"]
test_demos[verb] = (test_demo_dir, all_demo_info.keys())
return test_demos
def get_closest_scenes(data_dir, verb):
verb_data_accessor = multi_item_verbs.VerbDataAccessor(
test_info_dir=osp.join("test", BASE_DATA_DIR, data_dir))
demos_for_verb = verb_data_accessor.get_verb_info(verb)
closest = {}
for demo_name, info in demos_for_verb:
exp_clouds = scene_diff.get_clouds_for_demo(verb_data_accessor,
demo_name)
scene_diff_closest_name = scene_diff.get_closest_demo(
verb_data_accessor, verb, exp_clouds, ignore=[demo_name])
closest[demo_name] = scene_diff_closest_name
return closest
def get_closest_single_scene(data_dir, demo_name):
verb_data_accessor = multi_item_verbs.VerbDataAccessor(
test_info_dir=osp.join("test", BASE_DATA_DIR, data_dir))
all_demo_info = verb_data_accessor.get_all_demo_info()
all_demo_names = all_demo_info.keys()
assert demo_name in all_demo_names, "all_demo_names: %s" % (
str(all_demo_names))
verb = verb_data_accessor.get_verb_from_demo_name(demo_name)
exp_clouds = scene_diff.get_clouds_for_demo(verb_data_accessor, demo_name)
scene_diff_closest_name = scene_diff.get_closest_demo(verb_data_accessor,
verb,
exp_clouds,
ignore=[demo_name])
return scene_diff_closest_name
def do_empty_move(demo_name, exp_name, test_dir_name):
empty_move_init()
verb_data_accessor = multi_item_verbs.VerbDataAccessor(test_info_dir=osp.join("test", TEST_DATA_DIR, test_dir_name))
for stage_num in xrange(verb_data_accessor.get_num_stages(demo_name)):
# info and data for previous stage
if stage_num > 0:
prev_demo_info = verb_data_accessor.get_stage_info(demo_name, stage_num-1)
prev_exp_info = verb_data_accessor.get_stage_info(exp_name, stage_num-1)
prev_exp_data = verb_data_accessor.get_demo_stage_data(prev_exp_info.stage_name)
prev_exp_pc = prev_exp_data["object_cloud"][prev_exp_info.item]["xyz"]
else:
# first stage has no previous items
prev_demo_info, prev_exp_info, prev_exp_data, prev_exp_pc = None, None, None, None
# info and data for current stage
cur_demo_info = verb_data_accessor.get_stage_info(demo_name, stage_num)
cur_exp_info = verb_data_accessor.get_stage_info(exp_name, stage_num)
cur_exp_data = verb_data_accessor.get_demo_stage_data(cur_exp_info.stage_name)
cur_exp_pc = cur_exp_data["object_cloud"][cur_exp_info.item]["xyz"]
if stage_num == 0:
world_to_grip_transform_func = None
else:
world_to_grip_transform_func = multi_item_make_verb_traj.make_world_to_grip_transform_tf("%s_gripper_tool_frame" % cur_exp_info.arms_used)
warped_traj_resp = multi_item_make_verb_traj.make_traj_multi_stage_do_work(demo_name, cur_exp_pc, "base_footprint",
stage_num, prev_demo_info, prev_exp_pc,
verb_data_accessor, world_to_grip_transform_func, transform_type="tps_zrot")
yn = yes_or_no("continue?")
if yn:
can_move_lower = (stage_num == 0)
traj = warped_traj_resp.traj
exec_verb_traj.exec_traj_do_work(traj.l_gripper_poses.poses, traj.l_gripper_angles,
traj.r_gripper_poses.poses, traj.r_gripper_angles,
ik_functions.do_traj_ik_graph_search, cur_exp_pc,
cur_demo_info.item, can_move_lower)
if stage_num < verb_data_accessor.get_num_stages(demo_name)-1:
yn = yes_or_no("continue to next stage?")
if not yn:
break
def test_scene_diff(verb, exp_name, correct_closest_name, data_dir):
verb_data_accessor = multi_item_verbs.VerbDataAccessor(
test_info_dir=osp.join("test", TEST_DATA_DIR, data_dir))
exp_clouds = scene_diff.get_clouds_for_demo(verb_data_accessor, exp_name)
scene_diff_closest_name, scene_diff_dists = scene_diff.get_closest_demo(
verb_data_accessor,
verb,
exp_clouds,
ignore=[exp_name],
return_dists=True)
if correct_closest_name == scene_diff_closest_name:
report(True)
else:
if scene_diff_dists.has_key(correct_closest_name):
msg = "Closest demo was %s at distance %f; demo %s had distance %f" % \
(scene_diff_closest_name, scene_diff_dists[scene_diff_closest_name],
correct_closest_name, scene_diff_dists[correct_closest_name])
else:
msg = "Closest demo was %s at distance %f; demo %s was not found" % \
(scene_diff_closest_name, scene_diff_dists[scene_diff_closest_name],
correct_closest_name)
report(False, msg)
def test_translation(demo_name, exp_name, data_dir):
translation_test_init()
verb_data_accessor = multi_item_verbs.VerbDataAccessor(
test_info_dir=osp.join("test", TEST_DATA_DIR, data_dir))
current_stage = 1
# info and data for tool stage
demo_tool_info = verb_data_accessor.get_stage_info(demo_name,
current_stage - 1)
demo_tool_data = verb_data_accessor.get_demo_stage_data(
demo_tool_info.stage_name)
exp_tool_info = verb_data_accessor.get_stage_info(exp_name,
current_stage - 1)
exp_tool_data = verb_data_accessor.get_demo_stage_data(
exp_tool_info.stage_name)
# info and data for target stage
demo_target_info = verb_data_accessor.get_stage_info(
demo_name, current_stage)
demo_target_data = verb_data_accessor.get_demo_stage_data(
demo_target_info.stage_name)
exp_target_info = verb_data_accessor.get_stage_info(
exp_name, current_stage)
exp_target_data = verb_data_accessor.get_demo_stage_data(
exp_target_info.stage_name)
gripper_data_key = "%s_gripper_tool_frame" % demo_target_info.arms_used
# point clouds of tool for demo and experiment
exp_tool_pc = exp_tool_data["object_cloud"][exp_tool_info.item]["xyz"]
exp_target_pc = exp_target_data["object_cloud"][
exp_target_info.item]["xyz"]
# calculate the transformation from the world frame to the gripper frame in the experiment scene
world_to_grip_transform = get_world_to_grip_exp_transform(
exp_tool_data, gripper_data_key)
world_to_grip_transform_func = multi_item_make_verb_traj.make_world_to_grip_transform_hmat(
world_to_grip_transform)
warped_traj_resp = multi_item_make_verb_traj.make_traj_multi_stage_do_work(
demo_name, exp_target_pc, None, current_stage, demo_tool_info,
exp_tool_pc, verb_data_accessor, world_to_grip_transform_func, "tps")
# assuming that the arms_used for the target stage is 'l' or 'r'
if demo_target_info.arms_used == 'l':
warped_grip_traj_mats = [
juc.pose_to_hmat(pose)
for pose in warped_traj_resp.traj.l_gripper_poses.poses
]
elif demo_target_info.arms_used == 'r':
warped_grip_traj_mats = [
juc.pose_to_hmat(pose)
for pose in warped_traj_resp.traj.r_gripper_poses.poses
]
# get the manually measured transformation between the old and new target objects (just a translation for this test)
params = get_test_params()
actual_target_translation = jut.translation_matrix(params["translation"])
# find the expected warped gripper trajectory using the manual translation measurement
demo_spec_pt_traj_mats = get_demo_spec_pt_traj_mats(
demo_target_data, gripper_data_key)
expected_spec_pt_traj = [
np.dot(actual_target_translation, traj_mat)
for traj_mat in demo_spec_pt_traj_mats
]
expected_gripper_traj = get_expected_gripper_traj(
exp_tool_info.special_point, expected_spec_pt_traj)
result = similar_trajectories(expected_gripper_traj, warped_grip_traj_mats)
report(result)
else:
raise ValueError("Invalid 'single' argument")
do_single(demo_base_name, demo_index, stage_num, prev_demo_index, verb_data_accessor, prev_and_cur_pc2)
elif args.stages is not None:
if args.stages[0] == 'a':
demo_name = "%s%s" % (demo_base_name, args.stages[1:])
exp_clouds_pc2 = get_all_clouds_pc2(verb_data_accessor.get_num_stages(demo_name))
closest_demo_name = find_closest_demo(verb_data_accessor.get_verb_from_demo_name(demo_name), exp_clouds_pc2)
print "Closest demo is %s" % (closest_demo_name)
do_multiple_single_demo(demo_name, verb_data_accessor, exp_clouds_pc2)
else:
stages = [int(stage) for stage in args.stages.split(",")]
exp_clouds_pc2 = get_all_clouds_pc2(len(stages))
do_multiple_varied_demos(demo_base_name, stages, verb_data_accessor, exp_clouds_pc2)
if __name__ == "__main__":
if rospy.get_name() == "/unnamed":
rospy.init_node("multi_item_exec_one_demo", disable_signals=True)
do_globals_setup()
args = get_args()
pr2 = exec_verb_traj.Globals.pr2
verb_data_accessor = multi_item_verbs.VerbDataAccessor()
move_pr2_to_start_pos(pr2)
assert args.demo is not None or args.verb is not None
# use 'stages' to use all stages, use 'single' for just one stage
assert (args.stages is not None and args.single is None) or (args.stages is None and args.single is not None)
if args.demo is not None:
run_exp(args, verb_data_accessor)