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_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 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
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