def reshape_path_vector(v_flat, n_dof=3):
    """ Convert flat vector to n_path x n_dof path array

    n_path is the number of discrete path points.
    This reshaping if often needed to formulate optimization problems.
    The standard optimization variable shape in scipy.optimize is a
    1D vector.

    Parameters
    ----------
    v_flat : ndarray
        Vector of length n_path*ndof containing the joint position vector for
        every path point [q1, q2, ...] with q1 of length ndof.
    ndof : int
        Degrees of freedom of robot.

    Returns
    -------
    tuple of int and numpy.ndarray
        Tuple with first element the number of path poitns and the second
        element an array of shape (n_path, ndof) every row containt the joint
        positions for a single path point.
    
    Examples
    --------
    >>> a = np.array([1, 2, 3, 4, 5, 6])
    >>> N, a_new = reshape_path_vector(a)
    >>> N
    2
    >>> a_new
    array([[1, 2, 3],
           [4, 5, 6]])
    """
    n_path = int(len(v_flat) / n_dof)
    return n_path, v_flat.reshape(n_path, n_dof)
