diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..85f8593 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,43 @@ +name: CI + +on: + push: + branches: [ main, master ] + pull_request: + branches: [ "**" ] + +jobs: + build: + runs-on: ubuntu-latest + strategy: + matrix: + python-version: ["3.9", "3.10", "3.11", "3.12"] + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Set up Python ${{ matrix.python-version }} + uses: actions/setup-python@v5 + with: + python-version: ${{ matrix.python-version }} + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install -r requirements.txt + pip install .[dev] + pip install mypy==1.18.2 ruff==0.14.1 + + - name: Ruff Lint + run: | + ruff check . + ruff format --check . + + - name: Mypy + run: | + mypy urchin + + - name: Pytest + run: | + pytest -q diff --git a/.github/workflows/publish-to-pypi.yml b/.github/workflows/publish-to-pypi.yml index 54eaf0e..2949a3e 100644 --- a/.github/workflows/publish-to-pypi.yml +++ b/.github/workflows/publish-to-pypi.yml @@ -1,36 +1,31 @@ name: Publish Python 🐍 distributions 📦 to PyPI -on: push +on: + push: + tags: + - "*" # Only run on tag pushes jobs: build-n-publish: name: Build and publish Python 🐍 distributions 📦 to PyPI + if: github.repository == 'fishbotics/urchin' runs-on: ubuntu-latest - + steps: - - uses: actions/checkout@v3 - - name: Set up Python - uses: actions/setup-python@v4 - with: - python-version: "3.x" - - - name: Install pypa/build - run: >- - python -m - pip install - build - --user - - name: Build a binary wheel and a source tarball - run: >- - python -m - build - --sdist - --wheel - --outdir dist/ - . + - uses: actions/checkout@v3 + + - name: Set up Python + uses: actions/setup-python@v4 + with: + python-version: "3.x" + + - name: Install pypa/build + run: python -m pip install build --user + + - name: Build a binary wheel and a source tarball + run: python -m build --sdist --wheel --outdir dist/ - - name: Publish distribution 📦 to PyPI - if: startsWith(github.ref, 'refs/tags') - uses: pypa/gh-action-pypi-publish@release/v1 - with: - password: ${{ secrets.PYPI_API_TOKEN }} + - name: Publish distribution 📦 to PyPI + uses: pypa/gh-action-pypi-publish@release/v1 + with: + password: ${{ secrets.PYPI_API_TOKEN }} diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index bb25e5c..eec9daf 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -4,13 +4,12 @@ on: push: pull_request: schedule: - # * is a special character in YAML so you have to quote this string - # Execute a "weekly" build at 0 AM UTC on Mondays - - cron: '0 0 * * MON' + # * is a special character in YAML so you have to quote this string + # Execute a "weekly" build at 0 AM UTC on Mondays + - cron: "0 0 * * MON" jobs: build: - runs-on: ubuntu-latest strategy: fail-fast: false @@ -18,17 +17,17 @@ jobs: python-version: ["3.9", "3.10", "3.11", "3.12", "3.13"] steps: - - uses: actions/checkout@v3 - - name: Set up Python ${{ matrix.python-version }} - uses: actions/setup-python@v3 - with: - python-version: ${{ matrix.python-version }} - - name: Install package - run: | - python -m pip install --upgrade pip - python -m pip install flake8 pytest pytest-cov coveralls wheel - python -m pip install . + - uses: actions/checkout@v3 + - name: Set up Python ${{ matrix.python-version }} + uses: actions/setup-python@v3 + with: + python-version: ${{ matrix.python-version }} + - name: Install package + run: | + python -m pip install --upgrade pip + python -m pip install flake8 pytest pytest-cov coveralls wheel + python -m pip install . - - name: Test with pytest - run: | - pytest --cov=urchin tests + - name: Test with pytest + run: | + pytest --cov=urchin tests diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1817eb3..f15b043 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,6 +1,27 @@ repos: -- repo: https://gitlab.com/pycqa/flake8 - rev: 3.7.1 - hooks: - - id: flake8 - exclude: ^setup.py + - repo: https://gitlab.com/pycqa/flake8 + rev: 3.7.1 + hooks: + - id: flake8 + exclude: ^setup.py + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.14.1 + hooks: + - id: ruff + name: ruff (lint + isort) + args: ["--fix"] + - id: ruff-format + name: ruff (format) + - repo: https://github.com/pre-commit/mirrors-mypy + rev: v1.18.2 + hooks: + - id: mypy + name: mypy + args: ["--config=pyproject.toml", "urchin"] + - repo: local + hooks: + - id: pytest + name: pytest + entry: pytest -q + language: system + pass_filenames: false diff --git a/docs/source/conf.py b/docs/source/conf.py index 6fadac3..e103309 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -79,7 +79,7 @@ # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. -exclude_patterns = [] +exclude_patterns: list[str] = [] # The name of the Pygments (syntax highlighting) style to use. pygments_style = None @@ -124,7 +124,7 @@ # -- Options for LaTeX output ------------------------------------------------ -latex_elements = { +latex_elements: dict[str, str] = { # The paper size ('letterpaper' or 'a4paper'). # # 'papersize': 'letterpaper', @@ -217,9 +217,7 @@ class MyPythonDomain(PythonDomain): def find_obj(self, env, modname, classname, name, type, searchmode=0): """Ensures an object always resolves to the desired module if defined there.""" - orig_matches = PythonDomain.find_obj( - self, env, modname, classname, name, type, searchmode - ) + orig_matches = PythonDomain.find_obj(self, env, modname, classname, name, type, searchmode) if len(orig_matches) <= 1: return orig_matches diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..58bd40b --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,73 @@ +[build-system] +requires = ["setuptools>=61.0", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "urchin" +version = "0.0.30" +requires-python = ">=3.9" +description = "URDF parser and manipulator for Python" +readme = { text = "URDF parser and manipulator for Python. This package is a fork of urdfpy, which seems to be no longer maintained.", content-type = "text/plain" } +authors = [ + { name = "Adam Fishman", email = "hello@fishbotics.com" } +] +license = { text = "MIT License" } +urls = { "Homepage" = "https://github.com/fishbotics/urchin" } +keywords = ["robotics", "ros", "urdf", "robots", "parser"] +classifiers = [ + "Development Status :: 4 - Beta", + "License :: OSI Approved :: MIT License", + "Programming Language :: Python", + "Programming Language :: Python :: 3.9", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", + "Programming Language :: Python :: 3.12", + "Natural Language :: English", + "Topic :: Scientific/Engineering" +] +dependencies = [ + "lxml", # For XML DOM Tree + "networkx", # For joint graph + "numpy>=1.20", # Numpy + "pillow", # For texture image loading + "pycollada>=0.6", # COLLADA (.dae) mesh loading via trimesh + "pyribbit>=0.1.46", # For visualization + "scipy", # For trimesh, annoyingly + "trimesh" # Mesh geometry loading/creation/saving +] + +[project.optional-dependencies] +dev = [ + "flake8", # Code formatting checker + "pre-commit", # Pre-commit hooks + "pytest", # Code testing + "pytest-cov", # Coverage testing + "tox" # Automatic virtualenv testing +] +docs = [ + "sphinx", # General doc library + "sphinx_rtd_theme", # RTD theme for sphinx + "sphinx-automodapi" # For generating nice tables +] + +[tool.mypy] +python_version = "3.10" +strict = false +warn_unused_ignores = true + +[[tool.mypy.overrides]] +module = ["lxml.*", "trimesh.*", "networkx.*", "PIL.*", "pyribbit.*", "sphinx.*", "sphinx_rtd_theme"] +ignore_missing_imports = true + +[tool.ruff] +line-length = 100 +target-version = "py310" + +[tool.ruff.lint] +select = ["E", "F", "I"] +fixable = ["ALL"] + +[tool.ruff.format] +quote-style = "double" +indent-style = "space" +docstring-code-format = true diff --git a/scripts/vis_urdf.py b/scripts/vis_urdf.py index bd535ab..1eca005 100644 --- a/scripts/vis_urdf.py +++ b/scripts/vis_urdf.py @@ -2,17 +2,15 @@ Script for visualizing a robot from a URDF. Author: Matthew Matl """ + import argparse import urchin if __name__ == "__main__": - # Parse Args parser = argparse.ArgumentParser(description="Visualize a robot from a URDF file") - parser.add_argument( - "urdf", type=str, help="Path to URDF file that describes the robot" - ) + parser.add_argument("urdf", type=str, help="Path to URDF file that describes the robot") parser.add_argument("-a", action="store_true", help="Visualize robot articulation") parser.add_argument("-c", action="store_true", help="Use collision geometry") diff --git a/setup.py b/setup.py deleted file mode 100644 index a405e05..0000000 --- a/setup.py +++ /dev/null @@ -1,59 +0,0 @@ -""" -Author: Adam Fishman -""" -from setuptools import setup - -requirements = [ - "lxml", # For XML DOM Tree - "networkx", # For joint graph - "numpy", # Numpy - "pillow", # For texture image loading - "pycollada>=0.6", # COLLADA (.dae) mesh loading via trimesh - "pyribbit>=0.1.46", # For visualization - "scipy", # For trimesh, annoyingly - "six", # Python 2/3 compatability - "trimesh", # Mesh geometry loading/creation/saving -] - -dev_requirements = [ - "flake8", # Code formatting checker - "pre-commit", # Pre-commit hooks - "pytest", # Code testing - "pytest-cov", # Coverage testing - "tox", # Automatic virtualenv testing -] - -docs_requirements = [ - "sphinx", # General doc library - "sphinx_rtd_theme", # RTD theme for sphinx - "sphinx-automodapi", # For generating nice tables -] - -setup( - name="urchin", - version="0.0.29", - description="URDF parser and manipulator for Python", - long_description="URDF parser and manipulator for Python. This package is a fork of urdfpy, which seems to be no longer maintained. ", - author="Adam Fishman", - author_email="hello@fishbotics.com", - license="MIT License", - url="https://github.com/fishbotics/urchin", - keywords="robotics ros urdf robots parser", - classifiers=[ - "Development Status :: 4 - Beta", - "License :: OSI Approved :: MIT License", - "Programming Language :: Python", - "Programming Language :: Python :: 2.7", - "Programming Language :: Python :: 3.5", - "Programming Language :: Python :: 3.6", - "Natural Language :: English", - "Topic :: Scientific/Engineering", - ], - packages=["urchin"], - setup_requires=requirements, - install_requires=requirements, - extras_require={ - "dev": dev_requirements, - "docs": docs_requirements, - }, -) diff --git a/tests/unit/test_urdf.py b/tests/unit/test_urdf.py index 0af5644..114769c 100644 --- a/tests/unit/test_urdf.py +++ b/tests/unit/test_urdf.py @@ -2,7 +2,7 @@ import pytest import trimesh -from urchin import URDF, Link, Joint, Transmission, Material +from urchin import URDF, Joint, Link, Material, Transmission def test_urchin(tmpdir): @@ -14,8 +14,8 @@ def test_urchin(tmpdir): assert isinstance(u, URDF) for j in u.joints: assert isinstance(j, Joint) - for l in u.links: - assert isinstance(l, Link) + for link in u.links: + assert isinstance(link, Link) for t in u.transmissions: assert isinstance(t, Transmission) for m in u.materials: @@ -24,24 +24,24 @@ def test_urchin(tmpdir): # Test fk fk = u.link_fk() assert isinstance(fk, dict) - for l in fk: - assert isinstance(l, Link) - assert isinstance(fk[l], np.ndarray) - assert fk[l].shape == (4, 4) + for link in fk: + assert isinstance(link, Link) + assert isinstance(fk[link], np.ndarray) + assert fk[link].shape == (4, 4) fk = u.link_fk({"shoulder_pan_joint": 2.0}) assert isinstance(fk, dict) - for l in fk: - assert isinstance(l, Link) - assert isinstance(fk[l], np.ndarray) - assert fk[l].shape == (4, 4) + for link in fk: + assert isinstance(link, Link) + assert isinstance(fk[link], np.ndarray) + assert fk[link].shape == (4, 4) fk = u.link_fk(np.zeros(6)) assert isinstance(fk, dict) - for l in fk: - assert isinstance(l, Link) - assert isinstance(fk[l], np.ndarray) - assert fk[l].shape == (4, 4) + for link in fk: + assert isinstance(link, Link) + assert isinstance(fk[link], np.ndarray) + assert fk[link].shape == (4, 4) fk = u.link_fk(np.zeros(6), link="upper_arm_link") assert isinstance(fk, np.ndarray) @@ -50,18 +50,18 @@ def test_urchin(tmpdir): fk = u.link_fk(links=["shoulder_link", "upper_arm_link"]) assert isinstance(fk, dict) assert len(fk) == 2 - for l in fk: - assert isinstance(l, Link) - assert isinstance(fk[l], np.ndarray) - assert fk[l].shape == (4, 4) + for link in fk: + assert isinstance(link, Link) + assert isinstance(fk[link], np.ndarray) + assert fk[link].shape == (4, 4) fk = u.link_fk(links=list(u.links)[:2]) assert isinstance(fk, dict) assert len(fk) == 2 - for l in fk: - assert isinstance(l, Link) - assert isinstance(fk[l], np.ndarray) - assert fk[l].shape == (4, 4) + for link in fk: + assert isinstance(link, Link) + assert isinstance(fk[link], np.ndarray) + assert fk[link].shape == (4, 4) cfg = {j.name: 0.5 for j in u.actuated_joints} for _ in range(1000): diff --git a/tests/unit/test_utils.py b/tests/unit/test_utils.py index 01bff1a..888cdf5 100644 --- a/tests/unit/test_utils.py +++ b/tests/unit/test_utils.py @@ -1,7 +1,7 @@ import numpy as np import trimesh.transformations as tfs -from urchin import rpy_to_matrix, matrix_to_rpy, matrix_to_xyz_rpy, xyz_rpy_to_matrix +from urchin import matrix_to_rpy, matrix_to_xyz_rpy, rpy_to_matrix, xyz_rpy_to_matrix def test_rpy_to_matrix(): diff --git a/urchin/__init__.py b/urchin/__init__.py index 460e48e..4c38a6d 100644 --- a/urchin/__init__.py +++ b/urchin/__init__.py @@ -1,20 +1,56 @@ -from .urdf import (URDFType, URDFTypeWithMesh, - Box, Cylinder, Sphere, Mesh, Geometry, - Texture, Material, - Collision, Visual, Inertial, - JointCalibration, JointDynamics, JointLimit, JointMimic, - SafetyController, Actuator, TransmissionJoint, - Transmission, Joint, Link, URDF) -from .utils import (rpy_to_matrix, matrix_to_rpy, xyz_rpy_to_matrix, - matrix_to_xyz_rpy) +from .base import URDFType, URDFTypeWithMesh +from .joint import ( + Joint, + JointCalibration, + JointDynamics, + JointLimit, + JointMimic, + SafetyController, +) +from .link import ( + Box, + Collision, + Cylinder, + Geometry, + Inertial, + Link, + Mesh, + Sphere, + Visual, +) +from .material import Material, Texture +from .transmission import Actuator, Transmission, TransmissionJoint +from .urdf import URDF +from .utils import matrix_to_rpy, matrix_to_xyz_rpy, rpy_to_matrix, xyz_rpy_to_matrix from .version import __version__ __all__ = [ - 'URDFType', 'URDFTypeWithMesh', 'Box', 'Cylinder', 'Sphere', 'Mesh', 'Geometry', - 'Texture', 'Material', 'Collision', 'Visual', 'Inertial', - 'JointCalibration', 'JointDynamics', 'JointLimit', 'JointMimic', - 'SafetyController', 'Actuator', 'TransmissionJoint', - 'Transmission', 'Joint', 'Link', 'URDF', - 'rpy_to_matrix', 'matrix_to_rpy', 'xyz_rpy_to_matrix', 'matrix_to_xyz_rpy', - '__version__' + "URDFType", + "URDFTypeWithMesh", + "Box", + "Cylinder", + "Sphere", + "Mesh", + "Geometry", + "Texture", + "Material", + "Collision", + "Visual", + "Inertial", + "JointCalibration", + "JointDynamics", + "JointLimit", + "JointMimic", + "SafetyController", + "Actuator", + "TransmissionJoint", + "Transmission", + "Joint", + "Link", + "URDF", + "rpy_to_matrix", + "matrix_to_rpy", + "xyz_rpy_to_matrix", + "matrix_to_xyz_rpy", + "__version__", ] diff --git a/urchin/base.py b/urchin/base.py new file mode 100644 index 0000000..8b8a3b1 --- /dev/null +++ b/urchin/base.py @@ -0,0 +1,414 @@ +from __future__ import annotations + +from typing import Optional, Type, TypeVar, Union, cast + +import numpy as np +import numpy.typing as npt +from lxml import etree as ET + +ParsedAttribute = Union[bool, float, int, str, npt.NDArray[np.float64], None] +ParsedElement = Union["URDFType", list["URDFType"], None] +ParsedValue = Union[ParsedAttribute, ParsedElement] +ParsedAttributeDict = dict[str, ParsedAttribute] +ParsedElementDict = dict[str, ParsedElement] +ParsedValueDict = dict[str, ParsedValue] +T = TypeVar("T", bound="URDFType") + + +class URDFType: + """Abstract base class for all URDF types. + + This has useful class methods for automatic parsing/unparsing + of XML trees. + + There are three overridable class variables: + + - ``_ATTRIBS`` - This is a dictionary mapping attribute names to a tuple, + ``(type, required)`` where ``type`` is the Python type for the + attribute and ``required`` is a boolean stating whether the attribute + is required to be present. + - ``_ELEMENTS`` - This is a dictionary mapping element names to a tuple, + ``(type, required, multiple)`` where ``type`` is the Python type for the + element, ``required`` is a boolean stating whether the element + is required to be present, and ``multiple`` is a boolean indicating + whether multiple elements of this type could be present. + Elements are child nodes in the XML tree, and their type must be a + subclass of :class:`.URDFType`. + - ``_TAG`` - This is a string that represents the XML tag for the node + containing this type of object. + """ + + _ATTRIBS: dict[str, tuple[type, bool]] = {} # Map from attrib name to (type, required) + _ELEMENTS: dict[ + str, tuple[Type["URDFType"], bool, bool] + ] = {} # Map from element name to (type, required, multiple) + _TAG: str = "" # XML tag for this element + + def __init__(self): + pass + + @classmethod + def _parse_attrib(cls, val_type: type, val: str) -> ParsedAttribute: + """Parse an XML attribute into a python value. + + Parameters + ---------- + val_type : :class:`type` + The type of value to create. + val : str + The string value to parse. + + Returns + ------- + val : ParsedAttribute + The parsed attribute value. + """ + if val_type == np.ndarray: + array_value = cast(npt.NDArray[np.float64], np.fromstring(val, sep=" ")) + return array_value + return cast(ParsedAttribute, val_type(val)) + + @classmethod + def _parse_simple_attribs(cls, node: ET._Element) -> ParsedAttributeDict: + """Parse all attributes in the _ATTRIBS array for this class. + + Parameters + ---------- + node : :class:`lxml.etree.Element` + The node to parse attributes for. + + Returns + ------- + kwargs : ParsedAttributeDict + Map from attribute name to value. If the attribute is not + required and is not present, that attribute's name will map to + ``None``. + """ + kwargs: ParsedAttributeDict = {} + for attrib_name, (val_type, required) in cls._ATTRIBS.items(): + if required: + try: + value = cls._parse_attrib(val_type, node.attrib[attrib_name]) + except Exception: + raise ValueError( + "Missing required attribute {} when parsing an object of type {}".format( + attrib_name, cls.__name__ + ) + ) + else: + value = None + if attrib_name in node.attrib: + value = cls._parse_attrib(val_type, node.attrib[attrib_name]) + kwargs[attrib_name] = value + return kwargs + + @classmethod + def _parse_simple_elements( + cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None + ) -> ParsedElementDict: + """Parse all elements in the _ELEMENTS array from the children of + this node. + + Parameters + ---------- + node : :class:`lxml.etree.Element` + The node to parse children for. + path : str + The string path where the XML file is located (used for resolving + the location of mesh or image files). + lazy_load_meshes : bool + Whether a mesh element should be immediately loaded or loaded when + needed + + Returns + ------- + kwargs : ParsedElementDict + Map from element names to the :class:`URDFType` subclass (or list, + if ``multiple`` was set) created for that element. + """ + kwargs: ParsedElementDict = {} + for element_name, (element_type, required, multiple) in cls._ELEMENTS.items(): + value: ParsedElement + if not multiple: + element_node = node.find(element_type._TAG) + if required or element_node is not None: + value = cast( + ParsedElement, + element_type._from_xml(element_node, path), + ) + else: + value = None + else: + element_nodes = node.findall(element_type._TAG) + if len(element_nodes) == 0 and required: + print( + f"Missing required subelement(s) of type {element_type.__name__} when " + f"parsing an object of type {cls.__name__}." + ) + value = [element_type._from_xml(child, path) for child in element_nodes] + kwargs[element_name] = value + return kwargs + + @classmethod + def _parse( + cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None + ) -> ParsedValueDict: + """Parse all elements and attributes in the _ELEMENTS and _ATTRIBS + arrays for a node. + + Parameters + ---------- + node : :class:`lxml.etree.Element` + The node to parse. + path : str + The string path where the XML file is located (used for resolving + the location of mesh or image files). + + Returns + ------- + kwargs : ParsedValueDict + Map from names to Python values created from the attributes + and elements in the class arrays. + """ + kwargs: ParsedValueDict = {} + kwargs.update(cls._parse_simple_attribs(node)) + kwargs.update(cls._parse_simple_elements(node, path, lazy_load_meshes)) + return kwargs + + @classmethod + def _from_xml( + cls: type[T], + node: ET._Element, + path: str, + lazy_load_meshes: Optional[bool] = None, + ) -> T: + """Create an instance of this class from an XML node. + + Parameters + ---------- + node : :class:`lxml.etree.Element` + The node to parse. + path : str + The string path where the XML file is located (used for resolving + the location of mesh or image files). + + Returns + ------- + obj : :class:`URDFType` + An instance of this class parsed from the node. + """ + return cls(**cls._parse(node, path, lazy_load_meshes)) + + def _unparse_attrib(self, val_type: type, val: ParsedAttribute) -> str: + """Convert a Python value into a string for storage in an + XML attribute. + + Parameters + ---------- + val_type : :class:`type` + The type of the Python object. + val : ParsedAttribute + The actual value. + + Returns + ------- + s : str + The attribute string. + """ + if val_type == np.ndarray: + array_value = cast(npt.NDArray[np.float64], val) + return np.array2string(array_value)[1:-1] + return str(val) + + def _unparse_simple_attribs(self, node: ET._Element) -> None: + """Convert all Python types from the _ATTRIBS array back into attributes + for an XML node. + + Parameters + ---------- + node : :class:`lxml.etree.Element` + The XML node to add the attributes to. + """ + for attrib_name, (val_type, required) in self._ATTRIBS.items(): + value = cast(ParsedAttribute, getattr(self, attrib_name, None)) + if required or value is not None: + node.attrib[attrib_name] = self._unparse_attrib(val_type, value) + + def _unparse_simple_elements(self, node: ET._Element, path: str) -> None: + """Unparse all Python types from the _ELEMENTS array back into child + nodes of an XML node. + + Parameters + ---------- + node : :class:`lxml.etree.Element` + The XML node for this object. Elements will be added as children + of this node. + path : str + The string path where the XML file is being written to (used for + writing out meshes and image files). + """ + for element_name, (element_type, _required, multiple) in self._ELEMENTS.items(): + value = getattr(self, element_name, None) + if not multiple: + element_value = cast(Optional[URDFType], value) + if element_value is not None: + node.append(element_value._to_xml(node, path)) + else: + element_values = cast(Optional[list[URDFType]], value) + for child in element_values or []: + node.append(child._to_xml(node, path)) + + def _unparse(self, path: str) -> ET._Element: + """Create a node for this object and unparse all elements and + attributes in the class arrays. + + Parameters + ---------- + path : str + The string path where the XML file is being written to (used for + writing out meshes and image files). + + Returns + ------- + node : :class:`lxml.etree.Element` + The newly-created node. + """ + node = ET.Element(self._TAG) + self._unparse_simple_attribs(node) + self._unparse_simple_elements(node, path) + return node + + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: + """Create and return an XML node for this object. + + Parameters + ---------- + parent : :class:`lxml.etree.Element` + The parent node that this element will eventually be added to. + This base implementation doesn't use this information, but + classes that override this function may use it. + path : str + The string path where the XML file is being written to (used for + writing out meshes and image files). + + Returns + ------- + node : :class:`lxml.etree.Element` + The newly-created node. + """ + return self._unparse(path) + + +class URDFTypeWithMesh(URDFType): + @classmethod + def _parse_simple_elements( + cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None + ) -> ParsedElementDict: + """Parse all elements in the _ELEMENTS array from the children of + this node. + + Parameters + ---------- + node : :class:`lxml.etree.Element` + The node to parse children for. + path : str + The string path where the XML file is located (used for resolving + the location of mesh or image files). + lazy_load_meshes : bool + Whether a mesh element should be immediately loaded or loaded when + needed + + Returns + ------- + kwargs : ParsedElementDict + Map from element names to the :class:`URDFType` subclass (or list, + if ``multiple`` was set) created for that element. + """ + kwargs: ParsedElementDict = {} + for element_name, (element_type, required, multiple) in cls._ELEMENTS.items(): + value: ParsedElement + if not multiple: + element_node = node.find(element_type._TAG) + if required or element_node is not None: + if issubclass(element_type, URDFTypeWithMesh): + value = cast( + ParsedElement, + element_type._from_xml(element_node, path, lazy_load_meshes), + ) + else: + value = cast( + ParsedElement, + element_type._from_xml(element_node, path), + ) + else: + value = None + else: + element_nodes = node.findall(element_type._TAG) + if len(element_nodes) == 0 and required: + raise ValueError( + "Missing required subelement(s) of type {} when " + "parsing an object of type {}".format(element_type.__name__, cls.__name__) + ) + if issubclass(element_type, URDFTypeWithMesh): + value = [ + element_type._from_xml(child, path, lazy_load_meshes) + for child in element_nodes + ] + else: + value = [element_type._from_xml(child, path) for child in element_nodes] + kwargs[element_name] = value + return kwargs + + @classmethod + def _parse( + cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None + ) -> ParsedValueDict: + """Parse all elements and attributes in the _ELEMENTS and _ATTRIBS + arrays for a node. + + Parameters + ---------- + node : :class:`lxml.etree.Element` + The node to parse. + path : str + The string path where the XML file is located (used for resolving + the location of mesh or image files). + lazy_load_meshes : bool + Whether meshes should be loaded immediately or upon their first use + + Returns + ------- + kwargs : ParsedValueDict + Map from names to Python values created from the attributes + and elements in the class arrays. + """ + kwargs: ParsedValueDict = {} + kwargs.update(cls._parse_simple_attribs(node)) + kwargs.update(cls._parse_simple_elements(node, path, lazy_load_meshes)) + return kwargs + + @classmethod + def _from_xml( + cls: type[T], + node: ET._Element, + path: str, + lazy_load_meshes: Optional[bool] = None, + ) -> T: + """Create an instance of this class from an XML node. + + Parameters + ---------- + node : :class:`lxml.etree.Element` + The node to parse. + path : str + The string path where the XML file is located (used for resolving + the location of mesh or image files). + lazy_load_meshes : bool + Whether meshes should be loaded immediately or upon their first use + + Returns + ------- + obj : :class:`URDFType` + An instance of this class parsed from the node. + """ + return cls(**cls._parse(node, path, lazy_load_meshes)) diff --git a/urchin/joint.py b/urchin/joint.py new file mode 100644 index 0000000..af4efeb --- /dev/null +++ b/urchin/joint.py @@ -0,0 +1,896 @@ +from __future__ import annotations + +from typing import Optional, Sequence, Union + +import numpy as np +import numpy.typing as npt +import trimesh +from lxml import etree as ET + +from urchin.base import URDFType +from urchin.utils import configure_origin, parse_origin, unparse_origin + + +class SafetyController(URDFType): + """A controller for joint movement safety. + + Parameters + ---------- + k_velocity : float + An attribute specifying the relation between the effort and velocity + limits. + k_position : float, optional + An attribute specifying the relation between the position and velocity + limits. Defaults to 0.0. + soft_lower_limit : float, optional + The lower joint boundary where the safety controller kicks in. + Defaults to 0.0. + soft_upper_limit : float, optional + The upper joint boundary where the safety controller kicks in. + Defaults to 0.0. + """ + + _ATTRIBS = { + "k_velocity": (float, True), + "k_position": (float, False), + "soft_lower_limit": (float, False), + "soft_upper_limit": (float, False), + } + _TAG = "safety_controller" + + def __init__( + self, + k_velocity: float, + k_position: Optional[float] = None, + soft_lower_limit: Optional[float] = None, + soft_upper_limit: Optional[float] = None, + ): + self.k_velocity = k_velocity + self.k_position = k_position + self.soft_lower_limit = soft_lower_limit + self.soft_upper_limit = soft_upper_limit + + @property + def soft_lower_limit(self) -> float: + """float : The soft lower limit where the safety controller kicks in.""" + return self._soft_lower_limit + + @soft_lower_limit.setter + def soft_lower_limit(self, value: Optional[float]) -> None: + if value is not None: + value = float(value) + else: + value = 0.0 + self._soft_lower_limit = value + + @property + def soft_upper_limit(self) -> float: + """float : The soft upper limit where the safety controller kicks in.""" + return self._soft_upper_limit + + @soft_upper_limit.setter + def soft_upper_limit(self, value: Optional[float]) -> None: + if value is not None: + value = float(value) + else: + value = 0.0 + self._soft_upper_limit = value + + @property + def k_position(self) -> float: + """float : A relation between the position and velocity limits.""" + return self._k_position + + @k_position.setter + def k_position(self, value: Optional[float]) -> None: + if value is not None: + value = float(value) + else: + value = 0.0 + self._k_position = value + + @property + def k_velocity(self) -> float: + """float : A relation between the effort and velocity limits.""" + return self._k_velocity + + @k_velocity.setter + def k_velocity(self, value: float) -> None: + self._k_velocity = float(value) + + def copy( + self, prefix: str = "", scale: Union[float, Sequence[float], None] = None + ) -> "SafetyController": + """Create a deep copy of the visual with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + + Returns + ------- + :class:`.SafetyController` + A deep copy of the visual. + """ + return self.__class__( + k_velocity=self.k_velocity, + k_position=self.k_position, + soft_lower_limit=self.soft_lower_limit, + soft_upper_limit=self.soft_upper_limit, + ) + + +class JointCalibration(URDFType): + """The reference positions of the joint. + + Parameters + ---------- + rising : float, optional + When the joint moves in a positive direction, this position will + trigger a rising edge. + falling : + When the joint moves in a positive direction, this position will + trigger a falling edge. + """ + + _ATTRIBS = {"rising": (float, False), "falling": (float, False)} + _TAG = "calibration" + + def __init__(self, rising: Optional[float] = None, falling: Optional[float] = None): + self.rising = rising + self.falling = falling + + @property + def rising(self) -> Optional[float]: + """float : description.""" + return self._rising + + @rising.setter + def rising(self, value: Optional[float]) -> None: + if value is not None: + value = float(value) + self._rising = value + + @property + def falling(self) -> Optional[float]: + """float : description.""" + return self._falling + + @falling.setter + def falling(self, value: Optional[float]) -> None: + if value is not None: + value = float(value) + self._falling = value + + def copy( + self, prefix: str = "", scale: Union[float, Sequence[float], None] = None + ) -> "JointCalibration": + """Create a deep copy of the visual with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + + Returns + ------- + :class:`.JointCalibration` + A deep copy of the visual. + """ + return self.__class__( + rising=self.rising, + falling=self.falling, + ) + + +class JointDynamics(URDFType): + """The dynamic properties of the joint. + + Parameters + ---------- + damping : float + The damping value of the joint (Ns/m for prismatic joints, + Nms/rad for revolute). + friction : float + The static friction value of the joint (N for prismatic joints, + Nm for revolute). + """ + + _ATTRIBS = { + "damping": (float, False), + "friction": (float, False), + } + _TAG = "dynamics" + + def __init__(self, damping: Optional[float], friction: Optional[float]): + self.damping = damping + self.friction = friction + + @property + def damping(self) -> Optional[float]: + """float : The damping value of the joint.""" + return self._damping + + @damping.setter + def damping(self, value: Optional[float]) -> None: + if value is not None: + value = float(value) + self._damping = value + + @property + def friction(self) -> Optional[float]: + """float : The static friction value of the joint.""" + return self._friction + + @friction.setter + def friction(self, value: Optional[float]) -> None: + if value is not None: + value = float(value) + self._friction = value + + def copy( + self, prefix: str = "", scale: Union[float, Sequence[float], None] = None + ) -> "JointDynamics": + """Create a deep copy with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + + Returns + ------- + :class:`.JointDynamics` + A deep copy of the visual. + """ + return self.__class__( + damping=self.damping, + friction=self.friction, + ) + + +class JointLimit(URDFType): + """The limits of the joint. + + Parameters + ---------- + effort : float + The maximum joint effort (N for prismatic joints, Nm for revolute). + velocity : float + The maximum joint velocity (m/s for prismatic joints, rad/s for + revolute). + lower : float, optional + The lower joint limit (m for prismatic joints, rad for revolute). + upper : float, optional + The upper joint limit (m for prismatic joints, rad for revolute). + """ + + _ATTRIBS = { + "effort": (float, True), + "velocity": (float, True), + "lower": (float, False), + "upper": (float, False), + } + _TAG = "limit" + + def __init__( + self, + effort: float, + velocity: float, + lower: Optional[float] = None, + upper: Optional[float] = None, + ): + self.effort = effort + self.velocity = velocity + self.lower = lower + self.upper = upper + + @property + def effort(self) -> float: + """float : The maximum joint effort.""" + return self._effort + + @effort.setter + def effort(self, value: float) -> None: + self._effort = float(value) + + @property + def velocity(self) -> float: + """float : The maximum joint velocity.""" + return self._velocity + + @velocity.setter + def velocity(self, value: float) -> None: + self._velocity = float(value) + + @property + def lower(self) -> Optional[float]: + """float : The lower joint limit.""" + return self._lower + + @lower.setter + def lower(self, value: Optional[float]) -> None: + if value is not None: + value = float(value) + self._lower = value + + @property + def upper(self) -> Optional[float]: + """float : The upper joint limit.""" + return self._upper + + @upper.setter + def upper(self, value: Optional[float]) -> None: + if value is not None: + value = float(value) + self._upper = value + + def copy( + self, prefix: str = "", scale: Union[float, Sequence[float], None] = None + ) -> "JointLimit": + """Create a deep copy with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + + Returns + ------- + :class:`.JointLimit` + A deep copy of the visual. + """ + return self.__class__( + effort=self.effort, + velocity=self.velocity, + lower=self.lower, + upper=self.upper, + ) + + +class JointMimic(URDFType): + """A mimicry tag for a joint, which forces its configuration to + mimic another joint's. + + This joint's configuration value is set equal to + ``multiplier * other_joint_cfg + offset``. + + Parameters + ---------- + joint : str + The name of the joint to mimic. + multiplier : float + The joint configuration multiplier. Defaults to 1.0. + offset : float, optional + The joint configuration offset. Defaults to 0.0. + """ + + _ATTRIBS = { + "joint": (str, True), + "multiplier": (float, False), + "offset": (float, False), + } + _TAG = "mimic" + + def __init__( + self, joint: str, multiplier: Optional[float] = None, offset: Optional[float] = None + ): + self.joint = joint + self.multiplier = multiplier + self.offset = offset + + @property + def joint(self) -> str: + """float : The name of the joint to mimic.""" + return self._joint + + @joint.setter + def joint(self, value: str) -> None: + self._joint = str(value) + + @property + def multiplier(self) -> float: + """float : The multiplier for the joint configuration.""" + return self._multiplier + + @multiplier.setter + def multiplier(self, value: Optional[float]) -> None: + if value is not None: + value = float(value) + else: + value = 1.0 + self._multiplier = value + + @property + def offset(self) -> float: + """float : The offset for the joint configuration""" + return self._offset + + @offset.setter + def offset(self, value: Optional[float]) -> None: + if value is not None: + value = float(value) + else: + value = 0.0 + self._offset = value + + def copy( + self, prefix: str = "", scale: Union[float, Sequence[float], None] = None + ) -> "JointMimic": + """Create a deep copy of the joint mimic with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + + Returns + ------- + :class:`.JointMimic` + A deep copy of the joint mimic. + """ + return self.__class__( + joint="{}{}".format(prefix, self.joint), + multiplier=self.multiplier, + offset=self.offset, + ) + + +class Joint(URDFType): + """A connection between two links. + + There are several types of joints, including: + + - ``fixed`` - a joint that cannot move. + - ``prismatic`` - a joint that slides along the joint axis. + - ``revolute`` - a hinge joint that rotates about the axis with a limited + range of motion. + - ``continuous`` - a hinge joint that rotates about the axis with an + unlimited range of motion. + - ``planar`` - a joint that moves in the plane orthogonal to the axis. + - ``floating`` - a joint that can move in 6DoF. + + Parameters + ---------- + name : str + The name of this joint. + parent : str + The name of the parent link of this joint. + child : str + The name of the child link of this joint. + joint_type : str + The type of the joint. Must be one of :obj:`.Joint.TYPES`. + axis : (3,) float, optional + The axis of the joint specified in joint frame. Defaults to + ``[1,0,0]``. + origin : (4,4) float, optional + The pose of the child link with respect to the parent link's frame. + The joint frame is defined to be coincident with the child link's + frame, so this is also the pose of the joint frame with respect to + the parent link's frame. + limit : :class:`.JointLimit`, optional + Limit for the joint. Only required for revolute and prismatic + joints. + dynamics : :class:`.JointDynamics`, optional + Dynamics for the joint. + safety_controller : :class`.SafetyController`, optional + The safety controller for this joint. + calibration : :class:`.JointCalibration`, optional + Calibration information for the joint. + mimic : :class:`JointMimic`, optional + Joint mimicry information. + """ + + TYPES = ["fixed", "prismatic", "revolute", "continuous", "floating", "planar"] + _ATTRIBS = { + "name": (str, True), + } + _ELEMENTS = { + "dynamics": (JointDynamics, False, False), + "limit": (JointLimit, False, False), + "mimic": (JointMimic, False, False), + "safety_controller": (SafetyController, False, False), + "calibration": (JointCalibration, False, False), + } + _TAG = "joint" + + def __init__( + self, + name, + joint_type, + parent, + child, + axis=None, + origin=None, + limit=None, + dynamics=None, + safety_controller=None, + calibration=None, + mimic=None, + ): + self.name = name + self.parent = parent + self.child = child + self.joint_type = joint_type + self.axis = axis + self.origin = origin + self.limit = limit + self.dynamics = dynamics + self.safety_controller = safety_controller + self.calibration = calibration + self.mimic = mimic + + @property + def name(self): + """str : Name for this joint.""" + return self._name + + @name.setter + def name(self, value): + self._name = str(value) + + @property + def joint_type(self): + """str : The type of this joint.""" + return self._joint_type + + @joint_type.setter + def joint_type(self, value): + value = str(value) + if value not in Joint.TYPES: + raise ValueError("Unsupported joint type {}".format(value)) + self._joint_type = value + + @property + def parent(self): + """str : The name of the parent link.""" + return self._parent + + @parent.setter + def parent(self, value): + self._parent = str(value) + + @property + def child(self): + """str : The name of the child link.""" + return self._child + + @child.setter + def child(self, value): + self._child = str(value) + + @property + def axis(self): + """(3,) float : The joint axis in the joint frame.""" + return self._axis + + @axis.setter + def axis(self, value): + if value is None: + value = np.array([1.0, 0.0, 0.0], dtype=np.float64) + elif np.linalg.norm(value) < 1e-4: + value = np.array([1.0, 0.0, 0.0], dtype=np.float64) + else: + value = np.asanyarray(value, dtype=np.float64) + if value.shape != (3,): + raise ValueError("Invalid shape for axis, should be (3,)") + value = value / np.linalg.norm(value) + self._axis = value + + @property + def origin(self): + """(4,4) float : The pose of child and joint frames relative to the + parent link's frame. + """ + return self._origin + + @origin.setter + def origin(self, value): + self._origin = configure_origin(value) + + @property + def limit(self): + """:class:`.JointLimit` : The limits for this joint.""" + return self._limit + + @limit.setter + def limit(self, value): + if value is None: + if self.joint_type in ["prismatic", "revolute"]: + raise ValueError("Require joint limit for prismatic and revolute joints") + elif not isinstance(value, JointLimit): + raise TypeError("Expected JointLimit type") + self._limit = value + + @property + def dynamics(self): + """:class:`.JointDynamics` : The dynamics for this joint.""" + return self._dynamics + + @dynamics.setter + def dynamics(self, value): + if value is not None: + if not isinstance(value, JointDynamics): + raise TypeError("Expected JointDynamics type") + self._dynamics = value + + @property + def safety_controller(self): + """:class:`.SafetyController` : The safety controller for this joint.""" + return self._safety_controller + + @safety_controller.setter + def safety_controller(self, value): + if value is not None: + if not isinstance(value, SafetyController): + raise TypeError("Expected SafetyController type") + self._safety_controller = value + + @property + def calibration(self): + """:class:`.JointCalibration` : The calibration for this joint.""" + return self._calibration + + @calibration.setter + def calibration(self, value): + if value is not None: + if not isinstance(value, JointCalibration): + raise TypeError("Expected JointCalibration type") + self._calibration = value + + @property + def mimic(self): + """:class:`.JointMimic` : The mimic for this joint.""" + return self._mimic + + @mimic.setter + def mimic(self, value): + if value is not None: + if not isinstance(value, JointMimic): + raise TypeError("Expected JointMimic type") + self._mimic = value + + def is_valid(self, cfg: Union[float, npt.ArrayLike]) -> bool: + """Check if the provided configuration value is valid for this joint. + + Parameters + ---------- + cfg : float, (2,) float, (6,) float, or (4,4) float + The configuration of the joint. + + Returns + ------- + is_valid : bool + True if the configuration is valid, and False otherwise. + """ + if self.joint_type not in ["fixed", "revolute"]: + return True + if self.limit is None: + return True + cfg_val: float = float(cfg) # type: ignore[arg-type] + lower = -np.inf + upper = np.inf + if self.limit.lower is not None: + lower = self.limit.lower + if self.limit.upper is not None: + upper = self.limit.upper + return cfg_val >= lower and cfg_val <= upper + + def get_child_pose(self, cfg: Union[float, npt.ArrayLike, None] = None) -> np.ndarray: + """Computes the child pose relative to a parent pose for a given + configuration value. + + Parameters + ---------- + cfg : float, (2,) float, (6,) float, or (4,4) float + The configuration values for this joint. They are interpreted + based on the joint type as follows: + + - ``fixed`` - not used. + - ``prismatic`` - a translation along the axis in meters. + - ``revolute`` - a rotation about the axis in radians. + - ``continuous`` - a rotation about the axis in radians. + - ``planar`` - the x and y translation values in the plane. + - ``floating`` - the xyz values followed by the rpy values, + or a (4,4) matrix. + + If ``cfg`` is ``None``, then this just returns the joint pose. + + Returns + ------- + pose : (4,4) float + The pose of the child relative to the parent. + """ + if cfg is None: + return self.origin + elif self.joint_type == "fixed": + return self.origin + elif self.joint_type in ["revolute", "continuous"]: + if cfg is None: + cfg_val = 0.0 + else: + cfg_val = float(cfg) # type: ignore[arg-type] + R = trimesh.transformations.rotation_matrix(cfg_val, self.axis) + return self.origin.dot(R) + elif self.joint_type == "prismatic": + if cfg is None: + cfg_val = 0.0 + else: + cfg_val = float(cfg) # type: ignore[arg-type] + translation = np.eye(4, dtype=np.float64) + translation[:3, 3] = self.axis * cfg_val + return self.origin.dot(translation) + elif self.joint_type == "planar": + if cfg is None: + cfg = np.zeros(2, dtype=np.float64) + else: + cfg = np.asanyarray(cfg, dtype=np.float64) + if cfg.shape != (2,): + raise ValueError("(2,) float configuration required for planar joints") + translation = np.eye(4, dtype=np.float64) + translation[:3, 3] = self.origin[:3, :2].dot(cfg) + return self.origin.dot(translation) + elif self.joint_type == "floating": + if cfg is None: + cfg = np.zeros(6, dtype=np.float64) + else: + cfg = configure_origin(cfg) + if cfg is None: + raise ValueError("Invalid configuration for floating joint") + return self.origin.dot(cfg) + else: + raise ValueError("Invalid configuration") + + def get_child_poses(self, cfg: Optional[npt.ArrayLike], n_cfgs: int) -> np.ndarray: + """Computes the child pose relative to a parent pose for a given set of + configuration values. + + Parameters + ---------- + cfg : (n,) float or None + The configuration values for this joint. They are interpreted + based on the joint type as follows: + + - ``fixed`` - not used. + - ``prismatic`` - a translation along the axis in meters. + - ``revolute`` - a rotation about the axis in radians. + - ``continuous`` - a rotation about the axis in radians. + - ``planar`` - Not implemented. + - ``floating`` - Not implemented. + + If ``cfg`` is ``None``, then this just returns the joint pose. + + Returns + ------- + poses : (n,4,4) float + The poses of the child relative to the parent. + """ + if cfg is None: + return np.tile(self.origin, (n_cfgs, 1, 1)) + elif self.joint_type == "fixed": + return np.tile(self.origin, (n_cfgs, 1, 1)) + elif self.joint_type in ["revolute", "continuous"]: + if cfg is None: + cfg = np.zeros(n_cfgs) + return np.matmul(self.origin, self._rotation_matrices(cfg, self.axis)) + elif self.joint_type == "prismatic": + if cfg is None: + cfg_arr = np.zeros(n_cfgs, dtype=np.float64) + else: + cfg_arr = np.asanyarray(cfg, dtype=np.float64) + translation = np.tile(np.eye(4), (n_cfgs, 1, 1)) + translation[:, :3, 3] = self.axis * cfg_arr[:, np.newaxis] + return np.matmul(self.origin, translation) + elif self.joint_type == "planar": + raise NotImplementedError() + elif self.joint_type == "floating": + raise NotImplementedError() + else: + raise ValueError("Invalid configuration") + + @classmethod + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None): + kwargs = cls._parse(node, path) + kwargs["joint_type"] = str(node.attrib["type"]) + kwargs["parent"] = node.find("parent").attrib["link"] + kwargs["child"] = node.find("child").attrib["link"] + axis = node.find("axis") + if axis is not None: + axis = np.fromstring(axis.attrib["xyz"], sep=" ") + kwargs["axis"] = axis + kwargs["origin"] = parse_origin(node) + return cls(**kwargs) + + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: + node = self._unparse(path) + parent = ET.Element("parent") + parent.attrib["link"] = self.parent + node.append(parent) + child = ET.Element("child") + child.attrib["link"] = self.child + node.append(child) + if self.axis is not None: + axis = ET.Element("axis") + axis.attrib["xyz"] = np.array2string(self.axis)[1:-1] + node.append(axis) + node.append(unparse_origin(self.origin)) + node.attrib["type"] = self.joint_type + return node + + def _rotation_matrices(self, angles: npt.ArrayLike, axis: npt.ArrayLike) -> np.ndarray: + """Compute rotation matrices from angle/axis representations. + + Parameters + ---------- + angles : (n,) float + The angles. + axis : (3,) float + The axis. + + Returns + ------- + rots : (n,4,4) + The rotation matrices + """ + ang = np.asanyarray(angles, dtype=np.float64) + ax = np.asanyarray(axis, dtype=np.float64) + ax = ax / np.linalg.norm(ax) + sina = np.sin(ang) + cosa = np.cos(ang) + M = np.tile(np.eye(4), (len(ang), 1, 1)) + M[:, 0, 0] = cosa + M[:, 1, 1] = cosa + M[:, 2, 2] = cosa + M[:, :3, :3] += ( + np.tile(np.outer(ax, ax), (len(ang), 1, 1)) * (1.0 - cosa)[:, np.newaxis, np.newaxis] + ) + M[:, :3, :3] += ( + np.tile( + np.array( + [ + [0.0, -ax[2], ax[1]], + [ax[2], 0.0, -ax[0]], + [-ax[1], ax[0], 0.0], + ] + ), + (len(ang), 1, 1), + ) + * sina[:, np.newaxis, np.newaxis] + ) + return M + + def copy(self, prefix="", scale=None): + """Create a deep copy of the joint with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + + Returns + ------- + :class:`.Joint` + A deep copy of the joint. + """ + origin = self.origin.copy() + if scale is not None: + if not isinstance(scale, (list, np.ndarray)): + scale = np.repeat(scale, 3) + origin[:3, 3] *= scale + cpy = self.__class__( + name="{}{}".format(prefix, self.name), + joint_type=self.joint_type, + parent="{}{}".format(prefix, self.parent), + child="{}{}".format(prefix, self.child), + axis=self.axis.copy(), + origin=origin, + limit=(self.limit.copy(prefix, scale) if self.limit else None), + dynamics=(self.dynamics.copy(prefix, scale) if self.dynamics else None), + safety_controller=( + self.safety_controller.copy(prefix, scale) if self.safety_controller else None + ), + calibration=(self.calibration.copy(prefix, scale) if self.calibration else None), + mimic=(self.mimic.copy(prefix=prefix, scale=scale) if self.mimic else None), + ) + return cpy diff --git a/urchin/link.py b/urchin/link.py new file mode 100644 index 0000000..232ec61 --- /dev/null +++ b/urchin/link.py @@ -0,0 +1,1088 @@ +from __future__ import annotations + +import os +from typing import Optional, Sequence, Union + +import numpy as np +import numpy.typing as npt +import trimesh +from lxml import etree as ET + +from urchin.base import URDFType, URDFTypeWithMesh +from urchin.material import Material +from urchin.utils import ( + configure_origin, + get_filename, + load_meshes, + parse_origin, + unparse_origin, +) + + +class Box(URDFType): + _meshes: list[trimesh.Trimesh] + """A rectangular prism whose center is at the local origin. + + Parameters + ---------- + size : (3,) float + The length, width, and height of the box in meters. + """ + + _ATTRIBS = {"size": (np.ndarray, True)} + _TAG = "box" + + def __init__(self, size: npt.ArrayLike): + self.size = size + self._meshes = [] + + @property + def size(self) -> np.ndarray: + """(3,) float : The length, width, and height of the box in meters.""" + return self._size + + @size.setter + def size(self, value: npt.ArrayLike) -> None: + self._size = np.asanyarray(value).astype(np.float64) + self._meshes = [] + + @property + def meshes(self) -> list[trimesh.Trimesh]: + """list of :class:`~trimesh.base.Trimesh` : The triangular meshes + that represent this object. + """ + if len(self._meshes) == 0: + self._meshes = [trimesh.creation.box(extents=self.size)] + return self._meshes + + def copy(self, prefix: str = "", scale: Union[float, Sequence[float], None] = None) -> "Box": + """Create a deep copy with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all names. + scale : float or (3,) float, optional + Uniform or per-axis scale to apply to the box. + + Returns + ------- + :class:`.Box` + A deep copy. + """ + if scale is None: + scale = 1.0 + b = self.__class__( + size=self.size.copy() * scale, + ) + return b + + +class Cylinder(URDFType): + _meshes: list[trimesh.Trimesh] + """A cylinder whose center is at the local origin. + + Parameters + ---------- + radius : float + The radius of the cylinder in meters. + length : float + The length of the cylinder in meters. + """ + + _ATTRIBS = { + "radius": (float, True), + "length": (float, True), + } + _TAG = "cylinder" + + def __init__(self, radius: float, length: float): + self.radius = radius + self.length = length + self._meshes = [] + + @property + def radius(self) -> float: + """float : The radius of the cylinder in meters.""" + return self._radius + + @radius.setter + def radius(self, value: float) -> None: + self._radius = float(value) + self._meshes = [] + + @property + def length(self) -> float: + """float : The length of the cylinder in meters.""" + return self._length + + @length.setter + def length(self, value: float) -> None: + self._length = float(value) + self._meshes = [] + + @property + def meshes(self) -> list[trimesh.Trimesh]: + """list of :class:`~trimesh.base.Trimesh` : The triangular meshes + that represent this object. + """ + if len(self._meshes) == 0: + self._meshes = [trimesh.creation.cylinder(radius=self.radius, height=self.length)] + return self._meshes + + def copy( + self, prefix: str = "", scale: Union[float, Sequence[float], None] = None + ) -> "Cylinder": + """Create a deep copy with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all names. + scale : float or (3,) float, optional + Uniform or per-axis scale to apply. Per-axis must have equal + X/Y values for cylinders. + + Returns + ------- + :class:`.Cylinder` + A deep copy. + """ + if scale is None: + scale = 1.0 + if isinstance(scale, (list, tuple, np.ndarray)): + s = list(scale) + if s[0] != s[1]: + raise ValueError("Cannot rescale cylinder geometry with asymmetry in x/y") + c = self.__class__( + radius=self.radius * s[0], + length=self.length * s[2], + ) + else: + from typing import cast as _cast + + s_val: float = float(_cast(float, scale)) + c = self.__class__( + radius=self.radius * s_val, + length=self.length * s_val, + ) + return c + + +class Sphere(URDFType): + _meshes: list[trimesh.Trimesh] + """A sphere whose center is at the local origin. + + Parameters + ---------- + radius : float + The radius of the sphere in meters. + """ + + _ATTRIBS = { + "radius": (float, True), + } + _TAG = "sphere" + + def __init__(self, radius: float): + self.radius = radius + self._meshes = [] + + @property + def radius(self) -> float: + """float : The radius of the sphere in meters.""" + return self._radius + + @radius.setter + def radius(self, value: float) -> None: + self._radius = float(value) + self._meshes = [] + + @property + def meshes(self) -> list[trimesh.Trimesh]: + """list of :class:`~trimesh.base.Trimesh` : The triangular meshes + that represent this object. + """ + if len(self._meshes) == 0: + if self.radius == 0: + print("[urchin]: radius equal to 0 is not supported, using 1e-5.") + self.radius = 1e-5 + self._meshes = [trimesh.creation.icosphere(radius=self.radius)] + return self._meshes + + def copy(self, prefix: str = "", scale: Union[float, Sequence[float], None] = None) -> "Sphere": + """Create a deep copy with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all names. + scale : float or (3,) float, optional + Uniform scale only. Non-uniform scales are rejected. + + Returns + ------- + :class:`.Sphere` + A deep copy. + """ + if scale is None: + scale = 1.0 + if isinstance(scale, (list, tuple, np.ndarray)): + scale_list = list(scale) + if scale_list[0] != scale_list[1] or scale_list[0] != scale_list[2]: + raise ValueError("Spheres do not support non-uniform scaling!") + scale = scale_list[0] + from typing import cast as _cast + + sf: float = float(_cast(float, scale)) + result = self.__class__( + radius=self.radius * sf, + ) + return result + + +class Mesh(URDFTypeWithMesh): + """A triangular mesh object. + + Parameters + ---------- + filename : str + The path to the mesh that contains this object. This can be + relative to the top-level URDF or an absolute path. + combine : bool + If ``True``, combine geometries into a single mesh (used for + collision geometry). Visual meshes are typically kept separate to + preserve colors and textures. + scale : (3,) float, optional + The scaling value for the mesh along the XYZ axes. + If ``None``, assumes no scale is applied. + meshes : list of :class:`~trimesh.base.Trimesh` or :class:`~trimesh.base.Trimesh` or ``str`` + A list of meshes or a single mesh that composes this mesh. If a + ``str`` is provided, the mesh is loaded from disk. + The list of meshes is useful for visual geometries that + might be composed of separate trimesh objects. + If not specified, the mesh is loaded from the file using trimesh. + """ + + _ATTRIBS = {"filename": (str, True), "scale": (np.ndarray, False)} + _TAG = "mesh" + + def __init__( + self, + filename: str, + combine: bool, + scale: Optional[npt.ArrayLike] = None, + meshes: Union[list[trimesh.Trimesh], trimesh.Trimesh, str, None] = None, + lazy_filename: Optional[str] = None, + ): + if meshes is None: + if lazy_filename is None: + meshes = load_meshes(filename) + else: + meshes = None + self.filename = filename + self.scale = scale + self.lazy_filename = lazy_filename + self.combine = combine + self.meshes = meshes + + @property + def filename(self) -> str: + """str : The path to the mesh file for this object.""" + return self._filename + + @filename.setter + def filename(self, value: str) -> None: + self._filename = value + + @property + def scale(self) -> Optional[np.ndarray]: + """(3,) float : A scaling for the mesh along its local XYZ axes.""" + return self._scale + + @scale.setter + def scale(self, value: Optional[npt.ArrayLike]) -> None: + if value is not None: + value = np.asanyarray(value).astype(np.float64) + self._scale = value + + @property + def meshes(self) -> list[trimesh.Trimesh]: + """list of :class:`~trimesh.base.Trimesh` : The triangular meshes + that represent this object. + """ + if self.lazy_filename is not None and self._meshes is None: + self.meshes = self._load_and_combine_meshes(self.lazy_filename, self.combine) + # At this point meshes should be loaded or assigned + return self._meshes or [] + + @meshes.setter + def meshes( + self, + value: Union[list[trimesh.Trimesh], trimesh.Trimesh, str, None], + ) -> None: + if self.lazy_filename is not None and value is None: + self._meshes = None + elif isinstance(value, str): + value = load_meshes(value) + elif isinstance(value, (list, tuple, set, np.ndarray)): + value = list(value) + if len(value) == 0: + raise ValueError("Mesh must have at least one trimesh.Trimesh") + for m in value: + if not isinstance(m, trimesh.Trimesh): + raise TypeError("Mesh requires a trimesh.Trimesh or a list of them") + elif isinstance(value, trimesh.Trimesh): + value = [value] + else: + raise TypeError("Mesh requires a trimesh.Trimesh") + self._meshes = value + + @classmethod + def _load_and_combine_meshes(cls, fn: str, combine: bool) -> list[trimesh.Trimesh]: + meshes = load_meshes(fn) + if combine: + # Delete visuals for simplicity + for m in meshes: + m.visual = trimesh.visual.ColorVisuals(mesh=m) + merged = meshes[0] + for extra in meshes[1:]: + merged = merged + extra + return [merged] + return meshes + + @classmethod + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None): + # Explicit parse for filename and optional scale + filename_attr = str(node.attrib["filename"]) if "filename" in node.attrib else "" + scale_attr = node.attrib.get("scale") + scale_val = np.fromstring(scale_attr, sep=" ", dtype=np.float64) if scale_attr else None + + # Resolve actual file for loading + fn = get_filename(path, filename_attr) + combine = node.getparent().getparent().tag == Collision._TAG + if not lazy_load_meshes: + meshes = cls._load_and_combine_meshes(fn, combine) + lazy_filename = None + else: + meshes = None + lazy_filename = fn + + return cls( + filename=filename_attr, + combine=combine, + scale=scale_val, + meshes=meshes, + lazy_filename=lazy_filename, + ) + + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: + # Get the filename + fn = get_filename(path, self.filename, makedirs=True) + + # Export the meshes as a single file + if self._meshes is not None: + meshes_list = self.meshes or [] + export_obj: Union[trimesh.Trimesh, trimesh.Scene, list[trimesh.Trimesh]] + if len(meshes_list) == 1: + export_obj = meshes_list[0] + elif os.path.splitext(fn)[1] == ".glb": + export_obj = trimesh.scene.Scene(geometry=meshes_list) + else: + export_obj = meshes_list + trimesh.exchange.export.export_mesh(export_obj, fn) + + # Unparse the node + node = self._unparse(path) + return node + + def copy(self, prefix: str = "", scale: Union[float, Sequence[float], None] = None) -> "Mesh": + """Create a deep copy with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all names. + scale : float or (3,) float, optional + Uniform or per-axis scale applied via a transform. + + Returns + ------- + :class:`.Mesh` + A deep copy. + """ + meshes = [mesh_i.copy() for mesh_i in self.meshes] + if scale is not None: + sm = np.eye(4) + if isinstance(scale, (list, tuple, np.ndarray)): + sm[:3, :3] = np.diag(np.asanyarray(scale, dtype=float)) + else: + from typing import cast as _cast + + sm[:3, :3] = np.diag(np.repeat(_cast(float, scale), 3)) + for mesh_i in meshes: + mesh_i.apply_transform(sm) + base, fn = os.path.split(self.filename) + fn = "{}{}".format(prefix, self.filename) + new_mesh = self.__class__( + filename=os.path.join(base, fn), + combine=self.combine, + scale=(self.scale.copy() if self.scale is not None else None), + meshes=meshes, + lazy_filename=self.lazy_filename, + ) + return new_mesh + + +class Geometry(URDFTypeWithMesh): + """A wrapper for all geometry types. + + Only one of the following values can be set, all others should be set + to ``None``. + + Parameters + ---------- + box : :class:`.Box`, optional + Box geometry. + cylinder : :class:`.Cylinder` + Cylindrical geometry. + sphere : :class:`.Sphere` + Spherical geometry. + mesh : :class:`.Mesh` + Mesh geometry. + """ + + _ELEMENTS = { + "box": (Box, False, False), + "cylinder": (Cylinder, False, False), + "sphere": (Sphere, False, False), + "mesh": (Mesh, False, False), + } + _TAG = "geometry" + + def __init__( + self, + box: Optional[Box] = None, + cylinder: Optional[Cylinder] = None, + sphere: Optional[Sphere] = None, + mesh: Optional[Mesh] = None, + ): + if box is None and cylinder is None and sphere is None and mesh is None: + raise ValueError("At least one geometry element must be set") + self.box = box + self.cylinder = cylinder + self.sphere = sphere + self.mesh = mesh + + @property + def box(self) -> Optional[Box]: + """:class:`.Box` : Box geometry.""" + return self._box + + @box.setter + def box(self, value: Optional[Box]) -> None: + if value is not None and not isinstance(value, Box): + raise TypeError("Expected Box type") + self._box = value + + @property + def cylinder(self) -> Optional[Cylinder]: + """:class:`.Cylinder` : Cylinder geometry.""" + return self._cylinder + + @cylinder.setter + def cylinder(self, value: Optional[Cylinder]) -> None: + if value is not None and not isinstance(value, Cylinder): + raise TypeError("Expected Cylinder type") + self._cylinder = value + + @property + def sphere(self) -> Optional[Sphere]: + """:class:`.Sphere` : Spherical geometry.""" + return self._sphere + + @sphere.setter + def sphere(self, value: Optional[Sphere]) -> None: + if value is not None and not isinstance(value, Sphere): + raise TypeError("Expected Sphere type") + self._sphere = value + + @property + def mesh(self) -> Optional[Mesh]: + """:class:`.Mesh` : Mesh geometry.""" + return self._mesh + + @mesh.setter + def mesh(self, value: Optional[Mesh]) -> None: + if value is not None and not isinstance(value, Mesh): + raise TypeError("Expected Mesh type") + self._mesh = value + + @property + def geometry(self) -> Union[Box, Cylinder, Sphere, Mesh, None]: + """:class:`.Box`, :class:`.Cylinder`, :class:`.Sphere`, or + :class:`.Mesh` : The valid geometry element. + """ + if self.box is not None: + return self.box + if self.cylinder is not None: + return self.cylinder + if self.sphere is not None: + return self.sphere + if self.mesh is not None: + return self.mesh + return None + + @property + def meshes(self) -> list[trimesh.Trimesh]: + """list of :class:`~trimesh.base.Trimesh` : The geometry's triangular + mesh representation(s). + """ + assert self.geometry is not None + return self.geometry.meshes + + def copy( + self, prefix: str = "", scale: Union[float, Sequence[float], None] = None + ) -> "Geometry": + """Create a deep copy with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all names. + scale : float or (3,) float, optional + Uniform or per-axis scale to apply to the underlying geometry. + + Returns + ------- + :class:`.Geometry` + A deep copy. + """ + v = self.__class__( + box=(self.box.copy(prefix=prefix, scale=scale) if self.box else None), + cylinder=(self.cylinder.copy(prefix=prefix, scale=scale) if self.cylinder else None), + sphere=(self.sphere.copy(prefix=prefix, scale=scale) if self.sphere else None), + mesh=(self.mesh.copy(prefix=prefix, scale=scale) if self.mesh else None), + ) + return v + + +class Collision(URDFTypeWithMesh): + """Collision properties of a link. + + Parameters + ---------- + geometry : :class:`.Geometry` + The geometry of the element + name : str, optional + The name of the collision geometry. + origin : (4,4) float, optional + The pose of the collision element relative to the link frame. + Defaults to identity. + """ + + _ATTRIBS = {"name": (str, False)} + _ELEMENTS = { + "geometry": (Geometry, True, False), + } + _TAG = "collision" + + def __init__(self, name: Optional[str], origin: Optional[npt.ArrayLike], geometry: Geometry): + self.geometry = geometry + self.name = name + self.origin = origin + + @property + def geometry(self) -> Geometry: + """:class:`.Geometry` : The geometry of this element.""" + return self._geometry + + @geometry.setter + def geometry(self, value: Geometry) -> None: + if not isinstance(value, Geometry): + raise TypeError("Must set geometry with Geometry object") + self._geometry = value + + @property + def name(self) -> Optional[str]: + """str : The name of this collision element.""" + return self._name + + @name.setter + def name(self, value: Optional[str]) -> None: + if value is not None: + value = str(value) + self._name = value + + @property + def origin(self) -> np.ndarray: + """(4,4) float : The pose of this element relative to the link frame.""" + return self._origin + + @origin.setter + def origin(self, value: Optional[npt.ArrayLike]) -> None: + self._origin = configure_origin(value) + + @classmethod + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None): + name = node.attrib.get("name") + geom_node = node.find("geometry") + if geom_node is None: + raise ValueError("Collision element missing geometry") + geometry = Geometry._from_xml(geom_node, path, lazy_load_meshes) + origin = parse_origin(node) + return cls(name=name, origin=origin, geometry=geometry) + + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: + node = self._unparse(path) + node.append(unparse_origin(self.origin)) + return node + + def copy( + self, prefix: str = "", scale: Union[float, Sequence[float], None] = None + ) -> "Collision": + """Create a deep copy of the visual with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + scale : float or (3,) float, optional + Uniform or per-axis scale applied to the position offset. + + Returns + ------- + :class:`.Visual` + A deep copy of the visual. + """ + origin = self.origin.copy() + if scale is not None: + if not isinstance(scale, (list, tuple, np.ndarray)): + from typing import cast as _cast + + scale_arr = np.repeat(_cast(float, scale), 3) + else: + scale_arr = np.asanyarray(scale, dtype=float) + origin[:3, 3] *= scale_arr + return self.__class__( + name="{}{}".format(prefix, self.name), + origin=origin, + geometry=self.geometry.copy(prefix=prefix, scale=scale), + ) + + +class Visual(URDFTypeWithMesh): + """Visual properties of a link. + + Parameters + ---------- + geometry : :class:`.Geometry` + The geometry of the element + name : str, optional + The name of the visual geometry. + origin : (4,4) float, optional + The pose of the visual element relative to the link frame. + Defaults to identity. + material : :class:`.Material`, optional + The material of the element. + """ + + _ATTRIBS = {"name": (str, False)} + _ELEMENTS = { + "geometry": (Geometry, True, False), + "material": (Material, False, False), + } + _TAG = "visual" + + def __init__( + self, + geometry: Geometry, + name: Optional[str] = None, + origin: Optional[npt.ArrayLike] = None, + material: Optional[Material] = None, + ): + self.geometry = geometry + self.name = name + self.origin = origin + self.material = material + + @property + def geometry(self) -> Geometry: + """:class:`.Geometry` : The geometry of this element.""" + return self._geometry + + @geometry.setter + def geometry(self, value: Geometry) -> None: + if not isinstance(value, Geometry): + raise TypeError("Must set geometry with Geometry object") + self._geometry = value + + @property + def name(self) -> Optional[str]: + """str : The name of this visual element.""" + return self._name + + @name.setter + def name(self, value: Optional[str]) -> None: + if value is not None: + value = str(value) + self._name = value + + @property + def origin(self) -> np.ndarray: + """(4,4) float : The pose of this element relative to the link frame.""" + return self._origin + + @origin.setter + def origin(self, value: Optional[npt.ArrayLike]) -> None: + self._origin = configure_origin(value) + + @property + def material(self) -> Optional[Material]: + """:class:`.Material` : The material for this element.""" + return self._material + + @material.setter + def material(self, value: Optional[Material]) -> None: + if value is not None: + if not isinstance(value, Material): + raise TypeError("Must set material with Material object") + self._material = value + + @classmethod + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None): + geom_node = node.find("geometry") + if geom_node is None: + raise ValueError("Visual element missing geometry") + geometry = Geometry._from_xml(geom_node, path, lazy_load_meshes) + name = node.attrib.get("name") + origin = parse_origin(node) + mat_node = node.find("material") + material = Material._from_xml(mat_node, path) if mat_node is not None else None + return cls(geometry=geometry, name=name, origin=origin, material=material) + + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: + node = self._unparse(path) + node.append(unparse_origin(self.origin)) + return node + + def copy(self, prefix: str = "", scale: Union[float, Sequence[float], None] = None) -> "Visual": + """Create a deep copy of the visual with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + scale : float or (3,) float, optional + Uniform or per-axis scale applied to the position offset. + + Returns + ------- + :class:`.Visual` + A deep copy of the visual. + """ + origin = self.origin.copy() + if scale is not None: + if not isinstance(scale, (list, tuple, np.ndarray)): + from typing import cast as _cast + + scale_arr = np.repeat(_cast(float, scale), 3) + else: + scale_arr = np.asanyarray(scale, dtype=float) + origin[:3, 3] *= scale_arr + return self.__class__( + geometry=self.geometry.copy(prefix=prefix, scale=scale), + name="{}{}".format(prefix, self.name), + origin=origin, + material=(self.material.copy(prefix=prefix) if self.material else None), + ) + + +class Inertial(URDFType): + """The inertial properties of a link. + + Parameters + ---------- + mass : float + The mass of the link in kilograms. + inertia : (3,3) float + The 3x3 symmetric rotational inertia matrix. + origin : (4,4) float, optional + The pose of the inertials relative to the link frame. + Defaults to identity if not specified. + """ + + _TAG = "inertial" + + def __init__(self, mass: float, inertia: npt.ArrayLike, origin: Optional[npt.ArrayLike] = None): + self.mass = mass + self.inertia = inertia + self.origin = origin + + @property + def mass(self) -> float: + """float : The mass of the link in kilograms.""" + return self._mass + + @mass.setter + def mass(self, value: float) -> None: + self._mass = float(value) + + @property + def inertia(self) -> np.ndarray: + """(3,3) float : The 3x3 symmetric rotational inertia matrix.""" + return self._inertia + + @inertia.setter + def inertia(self, value: npt.ArrayLike) -> None: + value = np.asanyarray(value).astype(np.float64) + if not np.allclose(value, value.T): + raise ValueError("Inertia must be a symmetric matrix") + self._inertia = value + + @property + def origin(self) -> np.ndarray: + """(4,4) float : The pose of the inertials relative to the link frame.""" + return self._origin + + @origin.setter + def origin(self, value: Optional[npt.ArrayLike]) -> None: + self._origin = configure_origin(value) + + @classmethod + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None): + origin = parse_origin(node) + mass = float(node.find("mass").attrib["value"]) + n = node.find("inertia") + xx = float(n.attrib["ixx"]) + xy = float(n.attrib["ixy"]) + xz = float(n.attrib["ixz"]) + yy = float(n.attrib["iyy"]) + yz = float(n.attrib["iyz"]) + zz = float(n.attrib["izz"]) + inertia = np.array([[xx, xy, xz], [xy, yy, yz], [xz, yz, zz]], dtype=np.float64) + return cls(mass=mass, inertia=inertia, origin=origin) + + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: + node = ET.Element("inertial") + node.append(unparse_origin(self.origin)) + mass = ET.Element("mass") + mass.attrib["value"] = str(self.mass) + node.append(mass) + inertia = ET.Element("inertia") + inertia.attrib["ixx"] = str(self.inertia[0, 0]) + inertia.attrib["ixy"] = str(self.inertia[0, 1]) + inertia.attrib["ixz"] = str(self.inertia[0, 2]) + inertia.attrib["iyy"] = str(self.inertia[1, 1]) + inertia.attrib["iyz"] = str(self.inertia[1, 2]) + inertia.attrib["izz"] = str(self.inertia[2, 2]) + node.append(inertia) + return node + + def copy( + self, + prefix: str = "", + mass: Optional[float] = None, + origin: Optional[np.ndarray] = None, + inertia: Optional[np.ndarray] = None, + ) -> "Inertial": + """Create a deep copy of the visual with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + + Returns + ------- + :class:`.Inertial` + A deep copy of the visual. + """ + if mass is None: + mass = self.mass + if origin is None: + origin = self.origin.copy() + if inertia is None: + inertia = self.inertia.copy() + return self.__class__( + mass=mass, + inertia=inertia, + origin=origin, + ) + + +class Link(URDFTypeWithMesh): + """A link of a rigid object. + + Parameters + ---------- + name : str + The name of the link. + inertial : :class:`.Inertial`, optional + The inertial properties of the link. + visuals : list of :class:`.Visual`, optional + The visual properties of the link. + collsions : list of :class:`.Collision`, optional + The collision properties of the link. + """ + + _ATTRIBS = { + "name": (str, True), + } + _ELEMENTS = { + "inertial": (Inertial, False, False), + "visuals": (Visual, False, True), + "collisions": (Collision, False, True), + } + _TAG = "link" + + def __init__( + self, + name: str, + inertial: Optional[Inertial], + visuals: Optional[Sequence[Visual]], + collisions: Optional[Sequence[Collision]], + ): + self.name = name + self.inertial = inertial + self.visuals = visuals + self.collisions = collisions + + self._collision_mesh: Optional[trimesh.Trimesh] = None + + @property + def name(self) -> str: + """str : The name of this link.""" + return self._name + + @name.setter + def name(self, value: str) -> None: + self._name = str(value) + + @property + def inertial(self) -> Inertial: + """:class:`.Inertial` : Inertial properties of the link.""" + return self._inertial + + @inertial.setter + def inertial(self, value: Optional[Inertial]) -> None: + if value is not None and not isinstance(value, Inertial): + raise TypeError("Expected Inertial object") + # Set default inertial + if value is None: + value = Inertial(mass=1.0, inertia=np.eye(3)) + self._inertial = value + + @property + def visuals(self) -> list[Visual]: + """list of :class:`.Visual` : The visual properties of this link.""" + return self._visuals + + @visuals.setter + def visuals(self, value: Optional[Sequence[Visual]]) -> None: + if value is None: + value = [] + else: + value = list(value) + for v in value: + if not isinstance(v, Visual): + raise ValueError("Expected list of Visual objects") + self._visuals = value + + @property + def collisions(self) -> list[Collision]: + """list of :class:`.Collision` : The collision properties of this link.""" + return self._collisions + + @collisions.setter + def collisions(self, value: Optional[Sequence[Collision]]) -> None: + if value is None: + value = [] + else: + value = list(value) + for v in value: + if not isinstance(v, Collision): + raise ValueError("Expected list of Collision objects") + self._collisions = value + + @property + def collision_mesh(self) -> Optional[trimesh.Trimesh]: + """:class:`~trimesh.base.Trimesh` : A single collision mesh for + the link, specified in the link frame, or None if there isn't one. + """ + if len(self.collisions) == 0: + return None + if self._collision_mesh is None: + meshes = [] + for c in self.collisions: + for m in c.geometry.meshes: + m = m.copy() + pose = c.origin + if c.geometry.mesh is not None: + if c.geometry.mesh.scale is not None: + S = np.eye(4) + S[:3, :3] = np.diag(c.geometry.mesh.scale) + pose = pose.dot(S) + m.apply_transform(pose) + meshes.append(m) + if len(meshes) == 0: + return None + merged = meshes[0] + for extra in meshes[1:]: + merged = merged + extra + self._collision_mesh = merged + return self._collision_mesh + + def copy( + self, + prefix: str = "", + scale: Union[float, Sequence[float], None] = None, + collision_only: bool = False, + ) -> "Link": + """Create a deep copy of the link. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + scale : float or (3,) float, optional + Uniform or per-axis scale applied to meshes and inertial. + collision_only : bool, optional + If True, only collision geometry is preserved in the copy. + + Returns + ------- + link : :class:`.Link` + A deep copy of the Link. + """ + inertial = self.inertial.copy() if self.inertial is not None else None + cm = self._collision_mesh + if scale is not None: + if self.collision_mesh is not None and self.inertial is not None: + sm = np.eye(4) + if not isinstance(scale, (list, tuple, np.ndarray)): + from typing import cast as _cast + + scale_arr = np.repeat(_cast(float, scale), 3) + else: + scale_arr = np.asanyarray(scale, dtype=float) + sm[:3, :3] = np.diag(scale_arr) + cm = self.collision_mesh.copy() + cm.density = self.inertial.mass / cm.volume + cm.apply_transform(sm) + cmm = np.eye(4) + cmm[:3, 3] = cm.center_mass + inertial = Inertial(mass=float(cm.mass), inertia=cm.moment_inertia, origin=cmm) + + visuals = None + if not collision_only: + visuals = [v.copy(prefix=prefix, scale=scale) for v in self.visuals] + + cpy = self.__class__( + name="{}{}".format(prefix, self.name), + inertial=inertial, + visuals=visuals, + collisions=[v.copy(prefix=prefix, scale=scale) for v in self.collisions], + ) + cpy._collision_mesh = cm + return cpy diff --git a/urchin/material.py b/urchin/material.py new file mode 100644 index 0000000..3342e0f --- /dev/null +++ b/urchin/material.py @@ -0,0 +1,213 @@ +from __future__ import annotations + +from os import PathLike, fspath +from typing import Optional, Union + +import numpy as np +import numpy.typing as npt +import PIL +from lxml import etree as ET + +from urchin.base import URDFType +from urchin.utils import get_filename + + +class Texture(URDFType): + """An image-based texture. + + Parameters + ---------- + filename : str + The path to the image that contains this texture. This can be + relative to the top-level URDF or an absolute path. + image : :class:`PIL.Image.Image` or ``numpy.ndarray`` or ``str``, optional + The image for the texture. If a ``str`` path or a numpy array is + provided, it is converted to a PIL image. If not specified, it is + loaded automatically from ``filename``. + """ + + _ATTRIBS = {"filename": (str, True)} + _TAG = "texture" + + def __init__( + self, + filename: str, + image: Union[PIL.Image.Image, str, np.ndarray, None] = None, + ): + if image is None: + image = PIL.Image.open(filename) + self.filename = filename + self.image = image + + @property + def filename(self) -> str: + """str : Path to the image for this texture.""" + return self._filename + + @filename.setter + def filename(self, value: Union[str, PathLike[str]]) -> None: + self._filename = fspath(value) + + @property + def image(self) -> PIL.Image.Image: + """:class:`PIL.Image.Image` : The image for this texture.""" + return self._image + + @image.setter + def image(self, value: Union[PIL.Image.Image, str, np.ndarray]) -> None: + if isinstance(value, str): + value = PIL.Image.open(value) + if isinstance(value, np.ndarray): + value = PIL.Image.fromarray(value) + elif not isinstance(value, PIL.Image.Image): + raise ValueError("Texture only supports numpy arrays or PIL images") + self._image = value + + @classmethod + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None): + # Explicitly parse fields for typing clarity + filename = str(node.attrib["filename"]) if "filename" in node.attrib else "" + fn = get_filename(path, filename) + image = PIL.Image.open(fn) + return cls(filename=filename, image=image) + + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: + # Save the image + filepath = get_filename(path, self.filename, makedirs=True) + self.image.save(filepath) + + return self._unparse(path) + + def copy(self, prefix: str = "", scale: Union[float, np.ndarray, None] = None) -> "Texture": + """Create a deep copy with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all names. + + Returns + ------- + :class:`.Texture` + A deep copy. + """ + v = self.__class__(filename=self.filename, image=self.image.copy()) + return v + + +class Material(URDFType): + """A material for some geometry. + + Parameters + ---------- + name : str + The name of the material. + color : (4,) float, optional + The RGBA color of the material in the range [0,1]. + texture : :class:`.Texture`, optional + A texture for the material. + """ + + _ATTRIBS = {"name": (str, True)} + _ELEMENTS = { + "texture": (Texture, False, False), + } + _TAG = "material" + + def __init__( + self, + name: str, + color: Optional[npt.ArrayLike] = None, + texture: Union[Texture, str, None] = None, + ): + self.name = name + self.color = color + self.texture = texture + + @property + def name(self) -> str: + """str : The name of the material.""" + return self._name + + @name.setter + def name(self, value: str) -> None: + self._name = str(value) + + @property + def color(self) -> Optional[np.ndarray]: + """(4,) float : The RGBA color of the material, in the range [0,1].""" + return self._color + + @color.setter + def color(self, value: Optional[npt.ArrayLike]) -> None: + if value is not None: + value = np.asanyarray(value).astype(float) + value = np.clip(value, 0.0, 1.0) + if value.shape != (4,): + raise ValueError("Color must be a (4,) float") + self._color = value + + @property + def texture(self) -> Optional[Texture]: + """:class:`.Texture` : The texture for the material.""" + return self._texture + + @texture.setter + def texture(self, value: Union[Texture, str, None]) -> None: + if value is not None: + if isinstance(value, str): + image = PIL.Image.open(value) + value = Texture(filename=value, image=image) + elif not isinstance(value, Texture): + raise ValueError("Invalid type for texture -- expect path to image or Texture") + self._texture = value + + @classmethod + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None): + name = str(node.attrib["name"]) if "name" in node.attrib else "" + color_arr = None + color_node = node.find("color") + if color_node is not None and "rgba" in color_node.attrib: + color_arr = np.fromstring(color_node.attrib["rgba"], sep=" ", dtype=np.float64) + texture_node = node.find("texture") + texture = Texture._from_xml(texture_node, path) if texture_node is not None else None + return cls(name=name, color=color_arr, texture=texture) + + def _to_xml(self, parent: ET._Element, path: str) -> ET._Element: + # Simplify materials by collecting them at the top level. + + # For top-level elements, save the full material specification + if parent.tag == "robot": + node = self._unparse(path) + if self.color is not None: + color = ET.Element("color") + color.attrib["rgba"] = np.array2string(self.color)[1:-1] + node.append(color) + + else: + node = ET.Element("material") + node.attrib["name"] = self.name + if self.color is not None: + color = ET.Element("color") + color.attrib["rgba"] = np.array2string(self.color)[1:-1] + node.append(color) + return node + + def copy(self, prefix: str = "", scale: Union[float, np.ndarray, None] = None) -> "Material": + """Create a deep copy of the material with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + + Returns + ------- + :class:`.Material` + A deep copy of the material. + """ + return self.__class__( + name="{}{}".format(prefix, self.name), + color=self.color, + texture=self.texture, + ) diff --git a/urchin/transmission.py b/urchin/transmission.py new file mode 100644 index 0000000..b13f66c --- /dev/null +++ b/urchin/transmission.py @@ -0,0 +1,326 @@ +from __future__ import annotations + +from typing import Optional, Sequence, Union + +from lxml import etree as ET + +from urchin.base import URDFType + + +class Actuator(URDFType): + """An actuator. + + Parameters + ---------- + name : str + The name of this actuator. + mechanicalReduction : float, optional + Mechanical reduction (ratio) at the joint/actuator transmission. + hardwareInterfaces : list of str, optional + The supported hardware interfaces to the actuator. + """ + + _ATTRIBS = { + "name": (str, True), + } + _TAG = "actuator" + + def __init__( + self, + name: str, + mechanicalReduction: Optional[float] = None, + hardwareInterfaces: Optional[Sequence[str]] = None, + ): + self.name = name + self.mechanicalReduction = mechanicalReduction + self.hardwareInterfaces = hardwareInterfaces + + @property + def name(self) -> str: + """str : The name of this actuator.""" + return self._name + + @name.setter + def name(self, value: str) -> None: + self._name = str(value) + + @property + def mechanicalReduction(self) -> Optional[float]: + """float | None : Mechanical reduction (ratio).""" + return self._mechanicalReduction + + @mechanicalReduction.setter + def mechanicalReduction(self, value: Union[float, str, None]) -> None: + if value is not None: + value = float(value) + self._mechanicalReduction = value + + @property + def hardwareInterfaces(self) -> list[str]: + """list of str : The supported hardware interfaces.""" + return self._hardwareInterfaces + + @hardwareInterfaces.setter + def hardwareInterfaces(self, value: Optional[Sequence[str]]) -> None: + if value is None: + value = [] + else: + value = list(value) + for i, v in enumerate(value): + value[i] = str(v) + self._hardwareInterfaces = value + + @classmethod + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None): + name = str(node.attrib["name"]) if "name" in node.attrib else "" + mr_node = node.find("mechanicalReduction") + mr_val = float(mr_node.text) if mr_node is not None and mr_node.text else None + hi_nodes = node.findall("hardwareInterface") + hi_list = [str(h.text) for h in hi_nodes if h is not None and h.text] + return cls(name=name, mechanicalReduction=mr_val, hardwareInterfaces=hi_list) + + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: + node = self._unparse(path) + if self.mechanicalReduction is not None: + mr = ET.Element("mechanicalReduction") + mr.text = str(self.mechanicalReduction) + node.append(mr) + if len(self.hardwareInterfaces) > 0: + for hi in self.hardwareInterfaces: + h = ET.Element("hardwareInterface") + h.text = hi + node.append(h) + return node + + def copy(self, prefix: str = "", scale: Optional[float] = None) -> "Actuator": + """Create a deep copy with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all joint and link names. + + Returns + ------- + :class:`.Actuator` + A deep copy of the visual. + """ + return self.__class__( + name="{}{}".format(prefix, self.name), + mechanicalReduction=self.mechanicalReduction, + hardwareInterfaces=self.hardwareInterfaces.copy(), + ) + + +class TransmissionJoint(URDFType): + """A transmission joint specification. + + Parameters + ---------- + name : str + The name of this actuator. + hardwareInterfaces : list of str, optional + The supported hardware interfaces to the actuator. + """ + + _ATTRIBS = { + "name": (str, True), + } + _TAG = "joint" + + def __init__(self, name: str, hardwareInterfaces: Optional[Sequence[str]]): + self.name = name + self.hardwareInterfaces = hardwareInterfaces + + @property + def name(self) -> str: + """str : The name of this transmission joint.""" + return self._name + + @name.setter + def name(self, value: str) -> None: + self._name = str(value) + + @property + def hardwareInterfaces(self) -> list[str]: + """list of str : The supported hardware interfaces.""" + return self._hardwareInterfaces + + @hardwareInterfaces.setter + def hardwareInterfaces(self, value: Optional[Sequence[str]]) -> None: + if value is None: + value = [] + else: + value = list(value) + for i, v in enumerate(value): + value[i] = str(v) + self._hardwareInterfaces = value + + @classmethod + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None): + name = str(node.attrib["name"]) if "name" in node.attrib else "" + hi_nodes = node.findall("hardwareInterface") + hi_list = [str(h.text) for h in hi_nodes if h is not None and h.text] + return cls(name=name, hardwareInterfaces=hi_list) + + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: + node = self._unparse(path) + if len(self.hardwareInterfaces) > 0: + for hi in self.hardwareInterfaces: + h = ET.Element("hardwareInterface") + h.text = hi + node.append(h) + return node + + def copy(self, prefix: str = "", scale: Optional[float] = None) -> "TransmissionJoint": + """Create a deep copy with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all names. + + Returns + ------- + :class:`.TransmissionJoint` + A deep copy. + """ + return self.__class__( + name="{}{}".format(prefix, self.name), + hardwareInterfaces=self.hardwareInterfaces.copy(), + ) + + +############################################################################### +# Top-level types +############################################################################### + + +class Transmission(URDFType): + """An element that describes the relationship between an actuator and a + joint. + + Parameters + ---------- + name : str + The name of this transmission. + trans_type : str + The type of this transmission. + joints : list of :class:`.TransmissionJoint` + The joints connected to this transmission. + actuators : list of :class:`.Actuator` + The actuators connected to this transmission. + """ + + _ATTRIBS = { + "name": (str, True), + } + _ELEMENTS = { + "joints": (TransmissionJoint, True, True), + "actuators": (Actuator, True, True), + } + _TAG = "transmission" + + def __init__( + self, + name: str, + trans_type: str, + joints: Optional[Sequence["TransmissionJoint"]] = None, + actuators: Optional[Sequence[Actuator]] = None, + ): + self.name = name + self.trans_type = trans_type + self.joints = joints + self.actuators = actuators + + @property + def name(self) -> str: + """str : The name of this transmission.""" + return self._name + + @name.setter + def name(self, value: str) -> None: + self._name = str(value) + + @property + def trans_type(self) -> str: + """str : The type of this transmission.""" + return self._trans_type + + @trans_type.setter + def trans_type(self, value: str) -> None: + self._trans_type = str(value) + + @property + def joints(self) -> list["TransmissionJoint"]: + """:class:`.TransmissionJoint` : The joints the transmission is + connected to. + """ + return self._joints + + @joints.setter + def joints(self, value: Optional[Sequence["TransmissionJoint"]]) -> None: + if value is None: + value = [] + else: + value = list(value) + for v in value: + if not isinstance(v, TransmissionJoint): + raise TypeError("Joints expects a list of TransmissionJoint") + self._joints = value + + @property + def actuators(self) -> list[Actuator]: + """:class:`.Actuator` : The actuators the transmission is connected to.""" + return self._actuators + + @actuators.setter + def actuators(self, value: Optional[Sequence[Actuator]]) -> None: + if value is None: + value = [] + else: + value = list(value) + for v in value: + if not isinstance(v, Actuator): + raise TypeError("Actuators expects a list of Actuator") + self._actuators = value + + @classmethod + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None): + name = str(node.attrib["name"]) if "name" in node.attrib else "" + ttype = node.attrib.get("type") + if ttype is None: + t_node = node.find("type") + ttype = t_node.text if t_node is not None else "" + joints = [TransmissionJoint._from_xml(n, path) for n in node.findall("joint")] + actuators = [Actuator._from_xml(n, path) for n in node.findall("actuator")] + return cls(name=name, trans_type=str(ttype), joints=joints, actuators=actuators) + + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: + node = self._unparse(path) + ttype = ET.Element("type") + ttype.text = self.trans_type + node.append(ttype) + return node + + def copy( + self, prefix: str = "", scale: Union[float, Sequence[float], None] = None + ) -> "Transmission": + """Create a deep copy with the prefix applied to all names. + + Parameters + ---------- + prefix : str + A prefix to apply to all names. + + Returns + ------- + :class:`.Transmission` + A deep copy. + """ + return self.__class__( + name="{}{}".format(prefix, self.name), + trans_type=self.trans_type, + joints=[j.copy(prefix) for j in self.joints], + actuators=[a.copy(prefix) for a in self.actuators], + ) diff --git a/urchin/urdf.py b/urchin/urdf.py index 80e8a05..a7625c7 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -1,2736 +1,21 @@ -import copy -import os -import time -from collections import OrderedDict - -import networkx as nx -import numpy as np -import PIL -import six -import trimesh -from lxml import etree as ET - -from .utils import ( - configure_origin, - get_filename, - load_meshes, - parse_origin, - unparse_origin, -) - - -class URDFType(object): - """Abstract base class for all URDF types. - - This has useful class methods for automatic parsing/unparsing - of XML trees. - - There are three overridable class variables: - - - ``_ATTRIBS`` - This is a dictionary mapping attribute names to a tuple, - ``(type, required)`` where ``type`` is the Python type for the - attribute and ``required`` is a boolean stating whether the attribute - is required to be present. - - ``_ELEMENTS`` - This is a dictionary mapping element names to a tuple, - ``(type, required, multiple)`` where ``type`` is the Python type for the - element, ``required`` is a boolean stating whether the element - is required to be present, and ``multiple`` is a boolean indicating - whether multiple elements of this type could be present. - Elements are child nodes in the XML tree, and their type must be a - subclass of :class:`.URDFType`. - - ``_TAG`` - This is a string that represents the XML tag for the node - containing this type of object. - """ - - _ATTRIBS = {} # Map from attrib name to (type, required) - _ELEMENTS = {} # Map from element name to (type, required, multiple) - _TAG = "" # XML tag for this element - - def __init__(self): - pass - - @classmethod - def _parse_attrib(cls, val_type, val): - """Parse an XML attribute into a python value. - - Parameters - ---------- - val_type : :class:`type` - The type of value to create. - val : :class:`object` - The value to parse. - - Returns - ------- - val : :class:`object` - The parsed attribute. - """ - if val_type == np.ndarray: - val = np.fromstring(val, sep=" ") - else: - val = val_type(val) - return val - - @classmethod - def _parse_simple_attribs(cls, node): - """Parse all attributes in the _ATTRIBS array for this class. - - Parameters - ---------- - node : :class:`lxml.etree.Element` - The node to parse attributes for. - - Returns - ------- - kwargs : dict - Map from attribute name to value. If the attribute is not - required and is not present, that attribute's name will map to - ``None``. - """ - kwargs = {} - for a in cls._ATTRIBS: - t, r = cls._ATTRIBS[a] # t = type, r = required (bool) - if r: - try: - v = cls._parse_attrib(t, node.attrib[a]) - except Exception: - raise ValueError( - "Missing required attribute {} when parsing an object " - "of type {}".format(a, cls.__name__) - ) - else: - v = None - if a in node.attrib: - v = cls._parse_attrib(t, node.attrib[a]) - kwargs[a] = v - return kwargs - - @classmethod - def _parse_simple_elements(cls, node, path): - """Parse all elements in the _ELEMENTS array from the children of - this node. - - Parameters - ---------- - node : :class:`lxml.etree.Element` - The node to parse children for. - path : str - The string path where the XML file is located (used for resolving - the location of mesh or image files). - lazy_load_meshes : bool - Whether a mesh element should be immediately loaded or loaded when - needed - - Returns - ------- - kwargs : dict - Map from element names to the :class:`URDFType` subclass (or list, - if ``multiple`` was set) created for that element. - """ - kwargs = {} - for a in cls._ELEMENTS: - t, r, m = cls._ELEMENTS[a] - if not m: - v = node.find(t._TAG) - if r or v is not None: - v = t._from_xml(v, path) - else: - vs = node.findall(t._TAG) - if len(vs) == 0 and r: - print( - f"Missing required subelement(s) of type {t.__name__} when " - f"parsing an object of type {cls.__name__}." - ) - v = [t._from_xml(n, path) for n in vs] - kwargs[a] = v - return kwargs - - @classmethod - def _parse(cls, node, path): - """Parse all elements and attributes in the _ELEMENTS and _ATTRIBS - arrays for a node. - - Parameters - ---------- - node : :class:`lxml.etree.Element` - The node to parse. - path : str - The string path where the XML file is located (used for resolving - the location of mesh or image files). - - Returns - ------- - kwargs : dict - Map from names to Python classes created from the attributes - and elements in the class arrays. - """ - kwargs = cls._parse_simple_attribs(node) - kwargs.update(cls._parse_simple_elements(node, path)) - return kwargs - - @classmethod - def _from_xml(cls, node, path): - """Create an instance of this class from an XML node. - - Parameters - ---------- - node : :class:`lxml.etree.Element` - The node to parse. - path : str - The string path where the XML file is located (used for resolving - the location of mesh or image files). - - Returns - ------- - obj : :class:`URDFType` - An instance of this class parsed from the node. - """ - return cls(**cls._parse(node, path)) - - def _unparse_attrib(self, val_type, val): - """Convert a Python value into a string for storage in an - XML attribute. - - Parameters - ---------- - val_type : :class:`type` - The type of the Python object. - val : :class:`object` - The actual value. - - Returns - ------- - s : str - The attribute string. - """ - if val_type == np.ndarray: - val = np.array2string(val)[1:-1] - else: - val = str(val) - return val - - def _unparse_simple_attribs(self, node): - """Convert all Python types from the _ATTRIBS array back into attributes - for an XML node. - - Parameters - ---------- - node : :class:`object` - The XML node to add the attributes to. - """ - for a in self._ATTRIBS: - t, r = self._ATTRIBS[a] - v = getattr(self, a, None) - if r or v is not None: - node.attrib[a] = self._unparse_attrib(t, v) - - def _unparse_simple_elements(self, node, path): - """Unparse all Python types from the _ELEMENTS array back into child - nodes of an XML node. - - Parameters - ---------- - node : :class:`object` - The XML node for this object. Elements will be added as children - of this node. - path : str - The string path where the XML file is being written to (used for - writing out meshes and image files). - """ - for a in self._ELEMENTS: - t, r, m = self._ELEMENTS[a] - v = getattr(self, a, None) - if not m: - if r or v is not None: - node.append(v._to_xml(node, path)) - else: - vs = v - for v in vs: - node.append(v._to_xml(node, path)) - - def _unparse(self, path): - """Create a node for this object and unparse all elements and - attributes in the class arrays. - - Parameters - ---------- - path : str - The string path where the XML file is being written to (used for - writing out meshes and image files). - - Returns - ------- - node : :class:`lxml.etree.Element` - The newly-created node. - """ - node = ET.Element(self._TAG) - self._unparse_simple_attribs(node) - self._unparse_simple_elements(node, path) - return node - - def _to_xml(self, parent, path): - """Create and return an XML node for this object. - - Parameters - ---------- - parent : :class:`lxml.etree.Element` - The parent node that this element will eventually be added to. - This base implementation doesn't use this information, but - classes that override this function may use it. - path : str - The string path where the XML file is being written to (used for - writing out meshes and image files). - - Returns - ------- - node : :class:`lxml.etree.Element` - The newly-created node. - """ - return self._unparse(path) - - -class URDFTypeWithMesh(URDFType): - @classmethod - def _parse_simple_elements(cls, node, path, lazy_load_meshes): - """Parse all elements in the _ELEMENTS array from the children of - this node. - - Parameters - ---------- - node : :class:`lxml.etree.Element` - The node to parse children for. - path : str - The string path where the XML file is located (used for resolving - the location of mesh or image files). - lazy_load_meshes : bool - Whether a mesh element should be immediately loaded or loaded when - needed - - Returns - ------- - kwargs : dict - Map from element names to the :class:`URDFType` subclass (or list, - if ``multiple`` was set) created for that element. - """ - kwargs = {} - for a in cls._ELEMENTS: - t, r, m = cls._ELEMENTS[a] - if not m: - v = node.find(t._TAG) - if r or v is not None: - if issubclass(t, URDFTypeWithMesh): - v = t._from_xml(v, path, lazy_load_meshes) - else: - v = t._from_xml(v, path) - else: - vs = node.findall(t._TAG) - if len(vs) == 0 and r: - raise ValueError( - "Missing required subelement(s) of type {} when " - "parsing an object of type {}".format(t.__name__, cls.__name__) - ) - if issubclass(t, URDFTypeWithMesh): - v = [t._from_xml(n, path, lazy_load_meshes) for n in vs] - else: - v = [t._from_xml(n, path) for n in vs] - kwargs[a] = v - return kwargs - - @classmethod - def _parse(cls, node, path, lazy_load_meshes): - """Parse all elements and attributes in the _ELEMENTS and _ATTRIBS - arrays for a node. - - Parameters - ---------- - node : :class:`lxml.etree.Element` - The node to parse. - path : str - The string path where the XML file is located (used for resolving - the location of mesh or image files). - lazy_load_meshes : bool - Whether meshes should be loaded immediately or upon their first use - - Returns - ------- - kwargs : dict - Map from names to Python classes created from the attributes - and elements in the class arrays. - """ - kwargs = cls._parse_simple_attribs(node) - kwargs.update(cls._parse_simple_elements(node, path, lazy_load_meshes)) - return kwargs - - @classmethod - def _from_xml(cls, node, path, lazy_load_meshes): - """Create an instance of this class from an XML node. - - Parameters - ---------- - node : :class:`lxml.etree.Element` - The node to parse. - path : str - The string path where the XML file is located (used for resolving - the location of mesh or image files). - lazy_load_meshes : bool - Whether meshes should be loaded immediately or upon their first use - - Returns - ------- - obj : :class:`URDFType` - An instance of this class parsed from the node. - """ - return cls(**cls._parse(node, path, lazy_load_meshes)) - - -############################################################################### -# Link types -############################################################################### - - -class Box(URDFType): - """A rectangular prism whose center is at the local origin. - - Parameters - ---------- - size : (3,) float - The length, width, and height of the box in meters. - """ - - _ATTRIBS = {"size": (np.ndarray, True)} - _TAG = "box" - - def __init__(self, size): - self.size = size - self._meshes = [] - - @property - def size(self): - """(3,) float : The length, width, and height of the box in meters.""" - return self._size - - @size.setter - def size(self, value): - self._size = np.asanyarray(value).astype(np.float64) - self._meshes = [] - - @property - def meshes(self): - """list of :class:`~trimesh.base.Trimesh` : The triangular meshes - that represent this object. - """ - if len(self._meshes) == 0: - self._meshes = [trimesh.creation.box(extents=self.size)] - return self._meshes - - def copy(self, prefix="", scale=None): - """Create a deep copy with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all names. - - Returns - ------- - :class:`.Box` - A deep copy. - """ - if scale is None: - scale = 1.0 - b = self.__class__( - size=self.size.copy() * scale, - ) - return b - - -class Cylinder(URDFType): - """A cylinder whose center is at the local origin. - - Parameters - ---------- - radius : float - The radius of the cylinder in meters. - length : float - The length of the cylinder in meters. - """ - - _ATTRIBS = { - "radius": (float, True), - "length": (float, True), - } - _TAG = "cylinder" - - def __init__(self, radius, length): - self.radius = radius - self.length = length - self._meshes = [] - - @property - def radius(self): - """float : The radius of the cylinder in meters.""" - return self._radius - - @radius.setter - def radius(self, value): - self._radius = float(value) - self._meshes = [] - - @property - def length(self): - """float : The length of the cylinder in meters.""" - return self._length - - @length.setter - def length(self, value): - self._length = float(value) - self._meshes = [] - - @property - def meshes(self): - """list of :class:`~trimesh.base.Trimesh` : The triangular meshes - that represent this object. - """ - if len(self._meshes) == 0: - self._meshes = [ - trimesh.creation.cylinder(radius=self.radius, height=self.length) - ] - return self._meshes - - def copy(self, prefix="", scale=None): - """Create a deep copy with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all names. - - Returns - ------- - :class:`.Cylinder` - A deep copy. - """ - if scale is None: - scale = 1.0 - if isinstance(scale, (list, np.ndarray)): - if scale[0] != scale[1]: - raise ValueError( - "Cannot rescale cylinder geometry with asymmetry in x/y" - ) - c = self.__class__( - radius=self.radius * scale[0], - length=self.length * scale[2], - ) - else: - c = self.__class__( - radius=self.radius * scale, - length=self.length * scale, - ) - return c - - -class Sphere(URDFType): - """A sphere whose center is at the local origin. - - Parameters - ---------- - radius : float - The radius of the sphere in meters. - """ - - _ATTRIBS = { - "radius": (float, True), - } - _TAG = "sphere" - - def __init__(self, radius): - self.radius = radius - self._meshes = [] - - @property - def radius(self): - """float : The radius of the sphere in meters.""" - return self._radius - - @radius.setter - def radius(self, value): - self._radius = float(value) - self._meshes = [] - - @property - def meshes(self): - """list of :class:`~trimesh.base.Trimesh` : The triangular meshes - that represent this object. - """ - if len(self._meshes) == 0: - if self.radius == 0: - print("[urchin]: radius equal to 0 is not supported, using 1e-5.") - self.radius = 1e-5 - self._meshes = [trimesh.creation.icosphere(radius=self.radius)] - return self._meshes - - def copy(self, prefix="", scale=None): - """Create a deep copy with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all names. - - Returns - ------- - :class:`.Sphere` - A deep copy. - """ - if scale is None: - scale = 1.0 - if isinstance(scale, (list, np.ndarray)): - if scale[0] != scale[1] or scale[0] != scale[2]: - raise ValueError("Spheres do not support non-uniform scaling!") - scale = scale[0] - s = self.__class__( - radius=self.radius * scale, - ) - return s - - -class Mesh(URDFTypeWithMesh): - """A triangular mesh object. - - Parameters - ---------- - filename : str - The path to the mesh that contains this object. This can be - relative to the top-level URDF or an absolute path. - scale : (3,) float, optional - The scaling value for the mesh along the XYZ axes. - If ``None``, assumes no scale is applied. - meshes : list of :class:`~trimesh.base.Trimesh` - A list of meshes that compose this mesh. - The list of meshes is useful for visual geometries that - might be composed of separate trimesh objects. - If not specified, the mesh is loaded from the file using trimesh. - """ - - _ATTRIBS = {"filename": (str, True), "scale": (np.ndarray, False)} - _TAG = "mesh" - - def __init__(self, filename, combine, scale=None, meshes=None, lazy_filename=None): - if meshes is None: - if lazy_filename is None: - meshes = load_meshes(filename) - else: - meshes = None - self.filename = filename - self.scale = scale - self.lazy_filename = lazy_filename - self.combine = combine - self.meshes = meshes - - @property - def filename(self): - """str : The path to the mesh file for this object.""" - return self._filename - - @filename.setter - def filename(self, value): - self._filename = value - - @property - def scale(self): - """(3,) float : A scaling for the mesh along its local XYZ axes.""" - return self._scale - - @scale.setter - def scale(self, value): - if value is not None: - value = np.asanyarray(value).astype(np.float64) - self._scale = value - - @property - def meshes(self): - """list of :class:`~trimesh.base.Trimesh` : The triangular meshes - that represent this object. - """ - if self.lazy_filename is not None and self._meshes is None: - self.meshes = self._load_and_combine_meshes( - self.lazy_filename, self.combine - ) - return self._meshes - - @meshes.setter - def meshes(self, value): - if self.lazy_filename is not None and value is None: - self._meshes = None - elif isinstance(value, six.string_types): - value = load_meshes(value) - elif isinstance(value, (list, tuple, set, np.ndarray)): - value = list(value) - if len(value) == 0: - raise ValueError("Mesh must have at least one trimesh.Trimesh") - for m in value: - if not isinstance(m, trimesh.Trimesh): - raise TypeError( - "Mesh requires a trimesh.Trimesh or a " "list of them" - ) - elif isinstance(value, trimesh.Trimesh): - value = [value] - else: - raise TypeError("Mesh requires a trimesh.Trimesh") - self._meshes = value - - @classmethod - def _load_and_combine_meshes(cls, fn, combine): - meshes = load_meshes(fn) - if combine: - # Delete visuals for simplicity - for m in meshes: - m.visual = trimesh.visual.ColorVisuals(mesh=m) - meshes = [meshes[0] + meshes[1:]] - return meshes - - @classmethod - def _from_xml(cls, node, path, lazy_load_meshes): - kwargs = cls._parse(node, path, lazy_load_meshes) - - # Load the mesh, combining collision geometry meshes but keeping - # visual ones separate to preserve colors and textures - fn = get_filename(path, kwargs["filename"]) - combine = node.getparent().getparent().tag == Collision._TAG - if not lazy_load_meshes: - meshes = cls._load_and_combine_meshes(fn, combine) - kwargs["lazy_filename"] = None - else: - meshes = None - kwargs["lazy_filename"] = fn - kwargs["meshes"] = meshes - kwargs["combine"] = combine - - return cls(**kwargs) - - def _to_xml(self, parent, path): - # Get the filename - fn = get_filename(path, self.filename, makedirs=True) - - # Export the meshes as a single file - if self._meshes is not None: - meshes = self.meshes - if len(meshes) == 1: - meshes = meshes[0] - elif os.path.splitext(fn)[1] == ".glb": - meshes = trimesh.scene.Scene(geometry=meshes) - trimesh.exchange.export.export_mesh(meshes, fn) - - # Unparse the node - node = self._unparse(path) - return node - - def copy(self, prefix="", scale=None): - """Create a deep copy with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all names. - - Returns - ------- - :class:`.Mesh` - A deep copy. - """ - meshes = [m.copy() for m in self.meshes] - if scale is not None: - sm = np.eye(4) - if isinstance(scale, (list, np.ndarray)): - sm[:3, :3] = np.diag(scale) - else: - sm[:3, :3] = np.diag(np.repeat(scale, 3)) - for i, m in enumerate(meshes): - meshes[i] = m.apply_transform(sm) - base, fn = os.path.split(self.filename) - fn = "{}{}".format(prefix, self.filename) - m = self.__class__( - filename=os.path.join(base, fn), - combine=self.combine, - scale=(self.scale.copy() if self.scale is not None else None), - meshes=meshes, - lazy_filename=self.lazy_filename, - ) - return m - - -class Geometry(URDFTypeWithMesh): - """A wrapper for all geometry types. - - Only one of the following values can be set, all others should be set - to ``None``. - - Parameters - ---------- - box : :class:`.Box`, optional - Box geometry. - cylinder : :class:`.Cylinder` - Cylindrical geometry. - sphere : :class:`.Sphere` - Spherical geometry. - mesh : :class:`.Mesh` - Mesh geometry. - """ - - _ELEMENTS = { - "box": (Box, False, False), - "cylinder": (Cylinder, False, False), - "sphere": (Sphere, False, False), - "mesh": (Mesh, False, False), - } - _TAG = "geometry" - - def __init__(self, box=None, cylinder=None, sphere=None, mesh=None): - if box is None and cylinder is None and sphere is None and mesh is None: - raise ValueError("At least one geometry element must be set") - self.box = box - self.cylinder = cylinder - self.sphere = sphere - self.mesh = mesh - - @property - def box(self): - """:class:`.Box` : Box geometry.""" - return self._box - - @box.setter - def box(self, value): - if value is not None and not isinstance(value, Box): - raise TypeError("Expected Box type") - self._box = value - - @property - def cylinder(self): - """:class:`.Cylinder` : Cylinder geometry.""" - return self._cylinder - - @cylinder.setter - def cylinder(self, value): - if value is not None and not isinstance(value, Cylinder): - raise TypeError("Expected Cylinder type") - self._cylinder = value - - @property - def sphere(self): - """:class:`.Sphere` : Spherical geometry.""" - return self._sphere - - @sphere.setter - def sphere(self, value): - if value is not None and not isinstance(value, Sphere): - raise TypeError("Expected Sphere type") - self._sphere = value - - @property - def mesh(self): - """:class:`.Mesh` : Mesh geometry.""" - return self._mesh - - @mesh.setter - def mesh(self, value): - if value is not None and not isinstance(value, Mesh): - raise TypeError("Expected Mesh type") - self._mesh = value - - @property - def geometry(self): - """:class:`.Box`, :class:`.Cylinder`, :class:`.Sphere`, or - :class:`.Mesh` : The valid geometry element. - """ - if self.box is not None: - return self.box - if self.cylinder is not None: - return self.cylinder - if self.sphere is not None: - return self.sphere - if self.mesh is not None: - return self.mesh - return None - - @property - def meshes(self): - """list of :class:`~trimesh.base.Trimesh` : The geometry's triangular - mesh representation(s). - """ - return self.geometry.meshes - - def copy(self, prefix="", scale=None): - """Create a deep copy with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all names. - - Returns - ------- - :class:`.Geometry` - A deep copy. - """ - v = self.__class__( - box=(self.box.copy(prefix=prefix, scale=scale) if self.box else None), - cylinder=( - self.cylinder.copy(prefix=prefix, scale=scale) - if self.cylinder - else None - ), - sphere=( - self.sphere.copy(prefix=prefix, scale=scale) if self.sphere else None - ), - mesh=(self.mesh.copy(prefix=prefix, scale=scale) if self.mesh else None), - ) - return v - - -class Texture(URDFType): - """An image-based texture. - - Parameters - ---------- - filename : str - The path to the image that contains this texture. This can be - relative to the top-level URDF or an absolute path. - image : :class:`PIL.Image.Image`, optional - The image for the texture. - If not specified, it is loaded automatically from the filename. - """ - - _ATTRIBS = {"filename": (str, True)} - _TAG = "texture" - - def __init__(self, filename, image=None): - if image is None: - image = PIL.image.open(filename) - self.filename = filename - self.image = image - - @property - def filename(self): - """str : Path to the image for this texture.""" - return self._filename - - @filename.setter - def filename(self, value): - self._filename = str(value) - - @property - def image(self): - """:class:`PIL.Image.Image` : The image for this texture.""" - return self._image - - @image.setter - def image(self, value): - if isinstance(value, str): - value = PIL.Image.open(value) - if isinstance(value, np.ndarray): - value = PIL.Image.fromarray(value) - elif not isinstance(value, PIL.Image.Image): - raise ValueError("Texture only supports numpy arrays " "or PIL images") - self._image = value - - @classmethod - def _from_xml(cls, node, path): - kwargs = cls._parse(node, path) - - # Load image - fn = get_filename(path, kwargs["filename"]) - kwargs["image"] = PIL.Image.open(fn) - - return cls(**kwargs) - - def _to_xml(self, parent, path): - # Save the image - filepath = get_filename(path, self.filename, makedirs=True) - self.image.save(filepath) - - return self._unparse(path) - - def copy(self, prefix="", scale=None): - """Create a deep copy with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all names. - - Returns - ------- - :class:`.Texture` - A deep copy. - """ - v = self.__class__(filename=self.filename, image=self.image.copy()) - return v - - -class Material(URDFType): - """A material for some geometry. - - Parameters - ---------- - name : str - The name of the material. - color : (4,) float, optional - The RGBA color of the material in the range [0,1]. - texture : :class:`.Texture`, optional - A texture for the material. - """ - - _ATTRIBS = {"name": (str, True)} - _ELEMENTS = { - "texture": (Texture, False, False), - } - _TAG = "material" - - def __init__(self, name, color=None, texture=None): - self.name = name - self.color = color - self.texture = texture - - @property - def name(self): - """str : The name of the material.""" - return self._name - - @name.setter - def name(self, value): - self._name = str(value) - - @property - def color(self): - """(4,) float : The RGBA color of the material, in the range [0,1].""" - return self._color - - @color.setter - def color(self, value): - if value is not None: - value = np.asanyarray(value).astype(float) - value = np.clip(value, 0.0, 1.0) - if value.shape != (4,): - raise ValueError("Color must be a (4,) float") - self._color = value - - @property - def texture(self): - """:class:`.Texture` : The texture for the material.""" - return self._texture - - @texture.setter - def texture(self, value): - if value is not None: - if isinstance(value, six.string_types): - image = PIL.Image.open(value) - value = Texture(filename=value, image=image) - elif not isinstance(value, Texture): - raise ValueError( - "Invalid type for texture -- expect path to " "image or Texture" - ) - self._texture = value - - @classmethod - def _from_xml(cls, node, path): - kwargs = cls._parse(node, path) - - # Extract the color -- it's weirdly an attribute of a subelement - color = node.find("color") - if color is not None: - color = np.fromstring(color.attrib["rgba"], sep=" ", dtype=np.float64) - kwargs["color"] = color - - return cls(**kwargs) - - def _to_xml(self, parent, path): - # Simplify materials by collecting them at the top level. - - # For top-level elements, save the full material specification - if parent.tag == "robot": - node = self._unparse(path) - if self.color is not None: - color = ET.Element("color") - color.attrib["rgba"] = np.array2string(self.color)[1:-1] - node.append(color) - - else: - node = ET.Element("material") - node.attrib["name"] = self.name - if self.color is not None: - color = ET.Element("color") - color.attrib["rgba"] = np.array2string(self.color)[1:-1] - node.append(color) - return node - - def copy(self, prefix="", scale=None): - """Create a deep copy of the material with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. - - Returns - ------- - :class:`.Material` - A deep copy of the material. - """ - return self.__class__( - name="{}{}".format(prefix, self.name), - color=self.color, - texture=self.texture, - ) - - -class Collision(URDFTypeWithMesh): - """Collision properties of a link. - - Parameters - ---------- - geometry : :class:`.Geometry` - The geometry of the element - name : str, optional - The name of the collision geometry. - origin : (4,4) float, optional - The pose of the collision element relative to the link frame. - Defaults to identity. - """ - - _ATTRIBS = {"name": (str, False)} - _ELEMENTS = { - "geometry": (Geometry, True, False), - } - _TAG = "collision" - - def __init__(self, name, origin, geometry): - self.geometry = geometry - self.name = name - self.origin = origin - - @property - def geometry(self): - """:class:`.Geometry` : The geometry of this element.""" - return self._geometry - - @geometry.setter - def geometry(self, value): - if not isinstance(value, Geometry): - raise TypeError("Must set geometry with Geometry object") - self._geometry = value - - @property - def name(self): - """str : The name of this collision element.""" - return self._name - - @name.setter - def name(self, value): - if value is not None: - value = str(value) - self._name = value - - @property - def origin(self): - """(4,4) float : The pose of this element relative to the link frame.""" - return self._origin - - @origin.setter - def origin(self, value): - self._origin = configure_origin(value) - - @classmethod - def _from_xml(cls, node, path, lazy_load_meshes): - kwargs = cls._parse(node, path, lazy_load_meshes) - kwargs["origin"] = parse_origin(node) - return cls(**kwargs) - - def _to_xml(self, parent, path): - node = self._unparse(path) - node.append(unparse_origin(self.origin)) - return node - - def copy(self, prefix="", scale=None): - """Create a deep copy of the visual with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. - - Returns - ------- - :class:`.Visual` - A deep copy of the visual. - """ - origin = self.origin.copy() - if scale is not None: - if not isinstance(scale, (list, np.ndarray)): - scale = np.repeat(scale, 3) - origin[:3, 3] *= scale - return self.__class__( - name="{}{}".format(prefix, self.name), - origin=origin, - geometry=self.geometry.copy(prefix=prefix, scale=scale), - ) - - -class Visual(URDFTypeWithMesh): - """Visual properties of a link. - - Parameters - ---------- - geometry : :class:`.Geometry` - The geometry of the element - name : str, optional - The name of the visual geometry. - origin : (4,4) float, optional - The pose of the visual element relative to the link frame. - Defaults to identity. - material : :class:`.Material`, optional - The material of the element. - """ - - _ATTRIBS = {"name": (str, False)} - _ELEMENTS = { - "geometry": (Geometry, True, False), - "material": (Material, False, False), - } - _TAG = "visual" - - def __init__(self, geometry, name=None, origin=None, material=None): - self.geometry = geometry - self.name = name - self.origin = origin - self.material = material - - @property - def geometry(self): - """:class:`.Geometry` : The geometry of this element.""" - return self._geometry - - @geometry.setter - def geometry(self, value): - if not isinstance(value, Geometry): - raise TypeError("Must set geometry with Geometry object") - self._geometry = value - - @property - def name(self): - """str : The name of this visual element.""" - return self._name - - @name.setter - def name(self, value): - if value is not None: - value = str(value) - self._name = value - - @property - def origin(self): - """(4,4) float : The pose of this element relative to the link frame.""" - return self._origin - - @origin.setter - def origin(self, value): - self._origin = configure_origin(value) - - @property - def material(self): - """:class:`.Material` : The material for this element.""" - return self._material - - @material.setter - def material(self, value): - if value is not None: - if not isinstance(value, Material): - raise TypeError("Must set material with Material object") - self._material = value - - @classmethod - def _from_xml(cls, node, path, lazy_load_meshes): - kwargs = cls._parse(node, path, lazy_load_meshes) - kwargs["origin"] = parse_origin(node) - return cls(**kwargs) - - def _to_xml(self, parent, path): - node = self._unparse(path) - node.append(unparse_origin(self.origin)) - return node - - def copy(self, prefix="", scale=None): - """Create a deep copy of the visual with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. - - Returns - ------- - :class:`.Visual` - A deep copy of the visual. - """ - origin = self.origin.copy() - if scale is not None: - if not isinstance(scale, (list, np.ndarray)): - scale = np.repeat(scale, 3) - origin[:3, 3] *= scale - return self.__class__( - geometry=self.geometry.copy(prefix=prefix, scale=scale), - name="{}{}".format(prefix, self.name), - origin=origin, - material=(self.material.copy(prefix=prefix) if self.material else None), - ) - - -class Inertial(URDFType): - """The inertial properties of a link. - - Parameters - ---------- - mass : float - The mass of the link in kilograms. - inertia : (3,3) float - The 3x3 symmetric rotational inertia matrix. - origin : (4,4) float, optional - The pose of the inertials relative to the link frame. - Defaults to identity if not specified. - """ - - _TAG = "inertial" - - def __init__(self, mass, inertia, origin=None): - self.mass = mass - self.inertia = inertia - self.origin = origin - - @property - def mass(self): - """float : The mass of the link in kilograms.""" - return self._mass - - @mass.setter - def mass(self, value): - self._mass = float(value) - - @property - def inertia(self): - """(3,3) float : The 3x3 symmetric rotational inertia matrix.""" - return self._inertia - - @inertia.setter - def inertia(self, value): - value = np.asanyarray(value).astype(np.float64) - if not np.allclose(value, value.T): - raise ValueError("Inertia must be a symmetric matrix") - self._inertia = value - - @property - def origin(self): - """(4,4) float : The pose of the inertials relative to the link frame.""" - return self._origin - - @origin.setter - def origin(self, value): - self._origin = configure_origin(value) - - @classmethod - def _from_xml(cls, node, path): - origin = parse_origin(node) - mass = float(node.find("mass").attrib["value"]) - n = node.find("inertia") - xx = float(n.attrib["ixx"]) - xy = float(n.attrib["ixy"]) - xz = float(n.attrib["ixz"]) - yy = float(n.attrib["iyy"]) - yz = float(n.attrib["iyz"]) - zz = float(n.attrib["izz"]) - inertia = np.array([[xx, xy, xz], [xy, yy, yz], [xz, yz, zz]], dtype=np.float64) - return cls(mass=mass, inertia=inertia, origin=origin) - - def _to_xml(self, parent, path): - node = ET.Element("inertial") - node.append(unparse_origin(self.origin)) - mass = ET.Element("mass") - mass.attrib["value"] = str(self.mass) - node.append(mass) - inertia = ET.Element("inertia") - inertia.attrib["ixx"] = str(self.inertia[0, 0]) - inertia.attrib["ixy"] = str(self.inertia[0, 1]) - inertia.attrib["ixz"] = str(self.inertia[0, 2]) - inertia.attrib["iyy"] = str(self.inertia[1, 1]) - inertia.attrib["iyz"] = str(self.inertia[1, 2]) - inertia.attrib["izz"] = str(self.inertia[2, 2]) - node.append(inertia) - return node - - def copy(self, prefix="", mass=None, origin=None, inertia=None): - """Create a deep copy of the visual with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. - - Returns - ------- - :class:`.Inertial` - A deep copy of the visual. - """ - if mass is None: - mass = self.mass - if origin is None: - origin = self.origin.copy() - if inertia is None: - inertia = self.inertia.copy() - return self.__class__( - mass=mass, - inertia=inertia, - origin=origin, - ) - - -############################################################################### -# Joint types -############################################################################### - - -class JointCalibration(URDFType): - """The reference positions of the joint. - - Parameters - ---------- - rising : float, optional - When the joint moves in a positive direction, this position will - trigger a rising edge. - falling : - When the joint moves in a positive direction, this position will - trigger a falling edge. - """ - - _ATTRIBS = {"rising": (float, False), "falling": (float, False)} - _TAG = "calibration" - - def __init__(self, rising=None, falling=None): - self.rising = rising - self.falling = falling - - @property - def rising(self): - """float : description.""" - return self._rising - - @rising.setter - def rising(self, value): - if value is not None: - value = float(value) - self._rising = value - - @property - def falling(self): - """float : description.""" - return self._falling - - @falling.setter - def falling(self, value): - if value is not None: - value = float(value) - self._falling = value - - def copy(self, prefix="", scale=None): - """Create a deep copy of the visual with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. - - Returns - ------- - :class:`.JointCalibration` - A deep copy of the visual. - """ - return self.__class__( - rising=self.rising, - falling=self.falling, - ) - - -class JointDynamics(URDFType): - """The dynamic properties of the joint. - - Parameters - ---------- - damping : float - The damping value of the joint (Ns/m for prismatic joints, - Nms/rad for revolute). - friction : float - The static friction value of the joint (N for prismatic joints, - Nm for revolute). - """ - - _ATTRIBS = { - "damping": (float, False), - "friction": (float, False), - } - _TAG = "dynamics" - - def __init__(self, damping, friction): - self.damping = damping - self.friction = friction - - @property - def damping(self): - """float : The damping value of the joint.""" - return self._damping - - @damping.setter - def damping(self, value): - if value is not None: - value = float(value) - self._damping = value - - @property - def friction(self): - """float : The static friction value of the joint.""" - return self._friction - - @friction.setter - def friction(self, value): - if value is not None: - value = float(value) - self._friction = value - - def copy(self, prefix="", scale=None): - """Create a deep copy of the visual with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. - - Returns - ------- - :class:`.JointDynamics` - A deep copy of the visual. - """ - return self.__class__( - damping=self.damping, - friction=self.friction, - ) - - -class JointLimit(URDFType): - """The limits of the joint. - - Parameters - ---------- - effort : float - The maximum joint effort (N for prismatic joints, Nm for revolute). - velocity : float - The maximum joint velocity (m/s for prismatic joints, rad/s for - revolute). - lower : float, optional - The lower joint limit (m for prismatic joints, rad for revolute). - upper : float, optional - The upper joint limit (m for prismatic joints, rad for revolute). - """ - - _ATTRIBS = { - "effort": (float, True), - "velocity": (float, True), - "lower": (float, False), - "upper": (float, False), - } - _TAG = "limit" - - def __init__(self, effort, velocity, lower=None, upper=None): - self.effort = effort - self.velocity = velocity - self.lower = lower - self.upper = upper - - @property - def effort(self): - """float : The maximum joint effort.""" - return self._effort - - @effort.setter - def effort(self, value): - self._effort = float(value) - - @property - def velocity(self): - """float : The maximum joint velocity.""" - return self._velocity - - @velocity.setter - def velocity(self, value): - self._velocity = float(value) - - @property - def lower(self): - """float : The lower joint limit.""" - return self._lower - - @lower.setter - def lower(self, value): - if value is not None: - value = float(value) - self._lower = value - - @property - def upper(self): - """float : The upper joint limit.""" - return self._upper - - @upper.setter - def upper(self, value): - if value is not None: - value = float(value) - self._upper = value - - def copy(self, prefix="", scale=None): - """Create a deep copy of the visual with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. - - Returns - ------- - :class:`.JointLimit` - A deep copy of the visual. - """ - return self.__class__( - effort=self.effort, - velocity=self.velocity, - lower=self.lower, - upper=self.upper, - ) - - -class JointMimic(URDFType): - """A mimicry tag for a joint, which forces its configuration to - mimic another joint's. - - This joint's configuration value is set equal to - ``multiplier * other_joint_cfg + offset``. - - Parameters - ---------- - joint : str - The name of the joint to mimic. - multiplier : float - The joint configuration multiplier. Defaults to 1.0. - offset : float, optional - The joint configuration offset. Defaults to 0.0. - """ - - _ATTRIBS = { - "joint": (str, True), - "multiplier": (float, False), - "offset": (float, False), - } - _TAG = "mimic" - - def __init__(self, joint, multiplier=None, offset=None): - self.joint = joint - self.multiplier = multiplier - self.offset = offset - - @property - def joint(self): - """float : The name of the joint to mimic.""" - return self._joint - - @joint.setter - def joint(self, value): - self._joint = str(value) - - @property - def multiplier(self): - """float : The multiplier for the joint configuration.""" - return self._multiplier - - @multiplier.setter - def multiplier(self, value): - if value is not None: - value = float(value) - else: - value = 1.0 - self._multiplier = value - - @property - def offset(self): - """float : The offset for the joint configuration""" - return self._offset - - @offset.setter - def offset(self, value): - if value is not None: - value = float(value) - else: - value = 0.0 - self._offset = value - - def copy(self, prefix="", scale=None): - """Create a deep copy of the joint mimic with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. - - Returns - ------- - :class:`.JointMimic` - A deep copy of the joint mimic. - """ - return self.__class__( - joint="{}{}".format(prefix, self.joint), - multiplier=self.multiplier, - offset=self.offset, - ) - - -class SafetyController(URDFType): - """A controller for joint movement safety. - - Parameters - ---------- - k_velocity : float - An attribute specifying the relation between the effort and velocity - limits. - k_position : float, optional - An attribute specifying the relation between the position and velocity - limits. Defaults to 0.0. - soft_lower_limit : float, optional - The lower joint boundary where the safety controller kicks in. - Defaults to 0.0. - soft_upper_limit : float, optional - The upper joint boundary where the safety controller kicks in. - Defaults to 0.0. - """ - - _ATTRIBS = { - "k_velocity": (float, True), - "k_position": (float, False), - "soft_lower_limit": (float, False), - "soft_upper_limit": (float, False), - } - _TAG = "safety_controller" - - def __init__( - self, k_velocity, k_position=None, soft_lower_limit=None, soft_upper_limit=None - ): - self.k_velocity = k_velocity - self.k_position = k_position - self.soft_lower_limit = soft_lower_limit - self.soft_upper_limit = soft_upper_limit - - @property - def soft_lower_limit(self): - """float : The soft lower limit where the safety controller kicks in.""" - return self._soft_lower_limit - - @soft_lower_limit.setter - def soft_lower_limit(self, value): - if value is not None: - value = float(value) - else: - value = 0.0 - self._soft_lower_limit = value - - @property - def soft_upper_limit(self): - """float : The soft upper limit where the safety controller kicks in.""" - return self._soft_upper_limit - - @soft_upper_limit.setter - def soft_upper_limit(self, value): - if value is not None: - value = float(value) - else: - value = 0.0 - self._soft_upper_limit = value - - @property - def k_position(self): - """float : A relation between the position and velocity limits.""" - return self._k_position - - @k_position.setter - def k_position(self, value): - if value is not None: - value = float(value) - else: - value = 0.0 - self._k_position = value - - @property - def k_velocity(self): - """float : A relation between the effort and velocity limits.""" - return self._k_velocity - - @k_velocity.setter - def k_velocity(self, value): - self._k_velocity = float(value) - - def copy(self, prefix="", scale=None): - """Create a deep copy of the visual with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. - - Returns - ------- - :class:`.SafetyController` - A deep copy of the visual. - """ - return self.__class__( - k_velocity=self.k_velocity, - k_position=self.k_position, - soft_lower_limit=self.soft_lower_limit, - soft_upper_limit=self.soft_upper_limit, - ) - - -############################################################################### -# Transmission types -############################################################################### - - -class Actuator(URDFType): - """An actuator. - - Parameters - ---------- - name : str - The name of this actuator. - mechanicalReduction : str, optional - A specifier for the mechanical reduction at the joint/actuator - transmission. - hardwareInterfaces : list of str, optional - The supported hardware interfaces to the actuator. - """ - - _ATTRIBS = { - "name": (str, True), - } - _TAG = "actuator" - - def __init__(self, name, mechanicalReduction=None, hardwareInterfaces=None): - self.name = name - self.mechanicalReduction = mechanicalReduction - self.hardwareInterfaces = hardwareInterfaces - - @property - def name(self): - """str : The name of this actuator.""" - return self._name - - @name.setter - def name(self, value): - self._name = str(value) - - @property - def mechanicalReduction(self): - """str : A specifier for the type of mechanical reduction.""" - return self._mechanicalReduction - - @mechanicalReduction.setter - def mechanicalReduction(self, value): - if value is not None: - value = str(value) - self._mechanicalReduction = value - - @property - def hardwareInterfaces(self): - """list of str : The supported hardware interfaces.""" - return self._hardwareInterfaces - - @hardwareInterfaces.setter - def hardwareInterfaces(self, value): - if value is None: - value = [] - else: - value = list(value) - for i, v in enumerate(value): - value[i] = str(v) - self._hardwareInterfaces = value - - @classmethod - def _from_xml(cls, node, path): - kwargs = cls._parse(node, path) - mr = node.find("mechanicalReduction") - if mr is not None: - mr = float(mr.text) - kwargs["mechanicalReduction"] = mr - hi = node.findall("hardwareInterface") - if len(hi) > 0: - hi = [h.text for h in hi] - kwargs["hardwareInterfaces"] = hi - return cls(**kwargs) - - def _to_xml(self, parent, path): - node = self._unparse(path) - if self.mechanicalReduction is not None: - mr = ET.Element("mechanicalReduction") - mr.text = str(self.mechanicalReduction) - node.append(mr) - if len(self.hardwareInterfaces) > 0: - for hi in self.hardwareInterfaces: - h = ET.Element("hardwareInterface") - h.text = hi - node.append(h) - return node - - def copy(self, prefix="", scale=None): - """Create a deep copy of the visual with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. - - Returns - ------- - :class:`.Actuator` - A deep copy of the visual. - """ - return self.__class__( - name="{}{}".format(prefix, self.name), - mechanicalReduction=self.mechanicalReduction, - hardwareInterfaces=self.hardwareInterfaces.copy(), - ) - - -class TransmissionJoint(URDFType): - """A transmission joint specification. - - Parameters - ---------- - name : str - The name of this actuator. - hardwareInterfaces : list of str, optional - The supported hardware interfaces to the actuator. - """ - - _ATTRIBS = { - "name": (str, True), - } - _TAG = "joint" - - def __init__(self, name, hardwareInterfaces): - self.name = name - self.hardwareInterfaces = hardwareInterfaces - - @property - def name(self): - """str : The name of this transmission joint.""" - return self._name - - @name.setter - def name(self, value): - self._name = str(value) - - @property - def hardwareInterfaces(self): - """list of str : The supported hardware interfaces.""" - return self._hardwareInterfaces - - @hardwareInterfaces.setter - def hardwareInterfaces(self, value): - if value is None: - value = [] - else: - value = list(value) - for i, v in enumerate(value): - value[i] = str(v) - self._hardwareInterfaces = value - - @classmethod - def _from_xml(cls, node, path): - kwargs = cls._parse(node, path) - hi = node.findall("hardwareInterface") - if len(hi) > 0: - hi = [h.text for h in hi] - kwargs["hardwareInterfaces"] = hi - return cls(**kwargs) - - def _to_xml(self, parent, path): - node = self._unparse(path) - if len(self.hardwareInterfaces) > 0: - for hi in self.hardwareInterfaces: - h = ET.Element("hardwareInterface") - h.text = hi - node.append(h) - return node - - def copy(self, prefix="", scale=None): - """Create a deep copy with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all names. - - Returns - ------- - :class:`.TransmissionJoint` - A deep copy. - """ - return self.__class__( - name="{}{}".format(prefix, self.name), - hardwareInterfaces=self.hardwareInterfaces.copy(), - ) - - -############################################################################### -# Top-level types -############################################################################### +from __future__ import annotations +import os +import time +from collections import OrderedDict +from typing import IO, Mapping, Optional, Sequence, Union, cast -class Transmission(URDFType): - """An element that describes the relationship between an actuator and a - joint. - - Parameters - ---------- - name : str - The name of this transmission. - trans_type : str - The type of this transmission. - joints : list of :class:`.TransmissionJoint` - The joints connected to this transmission. - actuators : list of :class:`.Actuator` - The actuators connected to this transmission. - """ - - _ATTRIBS = { - "name": (str, True), - } - _ELEMENTS = { - "joints": (TransmissionJoint, True, True), - "actuators": (Actuator, True, True), - } - _TAG = "transmission" - - def __init__(self, name, trans_type, joints=None, actuators=None): - self.name = name - self.trans_type = trans_type - self.joints = joints - self.actuators = actuators - - @property - def name(self): - """str : The name of this transmission.""" - return self._name - - @name.setter - def name(self, value): - self._name = str(value) - - @property - def trans_type(self): - """str : The type of this transmission.""" - return self._trans_type - - @trans_type.setter - def trans_type(self, value): - self._trans_type = str(value) - - @property - def joints(self): - """:class:`.TransmissionJoint` : The joints the transmission is - connected to. - """ - return self._joints - - @joints.setter - def joints(self, value): - if value is None: - value = [] - else: - value = list(value) - for v in value: - if not isinstance(v, TransmissionJoint): - raise TypeError("Joints expects a list of TransmissionJoint") - self._joints = value - - @property - def actuators(self): - """:class:`.Actuator` : The actuators the transmission is connected to.""" - return self._actuators - - @actuators.setter - def actuators(self, value): - if value is None: - value = [] - else: - value = list(value) - for v in value: - if not isinstance(v, Actuator): - raise TypeError("Actuators expects a list of Actuator") - self._actuators = value - - @classmethod - def _from_xml(cls, node, path): - kwargs = cls._parse(node, path) - trans_type = node.attrib.get("type") - if trans_type is None: - trans_type = node.find("type").text - kwargs["trans_type"] = trans_type - return cls(**kwargs) - - def _to_xml(self, parent, path): - node = self._unparse(path) - ttype = ET.Element("type") - ttype.text = self.trans_type - node.append(ttype) - return node - - def copy(self, prefix="", scale=None): - """Create a deep copy with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all names. - - Returns - ------- - :class:`.Transmission` - A deep copy. - """ - return self.__class__( - name="{}{}".format(prefix, self.name), - trans_type=self.trans_type, - joints=[j.copy(prefix) for j in self.joints], - actuators=[a.copy(prefix) for a in self.actuators], - ) - - -class Joint(URDFType): - """A connection between two links. - - There are several types of joints, including: - - - ``fixed`` - a joint that cannot move. - - ``prismatic`` - a joint that slides along the joint axis. - - ``revolute`` - a hinge joint that rotates about the axis with a limited - range of motion. - - ``continuous`` - a hinge joint that rotates about the axis with an - unlimited range of motion. - - ``planar`` - a joint that moves in the plane orthogonal to the axis. - - ``floating`` - a joint that can move in 6DoF. - - Parameters - ---------- - name : str - The name of this joint. - parent : str - The name of the parent link of this joint. - child : str - The name of the child link of this joint. - joint_type : str - The type of the joint. Must be one of :obj:`.Joint.TYPES`. - axis : (3,) float, optional - The axis of the joint specified in joint frame. Defaults to - ``[1,0,0]``. - origin : (4,4) float, optional - The pose of the child link with respect to the parent link's frame. - The joint frame is defined to be coincident with the child link's - frame, so this is also the pose of the joint frame with respect to - the parent link's frame. - limit : :class:`.JointLimit`, optional - Limit for the joint. Only required for revolute and prismatic - joints. - dynamics : :class:`.JointDynamics`, optional - Dynamics for the joint. - safety_controller : :class`.SafetyController`, optional - The safety controller for this joint. - calibration : :class:`.JointCalibration`, optional - Calibration information for the joint. - mimic : :class:`JointMimic`, optional - Joint mimicry information. - """ - - TYPES = ["fixed", "prismatic", "revolute", "continuous", "floating", "planar"] - _ATTRIBS = { - "name": (str, True), - } - _ELEMENTS = { - "dynamics": (JointDynamics, False, False), - "limit": (JointLimit, False, False), - "mimic": (JointMimic, False, False), - "safety_controller": (SafetyController, False, False), - "calibration": (JointCalibration, False, False), - } - _TAG = "joint" - - def __init__( - self, - name, - joint_type, - parent, - child, - axis=None, - origin=None, - limit=None, - dynamics=None, - safety_controller=None, - calibration=None, - mimic=None, - ): - self.name = name - self.parent = parent - self.child = child - self.joint_type = joint_type - self.axis = axis - self.origin = origin - self.limit = limit - self.dynamics = dynamics - self.safety_controller = safety_controller - self.calibration = calibration - self.mimic = mimic - - @property - def name(self): - """str : Name for this joint.""" - return self._name - - @name.setter - def name(self, value): - self._name = str(value) - - @property - def joint_type(self): - """str : The type of this joint.""" - return self._joint_type - - @joint_type.setter - def joint_type(self, value): - value = str(value) - if value not in Joint.TYPES: - raise ValueError("Unsupported joint type {}".format(value)) - self._joint_type = value - - @property - def parent(self): - """str : The name of the parent link.""" - return self._parent - - @parent.setter - def parent(self, value): - self._parent = str(value) - - @property - def child(self): - """str : The name of the child link.""" - return self._child - - @child.setter - def child(self, value): - self._child = str(value) - - @property - def axis(self): - """(3,) float : The joint axis in the joint frame.""" - return self._axis - - @axis.setter - def axis(self, value): - if value is None: - value = np.array([1.0, 0.0, 0.0], dtype=np.float64) - elif np.linalg.norm(value) < 1e-4: - value = np.array([1.0, 0.0, 0.0], dtype=np.float64) - else: - value = np.asanyarray(value, dtype=np.float64) - if value.shape != (3,): - raise ValueError("Invalid shape for axis, should be (3,)") - value = value / np.linalg.norm(value) - self._axis = value - - @property - def origin(self): - """(4,4) float : The pose of child and joint frames relative to the - parent link's frame. - """ - return self._origin - - @origin.setter - def origin(self, value): - self._origin = configure_origin(value) - - @property - def limit(self): - """:class:`.JointLimit` : The limits for this joint.""" - return self._limit - - @limit.setter - def limit(self, value): - if value is None: - if self.joint_type in ["prismatic", "revolute"]: - raise ValueError( - "Require joint limit for prismatic and " "revolute joints" - ) - elif not isinstance(value, JointLimit): - raise TypeError("Expected JointLimit type") - self._limit = value - - @property - def dynamics(self): - """:class:`.JointDynamics` : The dynamics for this joint.""" - return self._dynamics - - @dynamics.setter - def dynamics(self, value): - if value is not None: - if not isinstance(value, JointDynamics): - raise TypeError("Expected JointDynamics type") - self._dynamics = value - - @property - def safety_controller(self): - """:class:`.SafetyController` : The safety controller for this joint.""" - return self._safety_controller - - @safety_controller.setter - def safety_controller(self, value): - if value is not None: - if not isinstance(value, SafetyController): - raise TypeError("Expected SafetyController type") - self._safety_controller = value - - @property - def calibration(self): - """:class:`.JointCalibration` : The calibration for this joint.""" - return self._calibration - - @calibration.setter - def calibration(self, value): - if value is not None: - if not isinstance(value, JointCalibration): - raise TypeError("Expected JointCalibration type") - self._calibration = value - - @property - def mimic(self): - """:class:`.JointMimic` : The mimic for this joint.""" - return self._mimic - - @mimic.setter - def mimic(self, value): - if value is not None: - if not isinstance(value, JointMimic): - raise TypeError("Expected JointMimic type") - self._mimic = value - - def is_valid(self, cfg): - """Check if the provided configuration value is valid for this joint. - - Parameters - ---------- - cfg : float, (2,) float, (6,) float, or (4,4) float - The configuration of the joint. - - Returns - ------- - is_valid : bool - True if the configuration is valid, and False otherwise. - """ - if self.joint_type not in ["fixed", "revolute"]: - return True - if self.joint_limit is None: - return True - cfg = float(cfg) - lower = -np.infty - upper = np.infty - if self.limit.lower is not None: - lower = self.limit.lower - if self.limit.upper is not None: - upper = self.limit.upper - return cfg >= lower and cfg <= upper - - def get_child_pose(self, cfg=None): - """Computes the child pose relative to a parent pose for a given - configuration value. - - Parameters - ---------- - cfg : float, (2,) float, (6,) float, or (4,4) float - The configuration values for this joint. They are interpreted - based on the joint type as follows: - - - ``fixed`` - not used. - - ``prismatic`` - a translation along the axis in meters. - - ``revolute`` - a rotation about the axis in radians. - - ``continuous`` - a rotation about the axis in radians. - - ``planar`` - the x and y translation values in the plane. - - ``floating`` - the xyz values followed by the rpy values, - or a (4,4) matrix. - - If ``cfg`` is ``None``, then this just returns the joint pose. - - Returns - ------- - pose : (4,4) float - The pose of the child relative to the parent. - """ - if cfg is None: - return self.origin - elif self.joint_type == "fixed": - return self.origin - elif self.joint_type in ["revolute", "continuous"]: - if cfg is None: - cfg = 0.0 - else: - cfg = float(cfg) - R = trimesh.transformations.rotation_matrix(cfg, self.axis) - return self.origin.dot(R) - elif self.joint_type == "prismatic": - if cfg is None: - cfg = 0.0 - else: - cfg = float(cfg) - translation = np.eye(4, dtype=np.float64) - translation[:3, 3] = self.axis * cfg - return self.origin.dot(translation) - elif self.joint_type == "planar": - if cfg is None: - cfg = np.zeros(2, dtype=np.float64) - else: - cfg = np.asanyarray(cfg, dtype=np.float64) - if cfg.shape != (2,): - raise ValueError("(2,) float configuration required for planar joints") - translation = np.eye(4, dtype=np.float64) - translation[:3, 3] = self.origin[:3, :2].dot(cfg) - return self.origin.dot(translation) - elif self.joint_type == "floating": - if cfg is None: - cfg = np.zeros(6, dtype=np.float64) - else: - cfg = configure_origin(cfg) - if cfg is None: - raise ValueError("Invalid configuration for floating joint") - return self.origin.dot(cfg) - else: - raise ValueError("Invalid configuration") - - def get_child_poses(self, cfg, n_cfgs): - """Computes the child pose relative to a parent pose for a given set of - configuration values. - - Parameters - ---------- - cfg : (n,) float or None - The configuration values for this joint. They are interpreted - based on the joint type as follows: - - - ``fixed`` - not used. - - ``prismatic`` - a translation along the axis in meters. - - ``revolute`` - a rotation about the axis in radians. - - ``continuous`` - a rotation about the axis in radians. - - ``planar`` - Not implemented. - - ``floating`` - Not implemented. - - If ``cfg`` is ``None``, then this just returns the joint pose. - - Returns - ------- - poses : (n,4,4) float - The poses of the child relative to the parent. - """ - if cfg is None: - return np.tile(self.origin, (n_cfgs, 1, 1)) - elif self.joint_type == "fixed": - return np.tile(self.origin, (n_cfgs, 1, 1)) - elif self.joint_type in ["revolute", "continuous"]: - if cfg is None: - cfg = np.zeros(n_cfgs) - return np.matmul(self.origin, self._rotation_matrices(cfg, self.axis)) - elif self.joint_type == "prismatic": - if cfg is None: - cfg = np.zeros(n_cfgs) - translation = np.tile(np.eye(4), (n_cfgs, 1, 1)) - translation[:, :3, 3] = self.axis * cfg[:, np.newaxis] - return np.matmul(self.origin, translation) - elif self.joint_type == "planar": - raise NotImplementedError() - elif self.joint_type == "floating": - raise NotImplementedError() - else: - raise ValueError("Invalid configuration") - - @classmethod - def _from_xml(cls, node, path): - kwargs = cls._parse(node, path) - kwargs["joint_type"] = str(node.attrib["type"]) - kwargs["parent"] = node.find("parent").attrib["link"] - kwargs["child"] = node.find("child").attrib["link"] - axis = node.find("axis") - if axis is not None: - axis = np.fromstring(axis.attrib["xyz"], sep=" ") - kwargs["axis"] = axis - kwargs["origin"] = parse_origin(node) - return cls(**kwargs) - - def _to_xml(self, parent, path): - node = self._unparse(path) - parent = ET.Element("parent") - parent.attrib["link"] = self.parent - node.append(parent) - child = ET.Element("child") - child.attrib["link"] = self.child - node.append(child) - if self.axis is not None: - axis = ET.Element("axis") - axis.attrib["xyz"] = np.array2string(self.axis)[1:-1] - node.append(axis) - node.append(unparse_origin(self.origin)) - node.attrib["type"] = self.joint_type - return node - - def _rotation_matrices(self, angles, axis): - """Compute rotation matrices from angle/axis representations. - - Parameters - ---------- - angles : (n,) float - The angles. - axis : (3,) float - The axis. - - Returns - ------- - rots : (n,4,4) - The rotation matrices - """ - axis = axis / np.linalg.norm(axis) - sina = np.sin(angles) - cosa = np.cos(angles) - M = np.tile(np.eye(4), (len(angles), 1, 1)) - M[:, 0, 0] = cosa - M[:, 1, 1] = cosa - M[:, 2, 2] = cosa - M[:, :3, :3] += ( - np.tile(np.outer(axis, axis), (len(angles), 1, 1)) - * (1.0 - cosa)[:, np.newaxis, np.newaxis] - ) - M[:, :3, :3] += ( - np.tile( - np.array( - [ - [0.0, -axis[2], axis[1]], - [axis[2], 0.0, -axis[0]], - [-axis[1], axis[0], 0.0], - ] - ), - (len(angles), 1, 1), - ) - * sina[:, np.newaxis, np.newaxis] - ) - return M - - def copy(self, prefix="", scale=None): - """Create a deep copy of the joint with the prefix applied to all names. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. - - Returns - ------- - :class:`.Joint` - A deep copy of the joint. - """ - origin = self.origin.copy() - if scale is not None: - if not isinstance(scale, (list, np.ndarray)): - scale = np.repeat(scale, 3) - origin[:3, 3] *= scale - cpy = self.__class__( - name="{}{}".format(prefix, self.name), - joint_type=self.joint_type, - parent="{}{}".format(prefix, self.parent), - child="{}{}".format(prefix, self.child), - axis=self.axis.copy(), - origin=origin, - limit=(self.limit.copy(prefix, scale) if self.limit else None), - dynamics=(self.dynamics.copy(prefix, scale) if self.dynamics else None), - safety_controller=( - self.safety_controller.copy(prefix, scale) - if self.safety_controller - else None - ), - calibration=( - self.calibration.copy(prefix, scale) if self.calibration else None - ), - mimic=(self.mimic.copy(prefix=prefix, scale=scale) if self.mimic else None), - ) - return cpy - - -class Link(URDFTypeWithMesh): - """A link of a rigid object. - - Parameters - ---------- - name : str - The name of the link. - inertial : :class:`.Inertial`, optional - The inertial properties of the link. - visuals : list of :class:`.Visual`, optional - The visual properties of the link. - collsions : list of :class:`.Collision`, optional - The collision properties of the link. - """ - - _ATTRIBS = { - "name": (str, True), - } - _ELEMENTS = { - "inertial": (Inertial, False, False), - "visuals": (Visual, False, True), - "collisions": (Collision, False, True), - } - _TAG = "link" - - def __init__(self, name, inertial, visuals, collisions): - self.name = name - self.inertial = inertial - self.visuals = visuals - self.collisions = collisions - - self._collision_mesh = None - - @property - def name(self): - """str : The name of this link.""" - return self._name - - @name.setter - def name(self, value): - self._name = str(value) - - @property - def inertial(self): - """:class:`.Inertial` : Inertial properties of the link.""" - return self._inertial - - @inertial.setter - def inertial(self, value): - if value is not None and not isinstance(value, Inertial): - raise TypeError("Expected Inertial object") - # Set default inertial - if value is None: - value = Inertial(mass=1.0, inertia=np.eye(3)) - self._inertial = value - - @property - def visuals(self): - """list of :class:`.Visual` : The visual properties of this link.""" - return self._visuals - - @visuals.setter - def visuals(self, value): - if value is None: - value = [] - else: - value = list(value) - for v in value: - if not isinstance(v, Visual): - raise ValueError("Expected list of Visual objects") - self._visuals = value - - @property - def collisions(self): - """list of :class:`.Collision` : The collision properties of this link.""" - return self._collisions - - @collisions.setter - def collisions(self, value): - if value is None: - value = [] - else: - value = list(value) - for v in value: - if not isinstance(v, Collision): - raise ValueError("Expected list of Collision objects") - self._collisions = value - - @property - def collision_mesh(self): - """:class:`~trimesh.base.Trimesh` : A single collision mesh for - the link, specified in the link frame, or None if there isn't one. - """ - if len(self.collisions) == 0: - return None - if self._collision_mesh is None: - meshes = [] - for c in self.collisions: - for m in c.geometry.meshes: - m = m.copy() - pose = c.origin - if c.geometry.mesh is not None: - if c.geometry.mesh.scale is not None: - S = np.eye(4) - S[:3, :3] = np.diag(c.geometry.mesh.scale) - pose = pose.dot(S) - m.apply_transform(pose) - meshes.append(m) - if len(meshes) == 0: - return None - self._collision_mesh = meshes[0] + meshes[1:] - return self._collision_mesh - - def copy(self, prefix="", scale=None, collision_only=False): - """Create a deep copy of the link. - - Parameters - ---------- - prefix : str - A prefix to apply to all joint and link names. +import networkx as nx +import numpy as np +import numpy.typing as npt +import trimesh +from lxml import etree as ET - Returns - ------- - link : :class:`.Link` - A deep copy of the Link. - """ - inertial = self.inertial.copy() if self.inertial is not None else None - cm = self._collision_mesh - if scale is not None: - if self.collision_mesh is not None and self.inertial is not None: - sm = np.eye(4) - if not isinstance(scale, (list, np.ndarray)): - scale = np.repeat(scale, 3) - sm[:3, :3] = np.diag(scale) - cm = self.collision_mesh.copy() - cm.density = self.inertial.mass / cm.volume - cm.apply_transform(sm) - cmm = np.eye(4) - cmm[:3, 3] = cm.center_mass - inertial = Inertial(mass=cm.mass, inertia=cm.moment_inertia, origin=cmm) - - visuals = None - if not collision_only: - visuals = [v.copy(prefix=prefix, scale=scale) for v in self.visuals] - - cpy = self.__class__( - name="{}{}".format(prefix, self.name), - inertial=inertial, - visuals=visuals, - collisions=[v.copy(prefix=prefix, scale=scale) for v in self.collisions], - ) - cpy._collision_mesh = cm - return cpy +from urchin.base import URDFTypeWithMesh +from urchin.joint import Joint +from urchin.link import Link +from urchin.material import Material +from urchin.transmission import Transmission class URDF(URDFTypeWithMesh): @@ -2769,13 +54,13 @@ class URDF(URDFTypeWithMesh): def __init__( self, - name, - links, - joints=None, - transmissions=None, - materials=None, - other_xml=None, - ): + name: str, + links: Sequence[Link], + joints: Optional[Sequence[Joint]] = None, + transmissions: Optional[Sequence[Transmission]] = None, + materials: Optional[Sequence[Material]] = None, + other_xml: Optional[Union[bytes, str]] = None, + ) -> None: if joints is None: joints = [] if transmissions is None: @@ -2785,41 +70,39 @@ def __init__( self.name = name self.other_xml = other_xml - self.mesh_need_to_mirror = [] + self.mesh_need_to_mirror: list[str] = [] # No setters for these - self._links = list(links) - self._joints = list(joints) - self._transmissions = list(transmissions) - self._materials = list(materials) + self._links: list[Link] = list(links) + self._joints: list[Joint] = list(joints) + self._transmissions: list[Transmission] = list(transmissions) + self._materials: list[Material] = list(materials) # Set up private helper maps from name to value - self._link_map = {} - self._joint_map = {} - self._transmission_map = {} - self._material_map = {} - - for x in self._links: - if x.name in self._link_map: - raise ValueError("Two links with name {} found".format(x.name)) - self._link_map[x.name] = x - - for x in self._joints: - if x.name in self._joint_map: - raise ValueError("Two joints with name {} " "found".format(x.name)) - self._joint_map[x.name] = x - - for x in self._transmissions: - if x.name in self._transmission_map: - raise ValueError( - "Two transmissions with name {} " "found".format(x.name) - ) - self._transmission_map[x.name] = x - - for x in self._materials: - if x.name in self._material_map: - raise ValueError("Two materials with name {} " "found".format(x.name)) - self._material_map[x.name] = x + self._link_map: dict[str, Link] = {} + self._joint_map: dict[str, Joint] = {} + self._transmission_map: dict[str, Transmission] = {} + self._material_map: dict[str, Material] = {} + + for link_obj in self._links: + if link_obj.name in self._link_map: + raise ValueError("Two links with name {} found".format(link_obj.name)) + self._link_map[link_obj.name] = link_obj + + for joint_obj in self._joints: + if joint_obj.name in self._joint_map: + raise ValueError("Two joints with name {} found".format(joint_obj.name)) + self._joint_map[joint_obj.name] = joint_obj + + for trans_obj in self._transmissions: + if trans_obj.name in self._transmission_map: + raise ValueError("Two transmissions with name {} found".format(trans_obj.name)) + self._transmission_map[trans_obj.name] = trans_obj + + for mat_obj in self._materials: + if mat_obj.name in self._material_map: + raise ValueError("Two materials with name {} found".format(mat_obj.name)) + self._material_map[mat_obj.name] = mat_obj # Synchronize materials between links and top-level set self._merge_materials() @@ -2855,105 +138,105 @@ def __init__( self._reverse_topo = list(reversed(list(nx.topological_sort(self._G)))) @property - def name(self): + def name(self) -> str: """str : The name of the URDF.""" return self._name @name.setter - def name(self, value): + def name(self, value: str) -> None: self._name = str(value) @property - def links(self): + def links(self) -> list[Link]: """list of :class:`.Link` : The links of the URDF. This returns a copy of the links array which cannot be edited directly. If you want to add or remove links, use the appropriate functions. """ - return copy.copy(self._links) + return list(self._links) @property - def link_map(self): + def link_map(self) -> dict[str, Link]: """dict : Map from link names to the links themselves. This returns a copy of the link map which cannot be edited directly. If you want to add or remove links, use the appropriate functions. """ - return copy.copy(self._link_map) + return dict(self._link_map) @property - def joints(self): + def joints(self) -> list[Joint]: """list of :class:`.Joint` : The links of the URDF. This returns a copy of the joints array which cannot be edited directly. If you want to add or remove joints, use the appropriate functions. """ - return copy.copy(self._joints) + return list(self._joints) @property - def joint_map(self): + def joint_map(self) -> dict[str, Joint]: """dict : Map from joint names to the joints themselves. This returns a copy of the joint map which cannot be edited directly. If you want to add or remove joints, use the appropriate functions. """ - return copy.copy(self._joint_map) + return dict(self._joint_map) @property - def transmissions(self): + def transmissions(self) -> list[Transmission]: """list of :class:`.Transmission` : The transmissions of the URDF. This returns a copy of the transmissions array which cannot be edited directly. If you want to add or remove transmissions, use the appropriate functions. """ - return copy.copy(self._transmissions) + return list(self._transmissions) @property - def transmission_map(self): + def transmission_map(self) -> dict[str, Transmission]: """dict : Map from transmission names to the transmissions themselves. This returns a copy of the transmission map which cannot be edited directly. If you want to add or remove transmissions, use the appropriate functions. """ - return copy.copy(self._transmission_map) + return dict(self._transmission_map) @property - def materials(self): + def materials(self) -> list[Material]: """list of :class:`.Material` : The materials of the URDF. This returns a copy of the materials array which cannot be edited directly. If you want to add or remove materials, use the appropriate functions. """ - return copy.copy(self._materials) + return list(self._materials) @property - def material_map(self): + def material_map(self) -> dict[str, Material]: """dict : Map from material names to the materials themselves. This returns a copy of the material map which cannot be edited directly. If you want to add or remove materials, use the appropriate functions. """ - return copy.copy(self._material_map) + return dict(self._material_map) @property - def other_xml(self): + def other_xml(self) -> Optional[Union[bytes, str]]: """str : Any extra XML that belongs with the URDF.""" return self._other_xml @other_xml.setter - def other_xml(self, value): + def other_xml(self, value: Optional[Union[bytes, str]]) -> None: self._other_xml = value @property - def actuated_joints(self): + def actuated_joints(self) -> list[Joint]: """list of :class:`.Joint` : The joints that are independently actuated. @@ -2963,7 +246,7 @@ def actuated_joints(self): return self._actuated_joints @property - def actuated_joint_names(self): + def actuated_joint_names(self) -> list[str]: """list of :class:`.Joint` : The names of joints that are independently actuated. @@ -2972,7 +255,15 @@ def actuated_joint_names(self): """ return [j.name for j in self._actuated_joints] - def cfg_to_vector(self, cfg): + def cfg_to_vector( + self, + cfg: Union[ + Mapping[str, float], + Sequence[float], + npt.ArrayLike, + None, + ], + ) -> Optional[npt.NDArray[np.float64]]: """Convert a configuration dictionary into a configuration vector. Parameters @@ -2999,10 +290,10 @@ def cfg_to_vector(self, cfg): vec[i] = cfg[jn] return vec else: - raise ValueError("Invalid configuration: {}".format(cfg)) + raise ValueError(f"Invalid configuration: {cfg!r}") @property - def base_link(self): + def base_link(self) -> Link: """:class:`.Link`: The base link for the URDF. The base link is the single link that has no parent. @@ -3010,7 +301,7 @@ def base_link(self): return self._base_link @property - def end_links(self): + def end_links(self) -> list[Link]: """list of :class:`.Link`: The end links for the URDF. The end links are the links that have no children. @@ -3018,7 +309,7 @@ def end_links(self): return self._end_links @property - def joint_limit_cfgs(self): + def joint_limit_cfgs(self) -> tuple[dict[Joint, float], dict[Joint, float]]: """tuple of dict : The lower-bound and upper-bound joint configuration maps. @@ -3027,8 +318,8 @@ def joint_limit_cfgs(self): The second map is the upper-bound map, which maps limited joints to their upper joint limits. """ - lb = {} - ub = {} + lb: dict[Joint, float] = {} + ub: dict[Joint, float] = {} for joint in self.actuated_joints: if joint.limit is not None: if joint.limit.lower is not None: @@ -3038,11 +329,11 @@ def joint_limit_cfgs(self): return (lb, ub) @property - def joint_limits(self): + def joint_limits(self) -> npt.NDArray[np.float64]: """(n,2) float : A lower and upper limit for each joint.""" limits = [] for joint in self.actuated_joints: - limit = [-np.infty, np.infty] + limit = [-np.inf, np.inf] if joint.limit is not None: if joint.limit.lower is not None: limit[0] = joint.limit.lower @@ -3051,7 +342,22 @@ def joint_limits(self): limits.append(limit) return np.array(limits) - def link_fk(self, cfg=None, link=None, links=None, use_names=False): + def link_fk( + self, + cfg: Union[ + Mapping[str, float], + Sequence[float], + npt.ArrayLike, + None, + ] = None, + link: Optional[Union[str, Link]] = None, + links: Optional[Sequence[Union[str, Link]]] = None, + use_names: bool = False, + ) -> Union[ + dict[Link, npt.NDArray[np.float64]], + dict[str, npt.NDArray[np.float64]], + npt.NDArray[np.float64], + ]: """Computes the poses of the URDF's links via forward kinematics. Parameters @@ -3083,32 +389,30 @@ def link_fk(self, cfg=None, link=None, links=None, use_names=False): joint_cfg = self._process_cfg(cfg) # Process link set - link_set = set() + link_set: set[Link] = set() if link is not None: - if isinstance(link, six.string_types): + if isinstance(link, str): link_set.add(self._link_map[link]) elif isinstance(link, Link): link_set.add(link) elif links is not None: for lnk in links: - if isinstance(lnk, six.string_types): + if isinstance(lnk, str): link_set.add(self._link_map[lnk]) elif isinstance(lnk, Link): link_set.add(lnk) else: - raise TypeError( - "Got object of type {} in links list".format(type(lnk)) - ) + raise TypeError("Got object of type {} in links list".format(type(lnk))) else: - link_set = self.links + link_set = set(self.links) # Compute forward kinematics in reverse topological order - fk = OrderedDict() + fk: "OrderedDict[Link, npt.NDArray[np.float64]]" = OrderedDict() for lnk in self._reverse_topo: if lnk not in link_set: continue pose = np.eye(4, dtype=np.float64) - path = self._paths_to_base[lnk] + path = cast(list[Link], self._paths_to_base[lnk]) for i in range(len(path) - 1): child = path[i] parent = path[i + 1] @@ -3131,7 +435,7 @@ def link_fk(self, cfg=None, link=None, links=None, use_names=False): fk[lnk] = pose if link: - if isinstance(link, six.string_types): + if isinstance(link, str): return fk[self._link_map[link]] else: return fk[link] @@ -3171,20 +475,18 @@ def link_fk_batch(self, cfgs=None, link=None, links=None, use_names=False): # Process link set link_set = set() if link is not None: - if isinstance(link, six.string_types): + if isinstance(link, str): link_set.add(self._link_map[link]) elif isinstance(link, Link): link_set.add(link) elif links is not None: for lnk in links: - if isinstance(lnk, six.string_types): + if isinstance(lnk, str): link_set.add(self._link_map[lnk]) elif isinstance(lnk, Link): link_set.add(lnk) else: - raise TypeError( - "Got object of type {} in links list".format(type(lnk)) - ) + raise TypeError("Got object of type {} in links list".format(type(lnk))) else: link_set = self.links @@ -3205,9 +507,7 @@ def link_fk_batch(self, cfgs=None, link=None, links=None, use_names=False): mimic_joint = self._joint_map[joint.mimic.joint] if mimic_joint in joint_cfgs: cfg_vals = joint_cfgs[mimic_joint] - cfg_vals = ( - joint.mimic.multiplier * cfg_vals + joint.mimic.offset - ) + cfg_vals = joint.mimic.multiplier * cfg_vals + joint.mimic.offset elif joint in joint_cfgs: cfg_vals = joint_cfgs[joint] poses = np.matmul(joint.get_child_poses(cfg_vals, n_cfgs), poses) @@ -3218,7 +518,7 @@ def link_fk_batch(self, cfgs=None, link=None, links=None, use_names=False): fk[lnk] = poses if link: - if isinstance(link, six.string_types): + if isinstance(link, str): return fk[self._link_map[link]] else: return fk[link] @@ -3226,7 +526,16 @@ def link_fk_batch(self, cfgs=None, link=None, links=None, use_names=False): return {ell.name: fk[ell] for ell in fk} return fk - def visual_geometry_fk(self, cfg=None, links=None): + def visual_geometry_fk( + self, + cfg: Union[ + Mapping[str, float], + Sequence[float], + npt.ArrayLike, + None, + ] = None, + links: Optional[Sequence[Union[str, Link]]] = None, + ) -> dict: """Computes the poses of the URDF's visual geometries using fk. Parameters @@ -3249,7 +558,7 @@ def visual_geometry_fk(self, cfg=None, links=None): elements of the specified links to the 4x4 homogenous transform matrices that position them relative to the base link's frame. """ - lfk = self.link_fk(cfg=cfg, links=links) + lfk = cast(dict[Link, npt.NDArray[np.float64]], self.link_fk(cfg=cfg, links=links)) fk = OrderedDict() for link in lfk: @@ -3257,7 +566,16 @@ def visual_geometry_fk(self, cfg=None, links=None): fk[visual.geometry] = lfk[link].dot(visual.origin) return fk - def visual_geometry_fk_batch(self, cfgs=None, links=None): + def visual_geometry_fk_batch( + self, + cfgs: Union[ + Mapping[str, Sequence[float]], + Sequence[Union[Mapping[str, float], None]], + npt.ArrayLike, + None, + ] = None, + links: Optional[Sequence[Union[str, Link]]] = None, + ) -> dict: """Computes the poses of the URDF's visual geometries using fk. Parameters @@ -3279,7 +597,7 @@ def visual_geometry_fk_batch(self, cfgs=None, links=None): elements of the specified links to the 4x4 homogenous transform matrices that position them relative to the base link's frame. """ - lfk = self.link_fk_batch(cfgs=cfgs, links=links) + lfk: dict[Link, npt.NDArray[np.float64]] = self.link_fk_batch(cfgs=cfgs, links=links) fk = OrderedDict() for link in lfk: @@ -3287,7 +605,16 @@ def visual_geometry_fk_batch(self, cfgs=None, links=None): fk[visual.geometry] = np.matmul(lfk[link], visual.origin) return fk - def visual_trimesh_fk(self, cfg=None, links=None): + def visual_trimesh_fk( + self, + cfg: Union[ + Mapping[str, float], + Sequence[float], + npt.ArrayLike, + None, + ] = None, + links: Optional[Sequence[Union[str, Link]]] = None, + ) -> dict[trimesh.Trimesh, npt.NDArray[np.float64]]: """Computes the poses of the URDF's visual trimeshes using fk. Parameters @@ -3311,7 +638,7 @@ def visual_trimesh_fk(self, cfg=None, links=None): 4x4 homogenous transform matrices that position them relative to the base link's frame. """ - lfk = self.link_fk(cfg=cfg, links=links) + lfk = cast(dict[Link, npt.NDArray[np.float64]], self.link_fk(cfg=cfg, links=links)) self.mesh_name_list = [] fk = OrderedDict() for link in lfk: @@ -3323,21 +650,15 @@ def visual_trimesh_fk(self, cfg=None, links=None): if visual.geometry.mesh.scale is not None: if ( np.sum( - visual.geometry.mesh.scale - != abs(visual.geometry.mesh.scale) + visual.geometry.mesh.scale != abs(visual.geometry.mesh.scale) ) > 0 ): - if ( - visual.geometry.mesh.filename - not in self.mesh_need_to_mirror - ): + if visual.geometry.mesh.filename not in self.mesh_need_to_mirror: print( f"[urchin]: {visual.geometry.mesh.filename} needs to mirror" ) - self.mesh_need_to_mirror.append( - visual.geometry.mesh.filename - ) + self.mesh_need_to_mirror.append(visual.geometry.mesh.filename) mesh_vertices = np.copy(mesh.vertices) mesh_faces = np.copy(mesh.faces) mesh_faces_new = np.hstack( @@ -3348,12 +669,8 @@ def visual_trimesh_fk(self, cfg=None, links=None): ] ) mesh = trimesh.Trimesh() - mirror_axis = np.where( - visual.geometry.mesh.scale < 0 - )[0][0] - mesh_vertices[:, mirror_axis] = -mesh_vertices[ - :, mirror_axis - ] + mirror_axis = np.where(visual.geometry.mesh.scale < 0)[0][0] + mesh_vertices[:, mirror_axis] = -mesh_vertices[:, mirror_axis] mesh.vertices = mesh_vertices mesh.faces = mesh_faces_new visual.geometry.meshes[i] = mesh @@ -3367,7 +684,16 @@ def visual_trimesh_fk(self, cfg=None, links=None): fk[mesh] = pose return fk - def visual_trimesh_fk_batch(self, cfgs=None, links=None): + def visual_trimesh_fk_batch( + self, + cfgs: Union[ + Mapping[str, Sequence[float]], + Sequence[Union[Mapping[str, float], None]], + npt.ArrayLike, + None, + ] = None, + links: Optional[Sequence[Union[str, Link]]] = None, + ) -> dict[trimesh.Trimesh, npt.NDArray[np.float64]]: """Computes the poses of the URDF's visual trimeshes using fk. Parameters @@ -3558,20 +884,20 @@ def animate(self, cfg_trajectory=None, loop_time=3.0, use_collision=False): You can run this without specifying a ``cfg_trajectory`` to view the full articulation of the URDF - >>> robot = URDF.load('ur5.urdf') + >>> robot = URDF.load("ur5.urdf") >>> robot.animate() .. image:: /_static/ur5.gif - >>> ct = {'shoulder_pan_joint': [0.0, 2 * np.pi]} + >>> ct = {"shoulder_pan_joint": [0.0, 2 * np.pi]} >>> robot.animate(cfg_trajectory=ct) .. image:: /_static/ur5_shoulder.gif >>> ct = { - ... 'shoulder_pan_joint' : [-np.pi / 4, np.pi / 4], - ... 'shoulder_lift_joint' : [0.0, -np.pi / 2.0], - ... 'elbow_joint' : [0.0, np.pi / 2.0] + ... "shoulder_pan_joint": [-np.pi / 4, np.pi / 4], + ... "shoulder_lift_joint": [0.0, -np.pi / 2.0], + ... "elbow_joint": [0.0, np.pi / 2.0], ... } >>> robot.animate(cfg_trajectory=ct) @@ -3611,9 +937,7 @@ def animate(self, cfg_trajectory=None, loop_time=3.0, use_collision=False): raise ValueError("Cfg trajectory must have entry for each joint") ct_np = {j: ct[:, i] for i, j in enumerate(self.actuated_joints)} else: - raise TypeError( - "Invalid type for cfg_trajectory: {}".format(type(cfg_trajectory)) - ) + raise TypeError("Invalid type for cfg_trajectory: {}".format(type(cfg_trajectory))) # If there isn't a trajectory to render, just show the model and exit if len(ct_np) == 0 or traj_len < 2: @@ -3637,10 +961,7 @@ def animate(self, cfg_trajectory=None, loop_time=3.0, use_collision=False): # Create the new interpolated trajectory new_ct = {} for k in ct_np: - new_ct[k] = ( - alphas * ct_np[k][right_inds - 1] - + (1.0 - alphas) * ct_np[k][right_inds] - ) + new_ct[k] = alphas * ct_np[k][right_inds - 1] + (1.0 - alphas) * ct_np[k][right_inds] # Create the scene if use_collision: @@ -3683,7 +1004,16 @@ def animate(self, cfg_trajectory=None, loop_time=3.0, use_collision=False): time.sleep(1.0 / fps) - def show(self, cfg=None, use_collision=False): + def show( + self, + cfg: Union[ + Mapping[str, float], + Sequence[float], + npt.ArrayLike, + None, + ] = None, + use_collision: bool = False, + ) -> None: """Visualize the URDF in a given configuration. Parameters @@ -3712,7 +1042,13 @@ def show(self, cfg=None, use_collision=False): scene.add(mesh, pose=pose) pyribbit.Viewer(scene, use_raymond_lighting=True) - def copy(self, name=None, prefix="", scale=None, collision_only=False): + def copy( + self, + name: Optional[str] = None, + prefix: str = "", + scale: Union[float, Sequence[float], None] = None, + collision_only: bool = False, + ) -> "URDF": """Make a deep copy of the URDF. Parameters @@ -3736,11 +1072,11 @@ def copy(self, name=None, prefix="", scale=None, collision_only=False): links=[v.copy(prefix, scale, collision_only) for v in self.links], joints=[v.copy(prefix, scale) for v in self.joints], transmissions=[v.copy(prefix, scale) for v in self.transmissions], - materials=[v.copy(prefix, scale) for v in self.materials], + materials=[v.copy(prefix) for v in self.materials], other_xml=self.other_xml, ) - def save(self, file_obj): + def save(self, file_obj: Union[str, IO[bytes], IO[str]]) -> None: """Save this URDF to a file. Parameters @@ -3753,10 +1089,10 @@ def save(self, file_obj): Returns ------- - urdf : :class:`.URDF` - The parsed URDF. + None + Nothing. Writes the URDF XML to ``file_obj``. """ - if isinstance(file_obj, six.string_types): + if isinstance(file_obj, str): path, _ = os.path.split(file_obj) else: path, _ = os.path.split(os.path.realpath(file_obj.name)) @@ -3765,7 +1101,14 @@ def save(self, file_obj): tree = ET.ElementTree(node) tree.write(file_obj, pretty_print=True, xml_declaration=True, encoding="utf-8") - def join(self, other, link, origin=None, name=None, prefix=""): + def join( + self, + other: "URDF", + link: Union[Link, str], + origin: Optional[npt.ArrayLike] = None, + name: Optional[str] = None, + prefix: str = "", + ) -> "URDF": """Join another URDF to this one by rigidly fixturing the two at a link. Parameters @@ -3829,7 +1172,7 @@ def join(self, other, link, origin=None, name=None, prefix=""): materials=materials, ) - def _merge_materials(self): + def _merge_materials(self) -> None: """Merge the top-level material set with the link materials.""" for link in self.links: for v in link.visuals: @@ -3842,7 +1185,9 @@ def _merge_materials(self): self._material_map[v.material.name] = v.material @classmethod - def load(cls, file_obj, lazy_load_meshes=False): + def load( + cls, file_obj: Union[str, IO[bytes], IO[str]], lazy_load_meshes: bool = False + ) -> "URDF": """Load a URDF from a file. Parameters @@ -3862,7 +1207,7 @@ def load(cls, file_obj, lazy_load_meshes=False): urdf : :class:`.URDF` The parsed URDF. """ - if isinstance(file_obj, six.string_types): + if isinstance(file_obj, str): if os.path.isfile(file_obj): parser = ET.XMLParser(remove_comments=True, remove_blank_text=True) tree = ET.parse(file_obj, parser=parser) @@ -3895,20 +1240,14 @@ def _validate_joints(self): for joint in self.joints: if joint.parent not in self._link_map: raise ValueError( - "Joint {} has invalid parent link name {}".format( - joint.name, joint.parent - ) + "Joint {} has invalid parent link name {}".format(joint.name, joint.parent) ) if joint.child not in self._link_map: raise ValueError( - "Joint {} has invalid child link name {}".format( - joint.name, joint.child - ) + "Joint {} has invalid child link name {}".format(joint.name, joint.child) ) if joint.child == joint.parent: - raise ValueError( - "Joint {} has matching parent and child".format(joint.name) - ) + raise ValueError("Joint {} has matching parent and child".format(joint.name)) if joint.mimic is not None: if joint.mimic.joint not in self._joint_map: raise ValueError( @@ -3917,16 +1256,14 @@ def _validate_joints(self): ) ) if joint.mimic.joint == joint.name: - raise ValueError( - "Joint {} set up to mimic itself".format(joint.mimic.joint) - ) + raise ValueError("Joint {} set up to mimic itself".format(joint.mimic.joint)) elif joint.joint_type != "fixed": actuated_joints.append(joint) # Do a depth-first search return actuated_joints - def _sort_joints(self, joints): + def _sort_joints(self, joints: list[Joint]) -> list[Joint]: """Sort joints by ascending distance from the base link (topologically). Parameters @@ -3946,7 +1283,7 @@ def _sort_joints(self, joints): order = np.argsort(lens) return np.array(joints)[order].tolist() - def _validate_transmissions(self): + def _validate_transmissions(self) -> None: """Raise an exception of any transmissions are invalidly specified. Checks for the following: @@ -3957,11 +1294,10 @@ def _validate_transmissions(self): for joint in t.joints: if joint.name not in self._joint_map: raise ValueError( - "Transmission {} has invalid joint name " - "{}".format(t.name, joint.name) + "Transmission {} has invalid joint name {}".format(t.name, joint.name) ) - def _validate_graph(self): + def _validate_graph(self) -> tuple[Link, list[Link]]: """Raise an exception if the link-joint structure is invalid. Checks for the following: @@ -3986,7 +1322,7 @@ def _validate_graph(self): for n in cc: cluster.append(n.name) link_clusters.append(cluster) - message = "Links are not all connected. " "Connected components are:" + message = "Links are not all connected. Connected components are:" for lc in link_clusters: message += "\n\t" for n in lc: @@ -3998,40 +1334,47 @@ def _validate_graph(self): raise ValueError("There are cycles in the link graph") # Ensure that there is exactly one base link, which has no parent - base_link = None - end_links = [] + base_link: Optional[Link] = None + end_links: list[Link] = [] for n in self._G: if len(nx.descendants(self._G, n)) == 0: if base_link is None: base_link = n else: raise ValueError( - "Links {} and {} are both base links!".format( - n.name, base_link.name - ) + "Links {} and {} are both base links!".format(n.name, base_link.name) ) if len(nx.ancestors(self._G, n)) == 0: end_links.append(n) + if base_link is None: + raise ValueError("URDF has no base link") return base_link, end_links - def _process_cfg(self, cfg): + def _process_cfg( + self, + cfg: Union[ + Mapping[str, float], + Sequence[float], + npt.ArrayLike, + None, + ], + ) -> dict[Joint, float]: """Process a joint configuration spec into a dictionary mapping joints to configuration values. """ - joint_cfg = {} + joint_cfg: dict[Joint, float] = {} if cfg is None: return joint_cfg if isinstance(cfg, dict): for joint in cfg: - if isinstance(joint, six.string_types): + if isinstance(joint, str): joint_cfg[self._joint_map[joint]] = cfg[joint] elif isinstance(joint, Joint): joint_cfg[joint] = cfg[joint] elif isinstance(cfg, (list, tuple, np.ndarray)): if len(cfg) != len(self.actuated_joints): raise ValueError( - "Cfg must have same length as actuated joints " - "if specified as a numerical array" + "Cfg must have same length as actuated joints if specified as a numerical array" ) for joint, value in zip(self.actuated_joints, cfg): joint_cfg[joint] = value @@ -4039,18 +1382,32 @@ def _process_cfg(self, cfg): raise TypeError("Invalid type for config") return joint_cfg - def _process_cfgs(self, cfgs): + def _process_cfgs( + self, + cfgs: Union[ + Mapping[str, Sequence[float]], + Sequence[Union[Mapping[str, float], None]], + npt.ArrayLike, + None, + ], + ) -> tuple[ + dict[Joint, Union[Sequence[float], npt.NDArray[np.float64], None]], + Optional[int], + ]: """Process a list of joint configurations into a dictionary mapping joints to configuration values. This should result in a dict mapping each joint to a list of cfg values, one per joint. """ - joint_cfg = {j: [] for j in self.actuated_joints} + joint_cfg: dict[ + Joint, + Union[list[float], npt.NDArray[np.float64], None], + ] = {j: [] for j in self.actuated_joints} n_cfgs = None if isinstance(cfgs, dict): for joint in cfgs: - if isinstance(joint, six.string_types): + if isinstance(joint, str): joint_cfg[self._joint_map[joint]] = cfgs[joint] else: joint_cfg[joint] = cfgs[joint] @@ -4061,42 +1418,73 @@ def _process_cfgs(self, cfgs): if isinstance(cfgs[0], dict): for cfg in cfgs: for joint in cfg: - if isinstance(joint, six.string_types): - joint_cfg[self._joint_map[joint]].append(cfg[joint]) + if isinstance(joint, str): + v = joint_cfg[self._joint_map[joint]] + assert isinstance(v, list) + v.append(cfg[joint]) + joint_cfg[self._joint_map[joint]] = v else: - joint_cfg[joint].append(cfg[joint]) + v2 = joint_cfg[joint] + assert isinstance(v2, list) + v2.append(cfg[joint]) + joint_cfg[joint] = v2 elif cfgs[0] is None: pass else: cfgs = np.asanyarray(cfgs, dtype=np.float64) for i, j in enumerate(self.actuated_joints): - joint_cfg[j] = cfgs[:, i] + joint_cfg[j] = cast(npt.NDArray[np.float64], cfgs[:, i]) else: raise ValueError("Incorrectly formatted config array") for j in joint_cfg: - if len(joint_cfg[j]) == 0: - joint_cfg[j] = None - elif len(joint_cfg[j]) != n_cfgs: - raise ValueError("Inconsistent number of configurations for joints") + if isinstance(joint_cfg[j], list): + from typing import cast as _cast - return joint_cfg, n_cfgs + if len(_cast(list[float], joint_cfg[j])) == 0: + joint_cfg[j] = None + elif n_cfgs is not None and len(_cast(list[float], joint_cfg[j])) != n_cfgs: + raise ValueError("Inconsistent number of configurations for joints") - @classmethod - def _from_xml(cls, node, path, lazy_load_meshes): - valid_tags = set(["joint", "link", "transmission", "material"]) - kwargs = cls._parse(node, path, lazy_load_meshes) + from typing import cast as _cast + return _cast( + dict[ + Joint, + Union[Sequence[float], npt.NDArray[np.float64], None], + ], + joint_cfg, + ), n_cfgs + + @classmethod + def _from_xml( + cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None + ) -> "URDF": + # Explicit parse of URDF components for typing clarity + name = str(node.attrib.get("name", "")) + links = [Link._from_xml(n, path, lazy_load_meshes) for n in node.findall("link")] + joints = [Joint._from_xml(n, path) for n in node.findall("joint")] + transmissions = [Transmission._from_xml(n, path) for n in node.findall("transmission")] + materials = [Material._from_xml(n, path) for n in node.findall("material")] + + # Capture any extra XML + valid_tags = {"joint", "link", "transmission", "material"} extra_xml_node = ET.Element("extra") for child in node: if child.tag not in valid_tags: extra_xml_node.append(child) + other_xml = ET.tostring(extra_xml_node) - data = ET.tostring(extra_xml_node) - kwargs["other_xml"] = data - return cls(**kwargs) + return cls( + name=name, + links=links, + joints=joints, + transmissions=transmissions, + materials=materials, + other_xml=other_xml, + ) - def _to_xml(self, parent, path): + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: node = self._unparse(path) if self.other_xml: extra_tree = ET.fromstring(self.other_xml) diff --git a/urchin/utils.py b/urchin/utils.py index a822416..36be7c1 100644 --- a/urchin/utils.py +++ b/urchin/utils.py @@ -1,13 +1,17 @@ -"""Utilities for URDF parsing. -""" +"""Utilities for URDF parsing.""" + +from __future__ import annotations + import os +from typing import Sequence, Union -from lxml import etree as ET import numpy as np +import numpy.typing as npt import trimesh +from lxml import etree as ET -def rpy_to_matrix(coords): +def rpy_to_matrix(coords: npt.ArrayLike) -> npt.NDArray[np.float64]: """Convert roll-pitch-yaw coordinates to a 3x3 homogenous rotation matrix. The roll-pitch-yaw axes in a typical URDF are defined as a @@ -32,14 +36,17 @@ def rpy_to_matrix(coords): c3, c2, c1 = np.cos(coords) s3, s2, s1 = np.sin(coords) - return np.array([ - [c1 * c2, (c1 * s2 * s3) - (c3 * s1), (s1 * s3) + (c1 * c3 * s2)], - [c2 * s1, (c1 * c3) + (s1 * s2 * s3), (c3 * s1 * s2) - (c1 * s3)], - [-s2, c2 * s3, c2 * c3] - ], dtype=np.float64) + return np.array( + [ + [c1 * c2, (c1 * s2 * s3) - (c3 * s1), (s1 * s3) + (c1 * c3 * s2)], + [c2 * s1, (c1 * c3) + (s1 * s2 * s3), (c3 * s1 * s2) - (c1 * s3)], + [-s2, c2 * s3, c2 * c3], + ], + dtype=np.float64, + ) -def matrix_to_rpy(R, solution=1): +def matrix_to_rpy(R: npt.ArrayLike, solution: int = 1) -> npt.NDArray[np.float64]: """Convert a 3x3 transform matrix to roll-pitch-yaw coordinates. The roll-pitchRyaw axes in a typical URDF are defined as a @@ -71,26 +78,26 @@ def matrix_to_rpy(R, solution=1): p = 0.0 y = 0.0 - if np.abs(R[2,0]) >= 1.0 - 1e-12: + if np.abs(R[2, 0]) >= 1.0 - 1e-12: y = 0.0 - if R[2,0] < 0: + if R[2, 0] < 0: p = np.pi / 2 - r = np.arctan2(R[0,1], R[0,2]) + r = np.arctan2(R[0, 1], R[0, 2]) else: p = -np.pi / 2 - r = np.arctan2(-R[0,1], -R[0,2]) + r = np.arctan2(-R[0, 1], -R[0, 2]) else: if solution == 1: - p = -np.arcsin(R[2,0]) + p = -np.arcsin(R[2, 0]) else: - p = np.pi + np.arcsin(R[2,0]) - r = np.arctan2(R[2,1] / np.cos(p), R[2,2] / np.cos(p)) - y = np.arctan2(R[1,0] / np.cos(p), R[0,0] / np.cos(p)) + p = np.pi + np.arcsin(R[2, 0]) + r = np.arctan2(R[2, 1] / np.cos(p), R[2, 2] / np.cos(p)) + y = np.arctan2(R[1, 0] / np.cos(p), R[0, 0] / np.cos(p)) return np.array([r, p, y], dtype=np.float64) -def matrix_to_xyz_rpy(matrix): +def matrix_to_xyz_rpy(matrix: npt.ArrayLike) -> npt.NDArray[np.float64]: """Convert a 4x4 homogenous matrix to xyzrpy coordinates. Parameters @@ -103,12 +110,13 @@ def matrix_to_xyz_rpy(matrix): xyz_rpy : (6,) float The xyz_rpy vector. """ - xyz = matrix[:3,3] - rpy = matrix_to_rpy(matrix[:3,:3]) + M = np.asanyarray(matrix, dtype=np.float64) + xyz = M[:3, 3] + rpy = matrix_to_rpy(M[:3, :3]) return np.hstack((xyz, rpy)) -def xyz_rpy_to_matrix(xyz_rpy): +def xyz_rpy_to_matrix(xyz_rpy: npt.ArrayLike) -> npt.NDArray[np.float64]: """Convert xyz_rpy coordinates to a 4x4 homogenous matrix. Parameters @@ -122,12 +130,13 @@ def xyz_rpy_to_matrix(xyz_rpy): The homogenous transform matrix. """ matrix = np.eye(4, dtype=np.float64) - matrix[:3,3] = xyz_rpy[:3] - matrix[:3,:3] = rpy_to_matrix(xyz_rpy[3:]) + arr = np.asanyarray(xyz_rpy, dtype=np.float64) + matrix[:3, 3] = arr[:3] + matrix[:3, :3] = rpy_to_matrix(arr[3:]) return matrix -def parse_origin(node): +def parse_origin(node: ET._Element) -> npt.NDArray[np.float64]: """Find the ``origin`` subelement of an XML node and convert it into a 4x4 homogenous transformation matrix. @@ -145,17 +154,17 @@ def parse_origin(node): child was found. """ matrix = np.eye(4, dtype=np.float64) - origin_node = node.find('origin') + origin_node = node.find("origin") if origin_node is not None: - if 'xyz' in origin_node.attrib: - matrix[:3,3] = np.fromstring(origin_node.attrib['xyz'], sep=' ') - if 'rpy' in origin_node.attrib: - rpy = np.fromstring(origin_node.attrib['rpy'], sep=' ') - matrix[:3,:3] = rpy_to_matrix(rpy) + if "xyz" in origin_node.attrib: + matrix[:3, 3] = np.fromstring(origin_node.attrib["xyz"], sep=" ") + if "rpy" in origin_node.attrib: + rpy = np.fromstring(origin_node.attrib["rpy"], sep=" ") + matrix[:3, :3] = rpy_to_matrix(rpy) return matrix -def unparse_origin(matrix): +def unparse_origin(matrix: npt.ArrayLike) -> ET._Element: """Turn a 4x4 homogenous matrix into an ``origin`` XML node. Parameters @@ -174,13 +183,14 @@ def unparse_origin(matrix): - ``rpy`` - A string with three space-delimited floats representing the rotation of the origin. """ - node = ET.Element('origin') - node.attrib['xyz'] = '{} {} {}'.format(*matrix[:3,3]) - node.attrib['rpy'] = '{} {} {}'.format(*matrix_to_rpy(matrix[:3,:3])) + node = ET.Element("origin") + M = np.asanyarray(matrix, dtype=np.float64) + node.attrib["xyz"] = "{} {} {}".format(*M[:3, 3]) + node.attrib["rpy"] = "{} {} {}".format(*matrix_to_rpy(M[:3, :3])) return node -def get_filename(base_path, file_path, makedirs=False): +def get_filename(base_path: str, file_path: str, makedirs: bool = False) -> str: """Formats a file path correctly for URDF loading. Parameters @@ -209,7 +219,7 @@ def get_filename(base_path, file_path, makedirs=False): return fn -def load_meshes(filename): +def load_meshes(filename: str) -> list[trimesh.Trimesh]: """Loads triangular meshes from a file. Parameters @@ -222,29 +232,30 @@ def load_meshes(filename): meshes : list of :class:`~trimesh.base.Trimesh` The meshes loaded from the file. """ - meshes = trimesh.load(filename) + meshes_obj: trimesh.Geometry = trimesh.load(filename) # If we got a scene, dump the meshes - if isinstance(meshes, trimesh.Scene): - meshes = list(meshes.dump()) - meshes = [g for g in meshes if isinstance(g, trimesh.Trimesh)] - - if isinstance(meshes, (list, tuple, set)): - meshes = list(meshes) + if isinstance(meshes_obj, trimesh.Scene): + dumped = list(meshes_obj.dump()) + meshes: list[trimesh.Trimesh] = [g for g in dumped if isinstance(g, trimesh.Trimesh)] + elif isinstance(meshes_obj, trimesh.Trimesh): + meshes = [meshes_obj] + elif isinstance(meshes_obj, (list, tuple, set)): + meshes = list(meshes_obj) if len(meshes) == 0: - raise ValueError('At least one mesh must be pmeshesent in file') + raise ValueError("At least one mesh must be pmeshesent in file") for r in meshes: if not isinstance(r, trimesh.Trimesh): - raise TypeError('Could not load meshes from file') - elif isinstance(meshes, trimesh.Trimesh): - meshes = [meshes] + raise TypeError("Could not load meshes from file") else: - raise ValueError('Unable to load mesh from file') + raise ValueError("Unable to load mesh from file") return meshes -def configure_origin(value): +def configure_origin( + value: Union[None, Sequence[float], npt.ArrayLike], +) -> npt.NDArray[np.float64]: """Convert a value into a 4x4 transform matrix. Parameters @@ -255,8 +266,8 @@ def configure_origin(value): Returns ------- - matrix : (4,4) float or None - The created matrix. + matrix : (4,4) float + The created matrix. If ``value`` is ``None``, returns the identity. """ if value is None: value = np.eye(4, dtype=np.float64) @@ -264,9 +275,8 @@ def configure_origin(value): value = np.asanyarray(value, dtype=np.float64) if value.shape == (6,): value = xyz_rpy_to_matrix(value) - elif value.shape != (4,4): - raise ValueError('Origin must be specified as a 4x4 ' - 'homogenous transformation matrix') + elif value.shape != (4, 4): + raise ValueError("Origin must be specified as a 4x4 homogenous transformation matrix") else: - raise TypeError('Invalid type for origin, expect 4x4 matrix') + raise TypeError("Invalid type for origin, expect 4x4 matrix") return value diff --git a/urchin/version.py b/urchin/version.py index 573b11c..ca5c44e 100644 --- a/urchin/version.py +++ b/urchin/version.py @@ -1 +1 @@ -__version__ = "0.0.27" +__version__ = "0.0.30"