def transform_demo(reg, demo, left=True, right=True, cloud_xyz=False, object_clouds=False, l_offset = None, r_offset = None):
"""
reg: NonrigidRegistration object
demo: array with the following fields: r_gripper_xyzs, l_gripper_xyzs, r_gripper_quats, l_gripper_quats, cloud_xyz
(todo: replace 'cloud_xyz' with 'object_points')
"""
warped_demo = group_to_dict(demo)
if left:
if l_offset is None:
l_tool_xyzs = demo["l_gripper_tool_frame"]["position"]
else:
l_tool_xyzs = np.array([xyz + np.dot(rot[:3,:3], l_offset) for (xyz, rot) in zip(demo["l_gripper_tool_frame"]["position"], quats2mats(demo["l_gripper_tool_frame"]["orientation"]))])
l_tool_xyzs_warped, rot_l_warped = reg.transform_frames(l_tool_xyzs, quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
l_gripper_quats_warped = mats2quats(rot_l_warped)
if l_offset is None:
l_gripper_xyzs_warped = l_tool_xyzs_warped
else:
l_gripper_xyzs_warped = np.array([xyz - np.dot(rot[:3,:3], l_offset) for (xyz, rot) in zip(l_tool_xyzs_warped, rot_l_warped)])
warped_demo["l_gripper_tool_frame"]["position"] = l_gripper_xyzs_warped
warped_demo["l_gripper_tool_frame"]["orientation"] = l_gripper_quats_warped
if right:
if r_offset is None:
r_tool_xyzs = demo["r_gripper_tool_frame"]["position"]
else:
r_tool_xyzs = np.array([xyz + np.dot(rot[:3,:3], r_offset) for (xyz, rot) in zip(demo["r_gripper_tool_frame"]["position"], quats2mats(demo["r_gripper_tool_frame"]["orientation"]))])
r_tool_xyzs_warped, rot_r_warped = reg.transform_frames(r_tool_xyzs, quats2mats(demo["r_gripper_tool_frame"]["orientation"]))
r_gripper_quats_warped = mats2quats(rot_r_warped)
if r_offset is None:
r_gripper_xyzs_warped = r_tool_xyzs_warped
else:
r_gripper_xyzs_warped = np.array([xyz - np.dot(rot[:3,:3], r_offset) for (xyz, rot) in zip(r_tool_xyzs_warped, rot_r_warped)])
warped_demo["r_gripper_tool_frame"]["position"] = r_gripper_xyzs_warped
warped_demo["r_gripper_tool_frame"]["orientation"] = r_gripper_quats_warped
if cloud_xyz:
old_cloud_xyz_pts = np.asarray(demo["cloud_xyz"]).reshape(-1,3)
new_cloud_xyz_pts = reg.transform_points(old_cloud_xyz_pts)
warped_demo["cloud_xyz"] = new_cloud_xyz_pts.reshape(demo["cloud_xyz"].shape)
if object_clouds:
for key in sorted(demo["object_clouds"].keys()):
old_cloud_xyz_pts = np.asarray(demo["object_clouds"][key]["xyz"]).reshape(-1,3)
new_cloud_xyz_pts = reg.transform_points(old_cloud_xyz_pts)
warped_demo["object_clouds"][key]["xyz"] = new_cloud_xyz_pts
return warped_demo
def transform_verb_demo(warp, demo,l_offset = None, r_offset = None):
"""
demo: array with the following fields: r_gripper_xyzs, l_gripper_xyzs, r_gripper_quats, l_gripper_quats, cloud_xyz, possible object_clouds
transform each field using warp
l_offset and r_offset tell you where the tools are, so you can do tool stuff
(note: this doesn't currently work when important point is moved relative to demo)
"""
warped_demo = group_to_dict(demo) # deep copy it
if demo["arms_used"] in "lb":
if l_offset is None:
l_tool_xyzs = demo["l_gripper_tool_frame"]["position"]
else:
l_tool_xyzs = np.array([xyz + np.dot(rot[:3,:3], l_offset) for (xyz, rot) in zip(demo["l_gripper_tool_frame"]["position"], quats2mats(demo["l_gripper_tool_frame"]["orientation"]))])
l_tool_xyzs_warped, rot_l_warped = warp.transform_frames(l_tool_xyzs, quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
l_gripper_quats_warped = mats2quats(rot_l_warped)
if l_offset is None:
l_gripper_xyzs_warped = l_tool_xyzs_warped
else:
l_gripper_xyzs_warped = np.array([xyz - np.dot(rot[:3,:3], l_offset) for (xyz, rot) in zip(l_tool_xyzs_warped, rot_l_warped)])
warped_demo["l_gripper_tool_frame"]["position"] = l_gripper_xyzs_warped
warped_demo["l_gripper_tool_frame"]["orientation"] = l_gripper_quats_warped
if demo["arms_used"] in "rb":
if r_offset is None:
r_tool_xyzs = demo["r_gripper_tool_frame"]["position"]
else:
r_tool_xyzs = np.array([xyz + np.dot(rot[:3,:3], r_offset) for (xyz, rot) in zip(demo["r_gripper_tool_frame"]["position"], quats2mats(demo["r_gripper_tool_frame"]["orientation"]))])
r_tool_xyzs_warped, rot_r_warped = warp.transform_frames(r_tool_xyzs, quats2mats(demo["r_gripper_tool_frame"]["orientation"]))
r_gripper_quats_warped = mats2quats(rot_r_warped)
if r_offset is None:
r_gripper_xyzs_warped = r_tool_xyzs_warped
else:
r_gripper_xyzs_warped = np.array([xyz - np.dot(rot[:3,:3], r_offset) for (xyz, rot) in zip(r_tool_xyzs_warped, rot_r_warped)])
warped_demo["r_gripper_tool_frame"]["position"] = r_gripper_xyzs_warped
warped_demo["r_gripper_tool_frame"]["orientation"] = r_gripper_quats_warped
if "object_clouds" in demo:
for key in sorted(demo["object_clouds"].keys()):
old_cloud_xyz_pts = np.asarray(demo["object_clouds"][key]["xyz"]).reshape(-1,3)
new_cloud_xyz_pts = warp.transform_points(old_cloud_xyz_pts)
warped_demo["object_clouds"][key]["xyz"] = new_cloud_xyz_pts
return warped_demo
def get_demo_data(demo_name):
h5file = h5py.File(h5path, "r")
out = group_to_dict(h5file[demo_name])
h5file.close()
return out
def get_demo_data(demo_name):
h5file = h5py.File(h5path, "r")
out = group_to_dict(h5file[demo_name])
h5file.close()
return out
def make_traj_multi_stage_do_work(demo_name, exp_target_cloud, frame_id, stage_num, tool_stage_info, exp_tool_cloud, verb_data_accessor, world_to_grip_transform_func, transform_type):
current_stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num)
arms_used = current_stage_info.arms_used
# make sure this is the first stage (no tool) or only one arm is being used with a tool
assert stage_num == 0 or (arms_used in ['r', 'l'])
if stage_num == 0:
tool_stage_data = None
# don't do any extra transformation for the first stage
demo_to_exp_tool_transform = None
# no special point translation for first stage since no tool yet
spec_pt_in_grip = np.zeros(3)
else:
tool_stage_data = verb_data_accessor.get_demo_stage_data(tool_stage_info.stage_name)
# make sure that the tool stage only uses one arm (the one with the tool)
demo_to_exp_tool_transform = get_demo_to_exp_tool_transform(verb_data_accessor, tool_stage_info,
exp_tool_cloud, transform_type)
spec_pt_in_grip = np.zeros(3) if tool_stage_info.special_point is None else tool_stage_info.special_point
current_stage_data = verb_data_accessor.get_demo_stage_data(current_stage_info.stage_name)
resp = MakeTrajectoryResponse()
resp.traj.arms_used = arms_used
# find the target transformation for the experiment scene
demo_to_exp_target_transform = get_demo_to_exp_target_transform(current_stage_data["object_cloud"][current_stage_info.item]["xyz"],
exp_target_cloud,
transform_type)
warped_stage_data = group_to_dict(current_stage_data) # deep copy it
arms_used_list = ['r', 'l'] if arms_used == 'b' else [arms_used]
for arm in arms_used_list:
gripper_data_key = "%s_gripper_tool_frame" % (arm)
# get the demo gripper trajectory
demo_target_grip_traj_mats = get_demo_grip_traj_mats(current_stage_data, arm)
# get the demo special point trajectory by applying the special point translation
demo_spec_pt_traj_mats = get_demo_spec_pt_traj_mats(demo_target_grip_traj_mats, spec_pt_in_grip)
# get the warped special point trajectory by applying the target warping transformation to the demo special point trajectory
warped_spec_pt_traj_mats = get_warped_spec_pt_traj_mats(demo_spec_pt_traj_mats, demo_to_exp_target_transform)
# get the warped trajectory for the gripper using the tool warping transformation
warped_grip_traj_mats = 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)
warped_transs, warped_rots = juc.hmats_to_transs_rots(warped_grip_traj_mats)
warped_stage_data[gripper_data_key]["position"] = warped_transs
warped_stage_data[gripper_data_key]["orientation"] = warped_rots
set_traj_fields_for_response(warped_stage_data, resp, arm, frame_id)
# save the demo special point traj for plotting
demo_spec_pt_xyzs, exp_spec_pt_xyzs = [], []
if stage_num > 0:
demo_spec_pt_xyzs = juc.hmats_to_transs_rots(demo_spec_pt_traj_mats)[0]
exp_spec_pt_xyzs = juc.hmats_to_transs_rots(warped_spec_pt_traj_mats)[0]
del Globals.handles[:]
# plot the demo and warped special points
current_spec_pt = current_stage_info.special_point
# currently, don't know which arm grabbed the tool if both arms were used in a stage
if stage_num == 0 and current_spec_pt is not None and arms_used in ['l', 'r']:
plot_demo_and_warped_tool_spec_pt(current_spec_pt, current_stage_data, demo_to_exp_target_transform, arms_used)
# plot the gripper and special point trajectories (red is demo, green is warped)
#plot_original_and_warped_demo_and_spec_pt(current_stage_data, warped_stage_data,
# demo_spec_pt_xyzs, exp_spec_pt_xyzs,
# arms_used)
return resp
def transform_demo(reg,
demo,
left=True,
right=True,
cloud_xyz=False,
object_clouds=False,
l_offset=None,
r_offset=None):
"""
reg: NonrigidRegistration object
demo: array with the following fields: r_gripper_xyzs, l_gripper_xyzs, r_gripper_quats, l_gripper_quats, cloud_xyz
(todo: replace 'cloud_xyz' with 'object_points')
"""
warped_demo = group_to_dict(demo)
if left:
if l_offset is None:
l_tool_xyzs = demo["l_gripper_tool_frame"]["position"]
else:
l_tool_xyzs = np.array([
xyz + np.dot(rot[:3, :3], l_offset) for (xyz, rot) in zip(
demo["l_gripper_tool_frame"]["position"],
quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
])
l_tool_xyzs_warped, rot_l_warped = reg.transform_frames(
l_tool_xyzs,
quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
l_gripper_quats_warped = mats2quats(rot_l_warped)
if l_offset is None:
l_gripper_xyzs_warped = l_tool_xyzs_warped
else:
l_gripper_xyzs_warped = np.array([
xyz - np.dot(rot[:3, :3], l_offset)
for (xyz, rot) in zip(l_tool_xyzs_warped, rot_l_warped)
])
warped_demo["l_gripper_tool_frame"]["position"] = l_gripper_xyzs_warped
warped_demo["l_gripper_tool_frame"][
"orientation"] = l_gripper_quats_warped
if right:
if r_offset is None:
r_tool_xyzs = demo["r_gripper_tool_frame"]["position"]
else:
r_tool_xyzs = np.array([
xyz + np.dot(rot[:3, :3], r_offset) for (xyz, rot) in zip(
demo["r_gripper_tool_frame"]["position"],
quats2mats(demo["r_gripper_tool_frame"]["orientation"]))
])
r_tool_xyzs_warped, rot_r_warped = reg.transform_frames(
r_tool_xyzs,
quats2mats(demo["r_gripper_tool_frame"]["orientation"]))
r_gripper_quats_warped = mats2quats(rot_r_warped)
if r_offset is None:
r_gripper_xyzs_warped = r_tool_xyzs_warped
else:
r_gripper_xyzs_warped = np.array([
xyz - np.dot(rot[:3, :3], r_offset)
for (xyz, rot) in zip(r_tool_xyzs_warped, rot_r_warped)
])
warped_demo["r_gripper_tool_frame"]["position"] = r_gripper_xyzs_warped
warped_demo["r_gripper_tool_frame"][
"orientation"] = r_gripper_quats_warped
if cloud_xyz:
old_cloud_xyz_pts = np.asarray(demo["cloud_xyz"]).reshape(-1, 3)
new_cloud_xyz_pts = reg.transform_points(old_cloud_xyz_pts)
warped_demo["cloud_xyz"] = new_cloud_xyz_pts.reshape(
demo["cloud_xyz"].shape)
if object_clouds:
for key in sorted(demo["object_clouds"].keys()):
old_cloud_xyz_pts = np.asarray(
demo["object_clouds"][key]["xyz"]).reshape(-1, 3)
new_cloud_xyz_pts = reg.transform_points(old_cloud_xyz_pts)
warped_demo["object_clouds"][key]["xyz"] = new_cloud_xyz_pts
return warped_demo
def transform_verb_demo(warp, demo, l_offset=None, r_offset=None):
"""
demo: array with the following fields: r_gripper_xyzs, l_gripper_xyzs, r_gripper_quats, l_gripper_quats, cloud_xyz, possible object_clouds
transform each field using warp
l_offset and r_offset tell you where the tools are, so you can do tool stuff
(note: this doesn't currently work when important point is moved relative to demo)
"""
warped_demo = group_to_dict(demo) # deep copy it
if demo["arms_used"] in "lb":
if l_offset is None:
l_tool_xyzs = demo["l_gripper_tool_frame"]["position"]
else:
l_tool_xyzs = np.array([
xyz + np.dot(rot[:3, :3], l_offset) for (xyz, rot) in zip(
demo["l_gripper_tool_frame"]["position"],
quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
])
l_tool_xyzs_warped, rot_l_warped = warp.transform_frames(
l_tool_xyzs,
quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
l_gripper_quats_warped = mats2quats(rot_l_warped)
if l_offset is None:
l_gripper_xyzs_warped = l_tool_xyzs_warped
else:
l_gripper_xyzs_warped = np.array([
xyz - np.dot(rot[:3, :3], l_offset)
for (xyz, rot) in zip(l_tool_xyzs_warped, rot_l_warped)
])
warped_demo["l_gripper_tool_frame"]["position"] = l_gripper_xyzs_warped
warped_demo["l_gripper_tool_frame"][
"orientation"] = l_gripper_quats_warped
if demo["arms_used"] in "rb":
if r_offset is None:
r_tool_xyzs = demo["r_gripper_tool_frame"]["position"]
else:
r_tool_xyzs = np.array([
xyz + np.dot(rot[:3, :3], r_offset) for (xyz, rot) in zip(
demo["r_gripper_tool_frame"]["position"],
quats2mats(demo["r_gripper_tool_frame"]["orientation"]))
])
r_tool_xyzs_warped, rot_r_warped = warp.transform_frames(
r_tool_xyzs,
quats2mats(demo["r_gripper_tool_frame"]["orientation"]))
r_gripper_quats_warped = mats2quats(rot_r_warped)
if r_offset is None:
r_gripper_xyzs_warped = r_tool_xyzs_warped
else:
r_gripper_xyzs_warped = np.array([
xyz - np.dot(rot[:3, :3], r_offset)
for (xyz, rot) in zip(r_tool_xyzs_warped, rot_r_warped)
])
warped_demo["r_gripper_tool_frame"]["position"] = r_gripper_xyzs_warped
warped_demo["r_gripper_tool_frame"][
"orientation"] = r_gripper_quats_warped
if "object_clouds" in demo:
for key in sorted(demo["object_clouds"].keys()):
old_cloud_xyz_pts = np.asarray(
demo["object_clouds"][key]["xyz"]).reshape(-1, 3)
new_cloud_xyz_pts = warp.transform_points(old_cloud_xyz_pts)
warped_demo["object_clouds"][key]["xyz"] = new_cloud_xyz_pts
return warped_demo
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_stage_data(self, stage_name):
h5file = h5py.File(self.h5path, "r")
out = group_to_dict(h5file[stage_name])
h5file.close()
return out
def make_traj_multi_stage_do_work(demo_name, exp_target_cloud, frame_id, stage_num, tool_stage_info, exp_tool_cloud, verb_data_accessor, world_to_grip_transform_func, transform_type):
current_stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num)
arms_used = current_stage_info.arms_used
# make sure this is the first stage (no tool) or only one arm is being used with a tool
assert stage_num == 0 or (arms_used in ['r', 'l'])
if stage_num == 0:
tool_stage_data = None
# don't do any extra transformation for the first stage
demo_to_exp_tool_transform = None
# no special point translation for first stage since no tool yet
spec_pt_in_grip = np.zeros(3)
else:
tool_stage_data = verb_data_accessor.get_demo_stage_data(tool_stage_info.stage_name)
# make sure that the tool stage only uses one arm (the one with the tool)
demo_to_exp_tool_transform = get_demo_to_exp_tool_transform(verb_data_accessor, tool_stage_info,
exp_tool_cloud, transform_type)
spec_pt_in_grip = np.zeros(3) if tool_stage_info.special_point is None else tool_stage_info.special_point
current_stage_data = verb_data_accessor.get_demo_stage_data(current_stage_info.stage_name)
resp = MakeTrajectoryResponse()
resp.traj.arms_used = arms_used
# find the target transformation for the experiment scene
demo_to_exp_target_transform = get_demo_to_exp_target_transform(current_stage_data["object_cloud"][current_stage_info.item]["xyz"],
exp_target_cloud,
transform_type)
warped_stage_data = group_to_dict(current_stage_data) # deep copy it
arms_used_list = ['r', 'l'] if arms_used == 'b' else [arms_used]
for arm in arms_used_list:
gripper_data_key = "%s_gripper_tool_frame" % (arm)
# get the demo gripper trajectory
demo_target_grip_traj_mats = get_demo_grip_traj_mats(current_stage_data, arm)
# get the demo special point trajectory by applying the special point translation
demo_spec_pt_traj_mats = get_demo_spec_pt_traj_mats(demo_target_grip_traj_mats, spec_pt_in_grip)
# get the warped special point trajectory by applying the target warping transformation to the demo special point trajectory
warped_spec_pt_traj_mats = get_warped_spec_pt_traj_mats(demo_spec_pt_traj_mats, demo_to_exp_target_transform)
# get the warped trajectory for the gripper using the tool warping transformation
warped_grip_traj_mats = 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)
warped_transs, warped_rots = juc.hmats_to_transs_rots(warped_grip_traj_mats)
warped_stage_data[gripper_data_key]["position"] = warped_transs
warped_stage_data[gripper_data_key]["orientation"] = warped_rots
set_traj_fields_for_response(warped_stage_data, resp, arm, frame_id)
# save the demo special point traj for plotting
demo_spec_pt_xyzs, exp_spec_pt_xyzs = [], []
if stage_num > 0:
demo_spec_pt_xyzs = juc.hmats_to_transs_rots(demo_spec_pt_traj_mats)[0]
exp_spec_pt_xyzs = juc.hmats_to_transs_rots(warped_spec_pt_traj_mats)[0]
del Globals.handles[:]
# plot the demo and warped special points
current_spec_pt = current_stage_info.special_point
# currently, don't know which arm grabbed the tool if both arms were used in a stage
if stage_num == 0 and current_spec_pt is not None and arms_used in ['l', 'r']:
plot_demo_and_warped_tool_spec_pt(current_spec_pt, current_stage_data, demo_to_exp_target_transform, arms_used)
# plot the gripper and special point trajectories (red is demo, green is warped)
plot_original_and_warped_demo_and_spec_pt(current_stage_data, warped_stage_data,
demo_spec_pt_xyzs, exp_spec_pt_xyzs,
arms_used)
return resp
