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 transform_demo_with_fingertips(f, demo):
"""
demo has a bunch of fields with arrays
this function uses the Transformation f to transform some of the fields of it using f
"""
warped_demo = {}
for lr in "lr":
if "%s_gripper_tool_frame" % lr in demo:
_, ori = f.transform_frames(
demo["%s_gripper_tool_frame" % lr]["position"],
quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
xyz_fingertip0 = f.transform_points(
demo["%s_gripper_l_finger_tip_link" % lr]["position"])
xyz_fingertip1 = f.transform_points(
demo["%s_gripper_r_finger_tip_link" % lr]["position"])
closing_only = get_gripper_closing_only_times(demo, lr)
assert len(closing_only) == len(ori)
tool_xyzs = []
tool_quats = []
tool_angles = []
print calc_hand_pose(xyz_fingertip0[0], xyz_fingertip1[0],
ori[0])[0][:3, :3]
first, last_xyz, last_quat = True, None, None
for (pos0, pos1, o, cl) in zip(xyz_fingertip0, xyz_fingertip1, ori,
closing_only):
hmat, ang = calc_hand_pose(pos0, pos1, o)
# if the gripper is closing and nothing else is moving,
# don't change anything but the gripper angle
if SPECIAL_GRIPPER_CLOSE and cl and not first:
xyz, quat = last_xyz, last_quat
else:
xyz, quat = conversions.hmat_to_trans_rot(hmat)
tool_xyzs.append(xyz)
tool_quats.append(quat)
tool_angles.append(ang)
last_xyz, last_quat = xyz, quat
first = False
warped_demo["%s_gripper_tool_frame" % lr] = {}
warped_demo["%s_gripper_l_finger_tip_link" % lr] = {}
warped_demo["%s_gripper_l_finger_tip_link" %
lr]["position"] = xyz_fingertip1
warped_demo["%s_gripper_r_finger_tip_link" % lr] = {}
warped_demo["%s_gripper_r_finger_tip_link" %
lr]["position"] = xyz_fingertip0
warped_demo["%s_gripper_tool_frame" %
lr]["position"] = np.array(tool_xyzs)
warped_demo["%s_gripper_tool_frame" %
lr]["orientation"] = np.array(tool_quats)
warped_demo["%s_gripper_joint" % lr] = np.array(tool_angles)
return warped_demo
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 transform_demo_with_fingertips(f, demo):
"""
demo has a bunch of fields with arrays
this function uses the Transformation f to transform some of the fields of it using f
"""
warped_demo = {}
for lr in "lr":
if "%s_gripper_tool_frame"%lr in demo:
_, ori = f.transform_frames(demo["%s_gripper_tool_frame"%lr]["position"], quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
xyz_fingertip0 = f.transform_points(demo["%s_gripper_l_finger_tip_link"%lr]["position"])
xyz_fingertip1 = f.transform_points(demo["%s_gripper_r_finger_tip_link"%lr]["position"])
closing_only = get_gripper_closing_only_times(demo, lr)
assert len(closing_only) == len(ori)
tool_xyzs = []
tool_quats = []
tool_angles = []
print calc_hand_pose(xyz_fingertip0[0], xyz_fingertip1[0], ori[0])[0][:3,:3]
first, last_xyz, last_quat = True, None, None
for (pos0, pos1, o, cl) in zip(xyz_fingertip0, xyz_fingertip1, ori, closing_only):
hmat, ang = calc_hand_pose(pos0, pos1, o)
# if the gripper is closing and nothing else is moving,
# don't change anything but the gripper angle
if SPECIAL_GRIPPER_CLOSE and cl and not first:
xyz, quat = last_xyz, last_quat
else:
xyz, quat = conversions.hmat_to_trans_rot(hmat)
tool_xyzs.append(xyz)
tool_quats.append(quat)
tool_angles.append(ang)
last_xyz, last_quat = xyz, quat
first = False
warped_demo["%s_gripper_tool_frame"%lr]={}
warped_demo["%s_gripper_l_finger_tip_link"%lr]={}
warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"]=xyz_fingertip1
warped_demo["%s_gripper_r_finger_tip_link"%lr]={}
warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]=xyz_fingertip0
warped_demo["%s_gripper_tool_frame"%lr]["position"] = np.array(tool_xyzs)
warped_demo["%s_gripper_tool_frame"%lr]["orientation"] = np.array(tool_quats)
warped_demo["%s_gripper_joint"%lr] = np.array(tool_angles)
return warped_demo
