def test_cup_pour(demo_name, exp_name): test_cup_pour_init() verb_data_accessor = multi_item_verbs.VerbDataAccessor(test=True) current_stage = 1 gripper_data_key = "r_gripper_tool_frame" # info and data for previous stage prev_demo_info = verb_data_accessor.get_stage_info(demo_name, current_stage-1) prev_demo_data = verb_data_accessor.get_demo_data(prev_demo_info.stage_name) prev_exp_info = verb_data_accessor.get_stage_info(exp_name, current_stage-1) prev_exp_data = verb_data_accessor.get_demo_data(prev_exp_info.stage_name) # info and data for current stage cur_demo_info = verb_data_accessor.get_stage_info(demo_name, current_stage) cur_demo_data = verb_data_accessor.get_demo_data(cur_demo_info.stage_name) cur_exp_info = verb_data_accessor.get_stage_info(exp_name, current_stage) cur_exp_data = verb_data_accessor.get_demo_data(cur_exp_info.stage_name) # point clouds of tool for demo and experiment prev_exp_pc = prev_exp_data["object_clouds"][prev_exp_info.item]["xyz"] cur_exp_pc = cur_exp_data["object_clouds"][cur_exp_info.item]["xyz"] # calculate the transformation from the world frame to the gripper frame prev_exp_gripper_pos = prev_exp_data[gripper_data_key]["position"][-1] prev_exp_gripper_orien = prev_exp_data[gripper_data_key]["orientation"][-1] prev_world_to_gripper_trans = np.linalg.inv(juc.trans_rot_to_hmat(prev_exp_gripper_pos, prev_exp_gripper_orien)) gripper_frame_trans = make_verb_traj.make_to_gripper_frame_hmat(prev_world_to_gripper_trans) warped_traj_resp = make_verb_traj.make_traj_multi_stage_do_work(cur_demo_info, [cur_exp_pc], None, current_stage, prev_demo_info, [prev_exp_pc], verb_data_accessor, gripper_frame_trans) # get the actual transformation between the old and new target objects (just a translation for this test) params = get_test_params() translation = params['translation'] actual_target_translation_matrix = jut.translation_matrix(translation) # get the demo special point trajectory cur_demo_gripper_traj_xyzs = cur_demo_data[gripper_data_key]["position"] cur_demo_gripper_traj_oriens = cur_demo_data[gripper_data_key]["orientation"] cur_demo_gripper_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(cur_demo_gripper_traj_xyzs, cur_demo_gripper_traj_oriens)] prev_demo_spec_pt_translation = jut.translation_matrix(np.array(prev_demo_info.special_point)) cur_demo_spec_pt_traj_as_mats = [np.dot(traj_mat, prev_demo_spec_pt_translation) for traj_mat in cur_demo_gripper_traj_mats] # get the expected experiment special point trajectory expected_spec_pt_traj = [np.dot(actual_target_translation_matrix, traj_mat) for traj_mat in cur_demo_spec_pt_traj_as_mats] # get the expected experiment gripper trajectory prev_exp_spec_pt_translation = jut.translation_matrix(np.array(prev_exp_info.special_point)) inv_cur_exp_spec_pt_translation = np.linalg.inv(prev_exp_spec_pt_translation) expected_gripper_traj = [np.dot(traj_mat, inv_cur_exp_spec_pt_translation) for traj_mat in expected_spec_pt_traj] # compare the expected new special point trajectory to the result of make_traj_multi_stage result_traj = warped_traj_resp.traj cur_exp_traj_as_mats = [juc.pose_to_hmat(pose) for pose in result_traj.r_gripper_poses.poses] report(similar_trajectories(expected_gripper_traj, cur_exp_traj_as_mats))
def get_demo_spec_pt_traj_mats(demo_tool_info, demo_target_data, gripper_data_key): demo_grip_traj_xyzs = demo_target_data[gripper_data_key]["position"] demo_grip_traj_oriens = demo_target_data[gripper_data_key]["orientation"] demo_grip_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(demo_grip_traj_xyzs, demo_grip_traj_oriens)] demo_tool_spec_pt_translation = jut.translation_matrix(np.array(demo_tool_info.special_point)) demo_spec_pt_traj_mats = [np.dot(traj_mat, demo_tool_spec_pt_translation) for traj_mat in demo_grip_traj_mats] return demo_spec_pt_traj_mats
def make_kin_tree_from_joint(ps,jointname, ns='default_ns',valuedict=None): rospy.logdebug("joint: %s"%jointname) U = get_pr2_urdf() joint = U.joints[jointname] joint.origin = joint.origin or urdf.Pose() if not joint.origin.position: joint.origin.position = [0,0,0] if not joint.origin.rotation: joint.origin.rotation = [0,0,0] parentFromChild = conversions.trans_rot_to_hmat(joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation)) childFromRotated = np.eye(4) if valuedict and jointname in valuedict: if joint.joint_type == 'revolute' or joint.joint_type == 'continuous': childFromRotated = transformations.rotation_matrix(valuedict[jointname], joint.axis) elif joint.joint_type == 'prismatic': childFromRotated = transformations.translation_matrix(np.array(joint.axis)* valuedict[jointname]) parentFromRotated = np.dot(parentFromChild, childFromRotated) originFromParent = conversions.pose_to_hmat(ps.pose) originFromRotated = np.dot(originFromParent, parentFromRotated) newps = gm.PoseStamped() newps.header = ps.header newps.pose = conversions.hmat_to_pose(originFromRotated) return make_kin_tree_from_link(newps,joint.child,ns=ns,valuedict=valuedict)
def make_kin_tree_from_joint(ps, jointname, ns='default_ns', valuedict=None): rospy.logdebug("joint: %s" % jointname) U = get_pr2_urdf() joint = U.joints[jointname] joint.origin = joint.origin or urdf.Pose() if not joint.origin.position: joint.origin.position = [0, 0, 0] if not joint.origin.rotation: joint.origin.rotation = [0, 0, 0] parentFromChild = conversions.trans_rot_to_hmat( joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation)) childFromRotated = np.eye(4) if valuedict and jointname in valuedict: if joint.joint_type == 'revolute' or joint.joint_type == 'continuous': childFromRotated = transformations.rotation_matrix( valuedict[jointname], joint.axis) elif joint.joint_type == 'prismatic': childFromRotated = transformations.translation_matrix( np.array(joint.axis) * valuedict[jointname]) parentFromRotated = np.dot(parentFromChild, childFromRotated) originFromParent = conversions.pose_to_hmat(ps.pose) originFromRotated = np.dot(originFromParent, parentFromRotated) newps = gm.PoseStamped() newps.header = ps.header newps.pose = conversions.hmat_to_pose(originFromRotated) return make_kin_tree_from_link(newps, joint.child, ns=ns, valuedict=valuedict)
def plot_demo_and_warped_tool_spec_pt(spec_pt_in_grip, tool_stage_data, demo_to_exp_tool_transform, arm): demo_tool_grip_to_world_transform = get_demo_tool_grip_to_world_transform(tool_stage_data, arm) demo_spec_pt_in_world = apply_mat_transform_to_xyz(demo_tool_grip_to_world_transform, spec_pt_in_grip) demo_spec_pt_in_world_frame = jut.translation_matrix(demo_spec_pt_in_world) plot_spec_pts(np.array([demo_spec_pt_in_world]), (1,0,0,1)) warped_spec_pt_in_world_frame = apply_tps_transform_to_hmat(demo_to_exp_tool_transform, demo_spec_pt_in_world_frame) warped_spec_pt_in_world_trans, warped_spec_pt_in_world_rot = juc.hmat_to_trans_rot(warped_spec_pt_in_world_frame) plot_spec_pts(np.array([warped_spec_pt_in_world_trans]), (0,1,0,1))
def get_warped_grip_traj_mats(warped_spec_pt_traj_mats, tool_stage_data, demo_to_exp_tool_transform, spec_pt_in_grip, world_to_grip_transform_func, arm): if demo_to_exp_tool_transform is None: grip_to_spec_pt_transform = np.linalg.inv(jut.translation_matrix(spec_pt_in_grip)) else: demo_tool_grip_to_world_transform = get_demo_tool_grip_to_world_transform(tool_stage_data, arm) demo_spec_pt_in_world = apply_mat_transform_to_xyz(demo_tool_grip_to_world_transform, spec_pt_in_grip) demo_spec_pt_in_world_frame = jut.translation_matrix(demo_spec_pt_in_world) # find the transformation from the new special point to the gripper frame warped_spec_pt_in_world_frame = apply_tps_transform_to_hmat(demo_to_exp_tool_transform, demo_spec_pt_in_world_frame) warped_spec_pt_in_world_trans, warped_spec_pt_in_world_rot = juc.hmat_to_trans_rot(warped_spec_pt_in_world_frame) warped_spec_pt_in_grip_trans = world_to_grip_transform_func(warped_spec_pt_in_world_trans) warped_spec_pt_in_grip_rot = warped_spec_pt_in_world_rot warped_spec_pt_in_grip_frame = juc.trans_rot_to_hmat(warped_spec_pt_in_grip_trans, warped_spec_pt_in_grip_rot) print_hmat_info(warped_spec_pt_in_grip_frame, "warped_spec_pt_in_grip_frame") # transform the warped special point trajectory back to a gripper trajectory in the experiment grip_to_spec_pt_transform = np.linalg.inv(warped_spec_pt_in_grip_frame) warped_grip_traj_mats = [np.dot(spec_pt_mat, grip_to_spec_pt_transform) for spec_pt_mat in warped_spec_pt_traj_mats] return warped_grip_traj_mats
def plot_demo_and_warped_tool_spec_pt(spec_pt_in_grip, tool_stage_data, demo_to_exp_tool_transform, arm): demo_tool_grip_to_world_transform = get_demo_tool_grip_to_world_transform(tool_stage_data, arm) demo_spec_pt_in_world = apply_mat_transform_to_xyz(demo_tool_grip_to_world_transform, spec_pt_in_grip) demo_spec_pt_in_world_frame = jut.translation_matrix(demo_spec_pt_in_world) plot_spec_pts(np.array([demo_spec_pt_in_world]), (1,0,0,0.5)) warped_spec_pt_in_world_frame = apply_tps_transform_to_hmat(demo_to_exp_tool_transform, demo_spec_pt_in_world_frame) warped_spec_pt_in_world_trans, warped_spec_pt_in_world_rot = juc.hmat_to_trans_rot(warped_spec_pt_in_world_frame) plot_spec_pts(np.array([warped_spec_pt_in_world_trans]), (0,1,0,0.5))
def get_expected_gripper_traj(special_point, expected_spec_pt_traj): exp_tool_spec_pt_translation = jut.translation_matrix( np.array(exp_tool_info.special_point)) inv_exp_tool_spec_pt_translation = np.linalg.inv( exp_tool_spec_pt_translation) expected_gripper_traj = [ np.dot(traj_mat, inv_exp_tool_spec_pt_translation) for traj_mat in expected_spec_pt_traj ] return expected_gripper_traj
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 get_demo_spec_pt_traj_mats(demo_target_data, gripper_data_key): demo_grip_traj_xyzs = demo_target_data[gripper_data_key]["position"] demo_grip_traj_oriens = demo_target_data[gripper_data_key]["orientation"] demo_grip_traj_mats = [ juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(demo_grip_traj_xyzs, demo_grip_traj_oriens) ] demo_tool_spec_pt_translation = jut.translation_matrix( np.array(demo_tool_info.special_point)) demo_spec_pt_traj_mats = [ np.dot(traj_mat, demo_tool_spec_pt_translation) for traj_mat in demo_grip_traj_mats ] return demo_spec_pt_traj_mats
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)
def get_expected_gripper_traj(special_point, expected_spec_pt_traj): exp_tool_spec_pt_translation = jut.translation_matrix(np.array(special_point)) inv_exp_tool_spec_pt_translation = np.linalg.inv(exp_tool_spec_pt_translation) expected_gripper_traj = [np.dot(traj_mat, inv_exp_tool_spec_pt_translation) for traj_mat in expected_spec_pt_traj] return expected_gripper_traj
def get_demo_spec_pt_traj_mats(demo_target_grip_traj_mats, spec_pt_in_grip): if np.all(spec_pt_in_grip == np.zeros(3)): return demo_target_grip_traj_mats grip_to_spec_pt_trans = jut.translation_matrix(spec_pt_in_grip) demo_spec_pt_traj_mats = [np.dot(gripper_mat, grip_to_spec_pt_trans) for gripper_mat in demo_target_grip_traj_mats] return demo_spec_pt_traj_mats
def make_traj_multi_stage_do_work(current_stage_info, cur_exp_clouds, clouds_frame_id, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor, to_gripper_frame_func=None): arms_used = current_stage_info.arms_used verb_stage_data = verb_data_accessor.get_demo_data(current_stage_info.stage_name) if stage_num == 0: # don't do any extra transformation for the first stage prev_demo_to_exp_grip_transform_lin_rigid = np.identity(4) # no special point translation for first stage since no tool yet special_point_translation = np.identity(4) elif stage_num > 0: # make sure that the tool stage only uses one arm (the one with the tool) assert arms_used in ['r', 'l'] prev_stage_data = verb_data_accessor.get_demo_data(prev_stage_info.stage_name) prev_demo_pc = prev_stage_data["object_clouds"][prev_stage_info.item]["xyz"] prev_exp_pc = prev_exp_clouds[0] prev_demo_pc_down = voxel_downsample(prev_demo_pc, .02) prev_exp_pc_down = voxel_downsample(prev_exp_pc, .02) gripper_data_key = "%s_gripper_tool_frame" % (arms_used) # transform point cloud in base frame to gripper frame # assume right hand has the tool for now # use the last pose of the gripper in the stage to figure out the point cloud of the tool in the gripper frame when the tool was grabbed prev_demo_gripper_pos = prev_stage_data[gripper_data_key]["position"][-1] prev_demo_gripper_orien = prev_stage_data[gripper_data_key]["orientation"][-1] prev_demo_gripper_to_base_transform = juc.trans_rot_to_hmat(prev_demo_gripper_pos, prev_demo_gripper_orien) prev_demo_base_to_gripper_transform = np.linalg.inv(prev_demo_gripper_to_base_transform) prev_demo_pc_in_gripper_frame = np.array([apply_transform(prev_demo_base_to_gripper_transform, point) for point in prev_demo_pc_down]) # get the new point cloud in the new gripper frame # prev_exp_pc_in_gripper_frame = [apply_transform(prev_exp_base_to_gripper_transform, point) for point in prev_exp_pc_down] if to_gripper_frame_func is None: prev_exp_pc_in_gripper_frame = to_gripper_frame_tf_listener(prev_exp_pc_down, gripper_data_key) else: prev_exp_pc_in_gripper_frame = to_gripper_frame_func(prev_exp_pc_down, gripper_data_key) # get the transformation from the new point cloud to the old point cloud for the previous stage prev_demo_to_exp_grip_transform = get_tps_transform(prev_demo_pc_in_gripper_frame, prev_exp_pc_in_gripper_frame) # transforms gripper trajectory point into special point trajectory point if prev_stage_info.special_point is None: # if there is no special point, linearize at origin prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.zeros(3)) # don't do a special point translation if there is no specified special point special_point_translation = np.identity(4) else: prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.array(prev_stage_info.special_point)) # translation from gripper pose in world frame to special point pose in world frame special_point_translation = jut.translation_matrix(np.array(prev_stage_info.special_point)) if arms_used != 'b': arms_used_list = [arms_used] else: arms_used_list = ['r', 'l'] warped_stage_data = group_to_dict(verb_stage_data) # deep copy it resp = MakeTrajectoryResponse() traj = resp.traj traj.arms_used = arms_used for arm in arms_used_list: gripper_data_key = "%s_gripper_tool_frame" % (arm) # find the special point trajectory before the target transformation cur_demo_gripper_traj_xyzs = verb_stage_data[gripper_data_key]["position"] cur_demo_gripper_traj_oriens = verb_stage_data[gripper_data_key]["orientation"] cur_demo_gripper_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(cur_demo_gripper_traj_xyzs, cur_demo_gripper_traj_oriens)] cur_mid_spec_pt_traj_mats = [np.dot(gripper_mat, special_point_translation) for gripper_mat in cur_demo_gripper_traj_mats] # find the transformation from the new special point to the gripper frame cur_exp_inv_special_point_transformation = np.linalg.inv(np.dot(prev_demo_to_exp_grip_transform_lin_rigid, special_point_translation)) # save the demo special point traj for plotting plot_spec_pt_traj = [] for gripper_mat in cur_demo_gripper_traj_mats: spec_pt_xyz, spec_pt_orien = juc.hmat_to_trans_rot(np.dot(gripper_mat, special_point_translation)) plot_spec_pt_traj.append(spec_pt_xyz) print 'grip transform:' print prev_demo_to_exp_grip_transform_lin_rigid print 'special point translation:' print special_point_translation print 'inverse special point translation:' print cur_exp_inv_special_point_transformation # find the target transformation for the experiment scene demo_object_clouds = [verb_stage_data["object_clouds"][obj_name]["xyz"] for obj_name in verb_stage_data["object_clouds"].keys()] if len(demo_object_clouds) > 1: raise Exception("i don't know what to do with multiple object clouds") x_nd = voxel_downsample(demo_object_clouds[0], .02) y_md = voxel_downsample(cur_exp_clouds[0], .02) # transformation from old target object to new target object in world frame cur_demo_to_exp_transform = get_tps_transform(x_nd, y_md) # apply the target warping transformation to the special point trajectory cur_mid_spec_pt_traj_xyzs, cur_mid_spec_pt_traj_oriens = [], [] for cur_mid_spec_pt_traj_mat in cur_mid_spec_pt_traj_mats: cur_mid_spec_pt_traj_xyz, cur_mid_spec_pt_traj_orien = juc.hmat_to_trans_rot(cur_mid_spec_pt_traj_mat) cur_mid_spec_pt_traj_xyzs.append(cur_mid_spec_pt_traj_xyz) cur_mid_spec_pt_traj_oriens.append(juc.quat2mat(cur_mid_spec_pt_traj_orien)) cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens = cur_demo_to_exp_transform.transform_frames(np.array(cur_mid_spec_pt_traj_xyzs), np.array(cur_mid_spec_pt_traj_oriens)) plot_warped_spec_pt_traj = cur_exp_spec_pt_traj_xyzs #save the special point traj for plotting cur_exp_spec_pt_traj_mats = [juc.trans_rot_to_hmat(cur_exp_spec_pt_traj_xyz, mat2quat(cur_exp_spec_pt_traj_orien)) for cur_exp_spec_pt_traj_xyz, cur_exp_spec_pt_traj_orien in zip(cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens)] # transform the warped special point trajectory back to a gripper trajectory in the experiment cur_exp_gripper_traj_mats = [np.dot(spec_pt_mat, cur_exp_inv_special_point_transformation) for spec_pt_mat in cur_exp_spec_pt_traj_mats] warped_stage_data[gripper_data_key]["position"] = [] warped_stage_data[gripper_data_key]["orientation"] = [] for exp_traj_mat in cur_exp_gripper_traj_mats: warped_pos, warped_orien = juc.hmat_to_trans_rot(exp_traj_mat) warped_stage_data[gripper_data_key]["position"].append(warped_pos) warped_stage_data[gripper_data_key]["orientation"].append(warped_orien) if arm == 'r': traj.r_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"]) print "poses: ", len(traj.r_gripper_poses.poses) traj.r_gripper_angles = warped_stage_data["r_gripper_joint"] traj.r_gripper_poses.header.frame_id = clouds_frame_id elif arm == 'l': traj.l_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"]) print "poses: ", len(traj.l_gripper_poses.poses) traj.l_gripper_angles = warped_stage_data["l_gripper_joint"] traj.l_gripper_poses.header.frame_id = clouds_frame_id Globals.handles = [] plot_original_and_warped_demo_and_spec_pt(verb_stage_data, warped_stage_data, plot_spec_pt_traj, plot_warped_spec_pt_traj, traj) pose_array = conversions.array_to_pose_array(y_md, 'base_footprint') Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,0,1,1),width=.01,type=Marker.CUBE_LIST)) return resp