//  [python]
def select_gripper_pose(sdf, pg, score, cp1, cp2, gripper_depth,
                        num_angle=32, max_width=0.08,
                        th_s=0.997, th_c=0.999,
                        check_tray=True,
                        post_processed=True, gaussian_blur=True):
    """
    Select collision-free pose according to the quality scores of the contact points.
    :param sdf: the SDF instance
    :param pg: a python dictionary containing the surface vertices of the gripper
    :param score: an N-D torch tensor representing the grasp qualities of contact point pairs.
    :param cp1: an Nx3-D torch tensor representing the 3D locations of the left contact points.
    :param cp2: an Nx3-D torch tensor representing the 3D locations of the right contact points.
    :param gripper_depth:y[:, 0:2]
    :param num_angle: the number of discretized angles in x-z plane
    :param max_width:
    :param th_s:
    :param th_c:
    :param check_tray:
    :param post_processed:
    :param gaussian_blur:
    :return: 4 torch tensor representing sets of gripper position, rotations, and contact points
    """
    dtype = score.dtype
    dev = score.device
    score = torch.sigmoid(score)
    score, rank = torch.sort(score, descending=True)
    th_s = score[0] * th_s  # select top 0.1% contact points as candidates
    rank = rank[score >= th_s]
    num_cp = rank.shape[0]
    cp1, cp2 = cp1[rank], cp2[rank]
    pos = (cp1 + cp2) / 2  # num_cp * 3
    y = cp1 - cp2
    distance = torch.linalg.norm(y, dim=1, keepdim=True)
    y = y / distance  # num_cp * 3
    width = torch.clamp(distance + 0.02, 0.0, max_width)  # num_cp * 1
    offset = (max_width - width) / 2
    offset = offset.unsqueeze(-1).unsqueeze(-1)  # num_cp * 1 * 1 * 1
    z = torch.zeros_like(y, dtype=dtype, device=dev)
    z_norm = torch.linalg.norm(y[:, 0:2], dim=1)
    z[:, 0] = y[:, 1] / z_norm
    z[:, 1] = -y[:, 0] / z_norm
    x = torch.cross(y, z, dim=1)
    x = x / torch.linalg.norm(x, dim=1, keepdim=True)
    # u, _, _ = torch.linalg.svd(y.view(num_cp, 3, 1))  # the shape of u: num_cp * 3 * 3
    # x = u[..., 1]  # num_cp * 3
    # z = torch.cross(x, y, dim=1)
    rot0 = torch.stack([x, y, z], dim=2)  # num_cp * 3 * 3
    angles = torch.arange(num_angle, dtype=dtype, device=dev) / num_angle * torch.pi  # num_angle
    delta_rots = basic_rots(angles, axis='y')  # num_angle * 3 * 3
    rots = torch.matmul(rot0.unsqueeze(dim=1), delta_rots.unsqueeze(dim=0))  # num_cp * num_angle * 3 * 3
    z_flag = rots[..., -1, -1] > 0
    i, j = torch.meshgrid(torch.arange(num_cp, device=dev), torch.arange(num_angle, device=dev))
    i, j = i[z_flag], j[z_flag]
    rots[i, j, :, 0], rots[i, j, :, 2] = -rots[i, j, :, 0], -rots[i, j, :, 2]
    ys = rots[..., 1].unsqueeze(dim=2)  # num_cp * num_angle * 1 * 3
    n_h, n_l, n_r = pg['hand'].shape[0], pg['left_finger'].shape[0], pg['right_finger'].shape[0]
    vs = torch.from_numpy(np.concatenate([pg['hand'], pg['left_finger'], pg['right_finger']], axis=0)).to(dtype).to(dev)
    num_v = vs.shape[0]
    gripper_pos = pos.reshape(num_cp, 1, 3) - rots[..., 2] * gripper_depth  # num_cp * num_angle * 3
    vs = torch.matmul(rots, vs.permute(1, 0).unsqueeze(0).unsqueeze(0)).permute(0, 1, 3, 2) + gripper_pos.unsqueeze(2)  # num_cp * num_angle * num_v * 3
    vs[..., n_h:n_h+n_l, :] = vs[..., n_h:n_h+n_l, :] - offset * ys  # left finger vertices
    vs[..., n_h+n_l:n_h+n_l+n_r, :] = vs[..., n_h+n_l:n_h+n_l+n_r, :] + offset * ys  # right finger vertices
    sdv = sdf.interpolation(vs.reshape(-1, 3),
                            use_post_processed=post_processed, smooth=gaussian_blur).reshape(num_cp, num_angle, num_v)
    if check_tray:
        vs_ids = sdf.get_ids(vs.reshape(-1, 3)).reshape(num_cp, num_angle, num_v, 3)
        oob_flag = torch.logical_or(torch.logical_or(vs_ids[..., 0] < 0, vs_ids[..., 0] >= sdf.shape[0]),
                                    torch.logical_or(vs_ids[..., 1] < 0, vs_ids[..., 1] >= sdf.shape[1]))
        sdv[oob_flag] = -1
    num_free = torch.sum(sdv > 0.2, dim=-1)  # num_cp * num_angle
    flag_c = num_free > torch.max(num_free) * th_c
    rots = rots[flag_c]
    gripper_pos = gripper_pos[flag_c]
    width = torch.cat([width] * num_angle, dim=1)[flag_c]
    distance = torch.cat([distance] * num_angle, dim=1)[flag_c]
    pos = torch.stack([pos] * num_angle, dim=1)[flag_c]
    num_free = num_free[flag_c]
    col_score = num_free / num_v
    # resort the contact points
    # _, ids = torch.sort(score_n, descending=True)
    # rots = rots[ids]
    # gripper_pos = gripper_pos[ids]
    # width = width[ids]
    # distance = distance[ids]
    # pos = pos[ids]
    grasp_directions = rots[..., 1]
    cp1 = (pos + grasp_directions * distance.reshape(-1, 1) / 2)
    cp2 = (pos - grasp_directions * distance.reshape(-1, 1) / 2)
    return gripper_pos, rots, width, cp1, cp2