Source code for adam.model.std_factories.std_model

import pathlib
from typing import List
import xml.etree.ElementTree as ET
import os
import urdf_parser_py.urdf

from adam.core.spatial_math import SpatialMath
from adam.model import ModelFactory, StdJoint, StdLink




[docs]
def urdf_remove_sensors_tags(xml_string):
    # Parse the XML string
    root = ET.fromstring(xml_string)

    # Find and remove all tags named "sensor" that are child of
    # root node (i.e. robot)
    for sensors_tag in root.findall("sensor"):
        root.remove(sensors_tag)

    # Convert the modified XML back to a string
    modified_xml_string = ET.tostring(root)

    return modified_xml_string






[docs]
def get_xml_string(path: str):
    isPath = False
    isUrdf = False
    # Checking if it is a path or an urdf
    if type(path) is not (pathlib.Path):
        if os.path.exists(path):
            path = pathlib.Path(path)
            isPath = True
        else:
            root = ET.fromstring(path)
            robot_el = None
            for elem in root.iter():
                if elem.tag == "robot":
                    xml_string = path
                    isUrdf = True
    elif path.exists():
        isPath = True

    if not (isPath) and not (isUrdf):
        raise ValueError(
            f"Invalid urdf string: {path}. It is neither a path nor a urdf string"
        )

    if isPath:
        if not (path.exists()):
            raise FileExistsError(path)
        path = pathlib.Path(path)
        with open(path, "r") as xml_file:
            xml_string = xml_file.read()
            xml_file.close()
    return xml_string






[docs]
class URDFModelFactory(ModelFactory):
    """This factory generates robot elements from urdf_parser_py

    Args:
        ModelFactory: the Model factory
    """

    # TODO: path can be either a path and an urdf-string, leaving path for back compatibility, to be changed to meaningfull name
    def __init__(self, path: str, math: SpatialMath):


[docs]
        self.math = math




[docs]
        xml_string = get_xml_string(path)



        # Read URDF, but before passing it to urdf_parser_py get rid of all sensor tags
        # sensor tags are valid elements of URDF (see ),
        # but they are ignored by urdf_parser_py, that complains every time it sees one.
        # As there is nothing to be fixed in the used models, and it is not useful
        # to have a useless and noisy warning, let's remove before hands all the sensor elements,
        # that anyhow are not parser by urdf_parser_py or adam
        # See https://github.com/ami-iit/ADAM/issues/59



[docs]
        xml_string_without_sensors_tags = urdf_remove_sensors_tags(xml_string)




[docs]
        self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_string(
            xml_string_without_sensors_tags
        )




[docs]
        self.name = self.urdf_desc.name





[docs]
    def get_joints(self) -> List[StdJoint]:
        """
        Returns:
            List[StdJoint]: build the list of the joints
        """
        return [self.build_joint(j) for j in self.urdf_desc.joints]








[docs]
    def get_frames(self) -> List[StdLink]:
        """
        Returns:
            List[StdLink]: build the list of the links

        A link is considered a "fake" link (frame) if
        - it has no inertial
        - it does not have children
        - it is connected to the parent with a fixed joint
        """
        return [
            self.build_link(l)
            for l in self.urdf_desc.links
            if l.inertial is None
            and l.name not in self.urdf_desc.child_map.keys()
            and all(
                j.type == "fixed" for j in self.urdf_desc.joints if j.child == l.name
            )
        ]





[docs]
    def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> StdJoint:
        """
        Args:
            joint (Joint): the urdf_parser_py joint

        Returns:
            StdJoint: our joint representation
        """
        return StdJoint(joint, self.math)