Convert a pose in the form of a list in PoseStamped
    :param poselist: a pose on the form [[x, y, z], [x, y, z, w]]
    :param frame_id: the frame_id on the outputed pose (facultative, empty otherwise)
    :param stamp: the stamp of the outputed pose (facultative, 0 otherwise)
    :return: the converted geometry_msgs/PoseStampted object

def list_to_pose(poselist, frame_id="", stamp=rospy.Time(0)):
    """
    Convert a pose in the form of a list in PoseStamped
    :param poselist: a pose on the form [[x, y, z], [x, y, z, w]]
    :param frame_id: the frame_id on the outputed pose (facultative, empty otherwise)
    :param stamp: the stamp of the outputed pose (facultative, 0 otherwise)
    :return: the converted geometry_msgs/PoseStampted object
    """
    p = PoseStamped()
    p.header.frame_id = frame_id
    p.header.stamp = stamp
    p.pose.position.x = poselist[0][0]
    p.pose.position.y = poselist[0][1]
    p.pose.position.z = poselist[0][2]
    p.pose.orientation.x = poselist[1][0]
    p.pose.orientation.y = poselist[1][1]
    p.pose.orientation.z = poselist[1][2]
    p.pose.orientation.w = poselist[1][3]
    return p