Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
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
Ejemplo n.º 5
0
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
Ejemplo n.º 6
0
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