def find_next_link(root, current_joint, next_links):
    """Find the next link in the URDF tree"""
    has_next = False
    next_link = None
    if next_links == []:
        next_link_name = current_joint.find('child').attrib['link']
    else:
        next_link_name = next_links.pop(0)
    for link in root.iter('link'):
        if link.attrib['name'] == next_link_name:
            next_link = link
            has_next = True
    return has_next, next_link
