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_tool_info, 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)
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_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)
