<commit_before># coding= utf8
"""
.. module:: URDF_utils
"""

from . import link as lib_link
import xml.etree.ElementTree as ET
import json
import numpy as np


def find_next_joint(root, current_link, next_joints):
    """Find the next joint in the URDF tree"""
    # Trouver le joint attaché
    has_next = False
    next_joint = None

    if next_joints == []:
        # If no next joints, find it automaticly
        search_by_name = False
        current_link_name = current_link.attrib["name"]
    else:
        # If a next joint is provided, use it
        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
        else:
            if joint.find("parent").attrib["link"] == current_link_name:
                has_next = True
                next_joint = joint

    return(has_next, next_joint)


def find_next_link(root, current_joint, next_links):
    """Find the next link in the URDF tree"""
    has_next = False
    next_link = None
    # If no next link, find it automaticly
    if next_links == []:
        next_link_name = current_joint.find("child").attrib["link"]
    else:
        # If a next link is provided, use it
        next_link_name = next_links.pop(0)

    for urdf_link in root.iter("link"):
        if urdf_link.attrib["name"] == next_link_name:
            next_link = urdf_link
            has_next = True
    return(has_next, next_link)


def get_urdf_parameters(urdf_file, base_elements=["base_link"], last_link_vector=None, base_elements_type="joint"):
    """Returns translated parameters from the given URDF file

   :param urdf_file: The path of the URDF file
   :type urdf_file: string
   :param base_elements: List of the links beginning the chain
   :type base_elements: list of strings
   :param last_link_vector: Optional : The translation vector of the tip.
   :type last_link_vector: numpy.array
    """
    tree = ET.parse(urdf_file)
    root = tree.getroot()

    base_joints = []
    base_links = []
    for index, element in enumerate(base_elements):
        if index % 2 == 0:
            base_links.append(element)
        else:
            base_joints.append(element)

    joints = []
    links = []
    has_next = True
    current_joint = None<commit_msg>Add the get_chain_from_urdf convenience method. It returns a list of [link, joint, link..., joint] from the list of joints.<commit_after># coding= utf8
"""
.. module:: URDF_utils
"""

from . import link as lib_link
import xml.etree.ElementTree as ET
import json
import numpy as np
import itertools


def find_next_joint(root, current_link, next_joints):
    """Find the next joint in the URDF tree"""
    # Trouver le joint attaché
    has_next = False
    next_joint = None

    if next_joints == []:
        # If no next joints, find it automaticly
        search_by_name = False
        current_link_name = current_link.attrib["name"]
    else:
        # If a next joint is provided, use it
        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
        else:
            if joint.find("parent").attrib["link"] == current_link_name:
                has_next = True
                next_joint = joint

    return(has_next, next_joint)


def find_next_link(root, current_joint, next_links):
    """Find the next link in the URDF tree"""
    has_next = False
    next_link = None
    # If no next link, find it automaticly
    if next_links == []:
        next_link_name = current_joint.find("child").attrib["link"]
    else:
        # If a next link is provided, use it
        next_link_name = next_links.pop(0)

    for urdf_link in root.iter("link"):
        if urdf_link.attrib["name"] == next_link_name:
            next_link = urdf_link
            has_next = True
    return(has_next, next_link)


def find_parent_link(root, joint_name):
    return next(joint.find("parent").attrib["link"]
                for joint in root.iter("joint")
                if joint.attrib["name"] == joint_name)


def get_chain_from_joints(urdf_file, joints):
    tree = ET.parse(urdf_file)
    root = tree.getroot()

    links = [find_parent_link(root, j) for j in joints]

    iters = [iter(links), iter(joints)]
    chain = list(it.next() for it in itertools.cycle(iters))

    return chain


def get_urdf_parameters(urdf_file, base_elements=["base_link"], last_link_vector=None, base_elements_type="joint"):
    """Returns translated parameters from the given URDF file

   :param urdf_file: The path of the URDF file
   :type urdf_file: string
   :param base_elements: List of the links beginning the chain
   :type base_elements: list of strings
   :param last_link_vector: Optional : The translation vector of the tip.
   :type last_link_vector: numpy.array
    """
    tree = ET.parse(urdf_file)
    root = tree.getroot()

    base_joints = []
    base_links = []
    for index, element in enumerate(base_elements):
        if index % 2 == 0:
            base_links.append(element)
        else:
            base_joints.append(element)

    joints = []
    links = []
    has_next = True
    current_joint = None
{'fraction_non_alphanumeric': 0.09521178637200736, 'fraction_numerical': 0.001841620626151013, 'mean_word_length': 2.6597035040431267, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 2}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '884315', 'n_tokens_mistral': 1720, 'n_tokens_neox': 1669, 'n_words': 482}