def _find_next_joint(root, current_link, next_joint_name):
    """
    Find the next joint in the URDF tree

    Parameters
    ----------
    root
    current_link: xml.etree.ElementTree
        The current URDF link
    next_joint_name: str
        Optional : The name of the next joint. If not provided, find it automatically as the first child of the link.
    """
    has_next = False
    next_joint = None
    search_by_name = True
    current_link_name = None
    if next_joint_name is None:
        search_by_name = False
        current_link_name = current_link.attrib['name']
    for joint in root.findall('joint'):
        if joint is not None:
            if search_by_name:
                if joint.attrib['name'] == next_joint_name:
                    has_next = True
                    next_joint = joint
            elif joint.find('parent').attrib['link'] == current_link_name:
                has_next = True
                next_joint = joint
                break
    return has_next, next_joint
