code: def find_next_joint(root, current_link, next_joint_name):
      
      # Find the joint attached to the link
      has_next = False
      next_joint = None
      search_by_name = True
      current_link_name = None
  
      if next_joint_name is None:
          # If no next joint is provided, find it automatically
          search_by_name = False
          current_link_name = current_link.attrib["name"]
  
      for joint in root.iter("joint"):
          # Iterate through all joints to find the good one
          if search_by_name:
              # Find the joint given its name
              if joint.attrib["name"] == next_joint_name:
                  has_next = True
                  next_joint = joint
          else:
              # Find the first joint whose parent is the current_link
              # FIXME: We are not sending a warning when we have two children for the same link
              # Even if this is not possible, we should ensure something coherent
              if joint.find("parent").attrib["link"] == current_link_name:
                  has_next = True
                  next_joint = joint
                  break
  
      return has_next, next_joint
docs: 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.