def find_next_joint(root, current_link, next_joints):
    """Find the next joint in the URDF tree"""
    has_next = False
    next_joint = None
    if next_joints == []:
        search_by_name = False
        current_link_name = current_link.attrib['name']
    else:
        next_joint_name = next_joints.pop(0)
        search_by_name = True
    for joint in root.iter('joint'):
        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
    return has_next, next_joint
