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)