From fc6b879f0c3a8d9c3ec7eb87ae60b1ad4e9a4cb4 Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Thu, 15 May 2025 21:51:12 -0700 Subject: [PATCH 01/13] Split classes into separate file to improve readability --- setup.py | 2 +- urchin/__init__.py | 16 +- urchin/base.py | 367 ++++++ urchin/joint.py | 869 +++++++++++++ urchin/link.py | 968 ++++++++++++++ urchin/material.py | 201 +++ urchin/transmission.py | 315 +++++ urchin/urdf.py | 2754 +--------------------------------------- urchin/version.py | 2 +- 9 files changed, 2746 insertions(+), 2748 deletions(-) create mode 100644 urchin/base.py create mode 100644 urchin/joint.py create mode 100644 urchin/link.py create mode 100644 urchin/material.py create mode 100644 urchin/transmission.py diff --git a/setup.py b/setup.py index a405e05..a325d38 100644 --- a/setup.py +++ b/setup.py @@ -31,7 +31,7 @@ setup( name="urchin", - version="0.0.29", + version="0.0.30", 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", diff --git a/urchin/__init__.py b/urchin/__init__.py index 460e48e..0d48040 100644 --- a/urchin/__init__.py +++ b/urchin/__init__.py @@ -1,12 +1,10 @@ -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 JointCalibration, JointDynamics, JointLimit, JointMimic, SafetyController, Joint +from .transmission import Actuator, TransmissionJoint, Transmission +from .link import Box, Cylinder, Sphere, Mesh, Geometry, Collision, Visual, Inertial, Link +from .material import Texture, Material +from .urdf import URDF +from .utils import (rpy_to_matrix, matrix_to_rpy, xyz_rpy_to_matrix, matrix_to_xyz_rpy) from .version import __version__ __all__ = [ diff --git a/urchin/base.py b/urchin/base.py new file mode 100644 index 0000000..182fc7b --- /dev/null +++ b/urchin/base.py @@ -0,0 +1,367 @@ +import numpy as np +from lxml import etree as ET + + +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, 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, 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)) + diff --git a/urchin/joint.py b/urchin/joint.py new file mode 100644 index 0000000..7477b84 --- /dev/null +++ b/urchin/joint.py @@ -0,0 +1,869 @@ +from urchin.base import URDFType +from urchin.utils import configure_origin, parse_origin, unparse_origin +import numpy as np +from lxml import etree as ET +import trimesh + +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, + ) + +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 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 \ No newline at end of file diff --git a/urchin/link.py b/urchin/link.py new file mode 100644 index 0000000..3605304 --- /dev/null +++ b/urchin/link.py @@ -0,0 +1,968 @@ +from urchin.base import URDFType, URDFTypeWithMesh +import numpy as np +from urchin.utils import configure_origin, parse_origin, unparse_origin, get_filename, load_meshes +import trimesh +from urchin.material import Material +from lxml import etree as ET +import os + +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, 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, 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 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, + ) + +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. + + 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 + diff --git a/urchin/material.py b/urchin/material.py new file mode 100644 index 0000000..e921e38 --- /dev/null +++ b/urchin/material.py @@ -0,0 +1,201 @@ +from urchin.base import URDFType +from urchin.utils import get_filename + +import numpy as np +import PIL +from lxml import etree as ET + +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, 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, 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, + ) diff --git a/urchin/transmission.py b/urchin/transmission.py new file mode 100644 index 0000000..dfe2f7e --- /dev/null +++ b/urchin/transmission.py @@ -0,0 +1,315 @@ +from urchin.base import URDFType +from lxml import etree as ET + + +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 +############################################################################### + + +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], + ) + + diff --git a/urchin/urdf.py b/urchin/urdf.py index 80e8a05..30d932a 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -2,2736 +2,16 @@ import os import time from collections import OrderedDict +from lxml import etree as ET 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 -############################################################################### - - -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. - - 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.transmission import Transmission +from urchin.material import Material +from urchin.link import Link +from urchin.joint import Joint +from urchin.base import URDFTypeWithMesh class URDF(URDFTypeWithMesh): """The top-level URDF specification. @@ -3085,13 +365,13 @@ def link_fk(self, cfg=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) @@ -3131,7 +411,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,13 +451,13 @@ 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) @@ -3218,7 +498,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] @@ -3756,7 +1036,7 @@ def save(self, file_obj): urdf : :class:`.URDF` The parsed URDF. """ - 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)) @@ -3862,7 +1142,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) @@ -4023,7 +1303,7 @@ def _process_cfg(self, cfg): 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] @@ -4050,7 +1330,7 @@ def _process_cfgs(self, cfgs): 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,7 +1341,7 @@ def _process_cfgs(self, cfgs): if isinstance(cfgs[0], dict): for cfg in cfgs: for joint in cfg: - if isinstance(joint, six.string_types): + if isinstance(joint, str): joint_cfg[self._joint_map[joint]].append(cfg[joint]) else: joint_cfg[joint].append(cfg[joint]) 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" From 6caacba973579f6b6a26242b89e4db3c26f6f924 Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Thu, 15 May 2025 21:58:47 -0700 Subject: [PATCH 02/13] Cleaned with isort and black --- urchin/__init__.py | 62 +++++++++++++++++++++++++++------- urchin/base.py | 9 +++-- urchin/joint.py | 11 +++--- urchin/link.py | 27 ++++++++++----- urchin/material.py | 7 ++-- urchin/transmission.py | 5 ++- urchin/urdf.py | 12 ++++--- urchin/utils.py | 76 ++++++++++++++++++++++-------------------- 8 files changed, 134 insertions(+), 75 deletions(-) diff --git a/urchin/__init__.py b/urchin/__init__.py index 0d48040..4c38a6d 100644 --- a/urchin/__init__.py +++ b/urchin/__init__.py @@ -1,18 +1,56 @@ from .base import URDFType, URDFTypeWithMesh -from .joint import JointCalibration, JointDynamics, JointLimit, JointMimic, SafetyController, Joint -from .transmission import Actuator, TransmissionJoint, Transmission -from .link import Box, Cylinder, Sphere, Mesh, Geometry, Collision, Visual, Inertial, Link -from .material import Texture, Material +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 (rpy_to_matrix, matrix_to_rpy, xyz_rpy_to_matrix, matrix_to_xyz_rpy) +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 index 182fc7b..682b301 100644 --- a/urchin/base.py +++ b/urchin/base.py @@ -25,8 +25,12 @@ class URDFType: containing this type of object. """ - _ATTRIBS: dict[str, tuple[type, bool]] = {} # Map from attrib name to (type, required) - _ELEMENTS: dict[str, tuple[type, bool, bool]] = {} # Map from element name to (type, required, multiple) + _ATTRIBS: dict[str, tuple[type, bool]] = ( + {} + ) # Map from attrib name to (type, required) + _ELEMENTS: dict[str, tuple[type, bool, bool]] = ( + {} + ) # Map from element name to (type, required, multiple) _TAG: str = "" # XML tag for this element def __init__(self): @@ -364,4 +368,3 @@ def _from_xml(cls, node, path, lazy_load_meshes): 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 index 7477b84..4c81cc8 100644 --- a/urchin/joint.py +++ b/urchin/joint.py @@ -1,8 +1,10 @@ -from urchin.base import URDFType -from urchin.utils import configure_origin, parse_origin, unparse_origin import numpy as np -from lxml import etree as ET 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. @@ -107,6 +109,7 @@ def copy(self, prefix="", scale=None): soft_upper_limit=self.soft_upper_limit, ) + class JointCalibration(URDFType): """The reference positions of the joint. @@ -866,4 +869,4 @@ def copy(self, prefix="", scale=None): ), mimic=(self.mimic.copy(prefix=prefix, scale=scale) if self.mimic else None), ) - return cpy \ No newline at end of file + return cpy diff --git a/urchin/link.py b/urchin/link.py index 3605304..5bc0714 100644 --- a/urchin/link.py +++ b/urchin/link.py @@ -1,10 +1,19 @@ -from urchin.base import URDFType, URDFTypeWithMesh +import os + import numpy as np -from urchin.utils import configure_origin, parse_origin, unparse_origin, get_filename, load_meshes import trimesh -from urchin.material import Material from lxml import etree as ET -import os + +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): """A rectangular prism whose center is at the local origin. @@ -61,6 +70,7 @@ def copy(self, prefix="", scale=None): ) return b + class Cylinder(URDFType): """A cylinder whose center is at the local origin. @@ -210,6 +220,7 @@ def copy(self, prefix="", scale=None): ) return s + class Mesh(URDFTypeWithMesh): """A triangular mesh object. @@ -375,10 +386,6 @@ def copy(self, prefix="", scale=None): return m - - - - class Geometry(URDFTypeWithMesh): """A wrapper for all geometry types. @@ -506,6 +513,7 @@ def copy(self, prefix="", scale=None): ) return v + class Collision(URDFTypeWithMesh): """Collision properties of a link. @@ -706,6 +714,7 @@ def copy(self, prefix="", scale=None): material=(self.material.copy(prefix=prefix) if self.material else None), ) + class Inertial(URDFType): """The inertial properties of a link. @@ -812,6 +821,7 @@ def copy(self, prefix="", mass=None, origin=None, inertia=None): origin=origin, ) + class Link(URDFTypeWithMesh): """A link of a rigid object. @@ -965,4 +975,3 @@ def copy(self, prefix="", scale=None, collision_only=False): ) cpy._collision_mesh = cm return cpy - diff --git a/urchin/material.py b/urchin/material.py index e921e38..89028a6 100644 --- a/urchin/material.py +++ b/urchin/material.py @@ -1,10 +1,11 @@ -from urchin.base import URDFType -from urchin.utils import get_filename - import numpy as np 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. diff --git a/urchin/transmission.py b/urchin/transmission.py index dfe2f7e..ef7f8cb 100644 --- a/urchin/transmission.py +++ b/urchin/transmission.py @@ -1,6 +1,7 @@ -from urchin.base import URDFType from lxml import etree as ET +from urchin.base import URDFType + class Actuator(URDFType): """An actuator. @@ -311,5 +312,3 @@ def copy(self, prefix="", scale=None): 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 30d932a..c6f769a 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -2,16 +2,18 @@ import os import time from collections import OrderedDict -from lxml import etree as ET import networkx as nx import numpy as np import trimesh -from urchin.transmission import Transmission -from urchin.material import Material -from urchin.link import Link -from urchin.joint import Joint +from lxml import etree as ET + 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): """The top-level URDF specification. diff --git a/urchin/utils.py b/urchin/utils.py index a822416..3e05626 100644 --- a/urchin/utils.py +++ b/urchin/utils.py @@ -1,10 +1,10 @@ -"""Utilities for URDF parsing. -""" +"""Utilities for URDF parsing.""" + import os -from lxml import etree as ET import numpy as np import trimesh +from lxml import etree as ET def rpy_to_matrix(coords): @@ -32,11 +32,14 @@ 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): @@ -71,21 +74,21 @@ 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) @@ -103,8 +106,8 @@ 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]) + xyz = matrix[:3, 3] + rpy = matrix_to_rpy(matrix[:3, :3]) return np.hstack((xyz, rpy)) @@ -122,8 +125,8 @@ 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:]) + matrix[:3, 3] = xyz_rpy[:3] + matrix[:3, :3] = rpy_to_matrix(xyz_rpy[3:]) return matrix @@ -145,13 +148,13 @@ 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 @@ -174,9 +177,9 @@ 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") + node.attrib["xyz"] = "{} {} {}".format(*matrix[:3, 3]) + node.attrib["rpy"] = "{} {} {}".format(*matrix_to_rpy(matrix[:3, :3])) return node @@ -232,14 +235,14 @@ def load_meshes(filename): if isinstance(meshes, (list, tuple, set)): meshes = list(meshes) 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') + raise TypeError("Could not load meshes from file") elif isinstance(meshes, trimesh.Trimesh): meshes = [meshes] else: - raise ValueError('Unable to load mesh from file') + raise ValueError("Unable to load mesh from file") return meshes @@ -264,9 +267,10 @@ 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 From b905f504c28d181e71b163e22de59b33918d9827 Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Wed, 4 Jun 2025 12:51:31 -0700 Subject: [PATCH 03/13] Updated pyproject --- pyproject.toml | 52 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 pyproject.toml diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..9963168 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,52 @@ +[build-system] +requires = ["setuptools>=61.0", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "urchin" +version = "0.0.30" +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.7", + "Programming Language :: Python :: 3.8", + "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", # 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 +] \ No newline at end of file From 705c6c409b786d2cc9a79bd643ce85c80e03637c Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Wed, 4 Jun 2025 13:15:44 -0700 Subject: [PATCH 04/13] Removed setup.py and updated python version to 3.9 or higher --- pyproject.toml | 5 ++--- setup.py | 59 -------------------------------------------------- 2 files changed, 2 insertions(+), 62 deletions(-) delete mode 100644 setup.py diff --git a/pyproject.toml b/pyproject.toml index 9963168..373258d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -5,6 +5,7 @@ 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 = [ @@ -17,8 +18,6 @@ classifiers = [ "Development Status :: 4 - Beta", "License :: OSI Approved :: MIT License", "Programming Language :: Python", - "Programming Language :: Python :: 3.7", - "Programming Language :: Python :: 3.8", "Programming Language :: Python :: 3.9", "Programming Language :: Python :: 3.10", "Programming Language :: Python :: 3.11", @@ -49,4 +48,4 @@ docs = [ "sphinx", # General doc library "sphinx_rtd_theme", # RTD theme for sphinx "sphinx-automodapi" # For generating nice tables -] \ No newline at end of file +] diff --git a/setup.py b/setup.py deleted file mode 100644 index a325d38..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.30", - 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, - }, -) From c85854c9a46818c3bd7c440ef44a48b3e52bf973 Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Wed, 4 Jun 2025 13:21:15 -0700 Subject: [PATCH 05/13] Turned off pypi push unless in original repo and under a tag --- .github/workflows/publish-to-pypi.yml | 49 ++++++++++++--------------- 1 file changed, 22 insertions(+), 27 deletions(-) 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 }} From e3c2d21732cd5732f372b1003425165e435090fd Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Wed, 4 Jun 2025 13:22:18 -0700 Subject: [PATCH 06/13] Turned off 3.8 in github actions as well --- .github/workflows/test.yml | 35 +++++++++++++++++------------------ 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 188f0e9..eec9daf 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -4,31 +4,30 @@ 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 matrix: - python-version: ["3.8", "3.9", "3.10", "3.11", "3.12", "3.13"] + 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 From e60eda39f97d1abdf7d7d788dcd4ae42728db90e Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Mon, 6 Oct 2025 10:46:01 -0700 Subject: [PATCH 07/13] Added typing via codex --- .github/workflows/ci.yml | 44 ++++ .pre-commit-config.yaml | 21 ++ docs/source/conf.py | 4 +- pyproject.toml | 22 ++ scripts/vis_urdf.py | 6 +- tests/unit/test_urdf.py | 46 ++--- tests/unit/test_utils.py | 2 +- urchin/base.py | 77 ++++--- urchin/joint.py | 178 +++++++++-------- urchin/link.py | 419 ++++++++++++++++++++++++--------------- urchin/material.py | 92 +++++---- urchin/transmission.py | 134 +++++++------ urchin/urdf.py | 406 +++++++++++++++++++++---------------- urchin/utils.py | 64 +++--- 14 files changed, 907 insertions(+), 608 deletions(-) create mode 100644 .github/workflows/ci.yml diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..6d75cb1 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,44 @@ +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 ruff + + - name: Ruff Lint + run: | + ruff check . + ruff format --check . + + - name: Mypy + run: | + mypy urchin + + - name: Pytest + run: | + pytest -q + diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1817eb3..1f21f23 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,3 +4,24 @@ repos: hooks: - id: flake8 exclude: ^setup.py + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.6.5 + 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.7.1 + 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..00c9cce 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -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 index 373258d..e6f138f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -49,3 +49,25 @@ docs = [ "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.*"] +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/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/base.py b/urchin/base.py index 682b301..e3113eb 100644 --- a/urchin/base.py +++ b/urchin/base.py @@ -1,3 +1,7 @@ +from __future__ import annotations + +from typing import Dict, Type, cast + import numpy as np from lxml import etree as ET @@ -25,19 +29,17 @@ class URDFType: containing this type of object. """ - _ATTRIBS: dict[str, tuple[type, bool]] = ( - {} - ) # Map from attrib name to (type, required) - _ELEMENTS: dict[str, tuple[type, bool, bool]] = ( - {} - ) # Map from element name to (type, required, multiple) + _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, val): + def _parse_attrib(cls, val_type: type, val: str) -> object: """Parse an XML attribute into a python value. Parameters @@ -53,13 +55,13 @@ def _parse_attrib(cls, val_type, val): The parsed attribute. """ if val_type == np.ndarray: - val = np.fromstring(val, sep=" ") + parsed: object = np.fromstring(val, sep=" ") else: - val = val_type(val) - return val + parsed = val_type(val) + return parsed @classmethod - def _parse_simple_attribs(cls, node): + def _parse_simple_attribs(cls, node: ET._Element) -> Dict[str, object]: """Parse all attributes in the _ATTRIBS array for this class. Parameters @@ -74,7 +76,7 @@ def _parse_simple_attribs(cls, node): required and is not present, that attribute's name will map to ``None``. """ - kwargs = {} + kwargs: Dict[str, object] = {} for a in cls._ATTRIBS: t, r = cls._ATTRIBS[a] # t = type, r = required (bool) if r: @@ -82,8 +84,9 @@ def _parse_simple_attribs(cls, node): 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__) + "Missing required attribute {} when parsing an object of type {}".format( + a, cls.__name__ + ) ) else: v = None @@ -93,7 +96,9 @@ def _parse_simple_attribs(cls, node): return kwargs @classmethod - def _parse_simple_elements(cls, node, path): + def _parse_simple_elements( + cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None + ) -> Dict[str, object]: """Parse all elements in the _ELEMENTS array from the children of this node. @@ -114,7 +119,7 @@ def _parse_simple_elements(cls, node, path): Map from element names to the :class:`URDFType` subclass (or list, if ``multiple`` was set) created for that element. """ - kwargs = {} + kwargs: Dict[str, object] = {} for a in cls._ELEMENTS: t, r, m = cls._ELEMENTS[a] if not m: @@ -133,7 +138,9 @@ def _parse_simple_elements(cls, node, path): return kwargs @classmethod - def _parse(cls, node, path): + def _parse( + cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None + ) -> Dict[str, object]: """Parse all elements and attributes in the _ELEMENTS and _ATTRIBS arrays for a node. @@ -151,12 +158,12 @@ def _parse(cls, node, path): 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)) + kwargs: Dict[str, object] = 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): + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): """Create an instance of this class from an XML node. Parameters @@ -174,7 +181,7 @@ def _from_xml(cls, node, path): """ return cls(**cls._parse(node, path)) - def _unparse_attrib(self, val_type, val): + def _unparse_attrib(self, val_type: type, val: object) -> str: """Convert a Python value into a string for storage in an XML attribute. @@ -191,12 +198,12 @@ def _unparse_attrib(self, val_type, val): The attribute string. """ if val_type == np.ndarray: - val = np.array2string(val)[1:-1] + val = np.array2string(cast(np.ndarray, val))[1:-1] else: val = str(val) return val - def _unparse_simple_attribs(self, node): + def _unparse_simple_attribs(self, node: ET._Element) -> None: """Convert all Python types from the _ATTRIBS array back into attributes for an XML node. @@ -211,7 +218,7 @@ def _unparse_simple_attribs(self, node): if r or v is not None: node.attrib[a] = self._unparse_attrib(t, v) - def _unparse_simple_elements(self, node, path): + 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. @@ -228,14 +235,14 @@ def _unparse_simple_elements(self, node, path): t, r, m = self._ELEMENTS[a] v = getattr(self, a, None) if not m: - if r or v is not None: + if v is not None: node.append(v._to_xml(node, path)) else: - vs = v + vs = v or [] for v in vs: node.append(v._to_xml(node, path)) - def _unparse(self, path): + def _unparse(self, path: str) -> ET._Element: """Create a node for this object and unparse all elements and attributes in the class arrays. @@ -255,7 +262,7 @@ def _unparse(self, path): self._unparse_simple_elements(node, path) return node - def _to_xml(self, parent, path): + def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: """Create and return an XML node for this object. Parameters @@ -278,7 +285,9 @@ def _to_xml(self, parent, path): class URDFTypeWithMesh(URDFType): @classmethod - def _parse_simple_elements(cls, node, path, lazy_load_meshes): + def _parse_simple_elements( + cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None + ) -> Dict[str, object]: """Parse all elements in the _ELEMENTS array from the children of this node. @@ -299,7 +308,7 @@ def _parse_simple_elements(cls, node, path, lazy_load_meshes): Map from element names to the :class:`URDFType` subclass (or list, if ``multiple`` was set) created for that element. """ - kwargs = {} + kwargs: Dict[str, object] = {} for a in cls._ELEMENTS: t, r, m = cls._ELEMENTS[a] if not m: @@ -324,7 +333,9 @@ def _parse_simple_elements(cls, node, path, lazy_load_meshes): return kwargs @classmethod - def _parse(cls, node, path, lazy_load_meshes): + def _parse( + cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None + ) -> Dict[str, object]: """Parse all elements and attributes in the _ELEMENTS and _ATTRIBS arrays for a node. @@ -344,12 +355,12 @@ def _parse(cls, node, path, lazy_load_meshes): Map from names to Python classes created from the attributes and elements in the class arrays. """ - kwargs = cls._parse_simple_attribs(node) + kwargs: Dict[str, object] = 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): + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): """Create an instance of this class from an XML node. Parameters diff --git a/urchin/joint.py b/urchin/joint.py index 4c81cc8..43fd9f9 100644 --- a/urchin/joint.py +++ b/urchin/joint.py @@ -1,4 +1,9 @@ +from __future__ import annotations + +from typing import Sequence + import numpy as np +import numpy.typing as npt import trimesh from lxml import etree as ET @@ -34,7 +39,11 @@ class SafetyController(URDFType): _TAG = "safety_controller" def __init__( - self, k_velocity, k_position=None, soft_lower_limit=None, soft_upper_limit=None + self, + k_velocity: float, + k_position: float | None = None, + soft_lower_limit: float | None = None, + soft_upper_limit: float | None = None, ): self.k_velocity = k_velocity self.k_position = k_position @@ -42,12 +51,12 @@ def __init__( self.soft_upper_limit = soft_upper_limit @property - def soft_lower_limit(self): + 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): + def soft_lower_limit(self, value: float | None) -> None: if value is not None: value = float(value) else: @@ -55,12 +64,12 @@ def soft_lower_limit(self, value): self._soft_lower_limit = value @property - def soft_upper_limit(self): + 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): + def soft_upper_limit(self, value: float | None) -> None: if value is not None: value = float(value) else: @@ -68,12 +77,12 @@ def soft_upper_limit(self, value): self._soft_upper_limit = value @property - def k_position(self): + 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): + def k_position(self, value: float | None) -> None: if value is not None: value = float(value) else: @@ -81,15 +90,17 @@ def k_position(self, value): self._k_position = value @property - def k_velocity(self): + 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): + def k_velocity(self, value: float) -> None: self._k_velocity = float(value) - def copy(self, prefix="", scale=None): + def copy( + self, prefix: str = "", scale: float | Sequence[float] | None = None + ) -> "SafetyController": """Create a deep copy of the visual with the prefix applied to all names. Parameters @@ -126,33 +137,35 @@ class JointCalibration(URDFType): _ATTRIBS = {"rising": (float, False), "falling": (float, False)} _TAG = "calibration" - def __init__(self, rising=None, falling=None): + def __init__(self, rising: float | None = None, falling: float | None = None): self.rising = rising self.falling = falling @property - def rising(self): + def rising(self) -> float | None: """float : description.""" return self._rising @rising.setter - def rising(self, value): + def rising(self, value: float | None) -> None: if value is not None: value = float(value) self._rising = value @property - def falling(self): + def falling(self) -> float | None: """float : description.""" return self._falling @falling.setter - def falling(self, value): + def falling(self, value: float | None) -> None: if value is not None: value = float(value) self._falling = value - def copy(self, prefix="", scale=None): + def copy( + self, prefix: str = "", scale: float | Sequence[float] | None = None + ) -> "JointCalibration": """Create a deep copy of the visual with the prefix applied to all names. Parameters @@ -190,34 +203,36 @@ class JointDynamics(URDFType): } _TAG = "dynamics" - def __init__(self, damping, friction): + def __init__(self, damping: float | None, friction: float | None): self.damping = damping self.friction = friction @property - def damping(self): + def damping(self) -> float | None: """float : The damping value of the joint.""" return self._damping @damping.setter - def damping(self, value): + def damping(self, value: float | None) -> None: if value is not None: value = float(value) self._damping = value @property - def friction(self): + def friction(self) -> float | None: """float : The static friction value of the joint.""" return self._friction @friction.setter - def friction(self, value): + def friction(self, value: float | None) -> None: 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. + def copy( + self, prefix: str = "", scale: float | Sequence[float] | None = None + ) -> "JointDynamics": + """Create a deep copy with the prefix applied to all names. Parameters ---------- @@ -259,54 +274,60 @@ class JointLimit(URDFType): } _TAG = "limit" - def __init__(self, effort, velocity, lower=None, upper=None): + def __init__( + self, + effort: float, + velocity: float, + lower: float | None = None, + upper: float | None = None, + ): self.effort = effort self.velocity = velocity self.lower = lower self.upper = upper @property - def effort(self): + def effort(self) -> float: """float : The maximum joint effort.""" return self._effort @effort.setter - def effort(self, value): + def effort(self, value: float) -> None: self._effort = float(value) @property - def velocity(self): + def velocity(self) -> float: """float : The maximum joint velocity.""" return self._velocity @velocity.setter - def velocity(self, value): + def velocity(self, value: float) -> None: self._velocity = float(value) @property - def lower(self): + def lower(self) -> float | None: """float : The lower joint limit.""" return self._lower @lower.setter - def lower(self, value): + def lower(self, value: float | None) -> None: if value is not None: value = float(value) self._lower = value @property - def upper(self): + def upper(self) -> float | None: """float : The upper joint limit.""" return self._upper @upper.setter - def upper(self, value): + def upper(self, value: float | None) -> None: 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. + def copy(self, prefix: str = "", scale: float | Sequence[float] | None = None) -> "JointLimit": + """Create a deep copy with the prefix applied to all names. Parameters ---------- @@ -350,27 +371,27 @@ class JointMimic(URDFType): } _TAG = "mimic" - def __init__(self, joint, multiplier=None, offset=None): + def __init__(self, joint: str, multiplier: float | None = None, offset: float | None = None): self.joint = joint self.multiplier = multiplier self.offset = offset @property - def joint(self): + def joint(self) -> str: """float : The name of the joint to mimic.""" return self._joint @joint.setter - def joint(self, value): + def joint(self, value: object) -> None: self._joint = str(value) @property - def multiplier(self): + def multiplier(self) -> float: """float : The multiplier for the joint configuration.""" return self._multiplier @multiplier.setter - def multiplier(self, value): + def multiplier(self, value: float | None) -> None: if value is not None: value = float(value) else: @@ -378,19 +399,19 @@ def multiplier(self, value): self._multiplier = value @property - def offset(self): + def offset(self) -> float: """float : The offset for the joint configuration""" return self._offset @offset.setter - def offset(self, value): + def offset(self, value: float | None) -> None: if value is not None: value = float(value) else: value = 0.0 self._offset = value - def copy(self, prefix="", scale=None): + def copy(self, prefix: str = "", scale: float | Sequence[float] | None = None) -> "JointMimic": """Create a deep copy of the joint mimic with the prefix applied to all names. Parameters @@ -571,9 +592,7 @@ def limit(self): 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" - ) + raise ValueError("Require joint limit for prismatic and revolute joints") elif not isinstance(value, JointLimit): raise TypeError("Expected JointLimit type") self._limit = value @@ -626,7 +645,7 @@ def mimic(self, value): raise TypeError("Expected JointMimic type") self._mimic = value - def is_valid(self, cfg): + def is_valid(self, cfg: float | npt.ArrayLike) -> bool: """Check if the provided configuration value is valid for this joint. Parameters @@ -641,18 +660,18 @@ def is_valid(self, cfg): """ if self.joint_type not in ["fixed", "revolute"]: return True - if self.joint_limit is None: + if self.limit is None: return True - cfg = float(cfg) - lower = -np.infty - upper = np.infty + 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 >= lower and cfg <= upper + return cfg_val >= lower and cfg_val <= upper - def get_child_pose(self, cfg=None): + def get_child_pose(self, cfg: float | npt.ArrayLike | None = None) -> np.ndarray: """Computes the child pose relative to a parent pose for a given configuration value. @@ -683,18 +702,18 @@ def get_child_pose(self, cfg=None): return self.origin elif self.joint_type in ["revolute", "continuous"]: if cfg is None: - cfg = 0.0 + cfg_val = 0.0 else: - cfg = float(cfg) - R = trimesh.transformations.rotation_matrix(cfg, self.axis) + 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 = 0.0 + cfg_val = 0.0 else: - cfg = float(cfg) + cfg_val = float(cfg) # type: ignore[arg-type] translation = np.eye(4, dtype=np.float64) - translation[:3, 3] = self.axis * cfg + translation[:3, 3] = self.axis * cfg_val return self.origin.dot(translation) elif self.joint_type == "planar": if cfg is None: @@ -717,7 +736,7 @@ def get_child_pose(self, cfg=None): else: raise ValueError("Invalid configuration") - def get_child_poses(self, cfg, n_cfgs): + def get_child_poses(self, cfg: npt.ArrayLike | None, n_cfgs: int) -> np.ndarray: """Computes the child pose relative to a parent pose for a given set of configuration values. @@ -751,9 +770,11 @@ def get_child_poses(self, cfg, 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) + 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[:, np.newaxis] + translation[:, :3, 3] = self.axis * cfg_arr[:, np.newaxis] return np.matmul(self.origin, translation) elif self.joint_type == "planar": raise NotImplementedError() @@ -763,7 +784,7 @@ def get_child_poses(self, cfg, n_cfgs): raise ValueError("Invalid configuration") @classmethod - def _from_xml(cls, node, path): + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): kwargs = cls._parse(node, path) kwargs["joint_type"] = str(node.attrib["type"]) kwargs["parent"] = node.find("parent").attrib["link"] @@ -775,7 +796,7 @@ def _from_xml(cls, node, path): kwargs["origin"] = parse_origin(node) return cls(**kwargs) - def _to_xml(self, parent, path): + def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: node = self._unparse(path) parent = ET.Element("parent") parent.attrib["link"] = self.parent @@ -791,7 +812,7 @@ def _to_xml(self, parent, path): node.attrib["type"] = self.joint_type return node - def _rotation_matrices(self, angles, axis): + def _rotation_matrices(self, angles: npt.ArrayLike, axis: npt.ArrayLike) -> np.ndarray: """Compute rotation matrices from angle/axis representations. Parameters @@ -806,27 +827,28 @@ def _rotation_matrices(self, angles, axis): 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)) + 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(axis, axis), (len(angles), 1, 1)) - * (1.0 - cosa)[:, np.newaxis, np.newaxis] + 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, -axis[2], axis[1]], - [axis[2], 0.0, -axis[0]], - [-axis[1], axis[0], 0.0], + [0.0, -ax[2], ax[1]], + [ax[2], 0.0, -ax[0]], + [-ax[1], ax[0], 0.0], ] ), - (len(angles), 1, 1), + (len(ang), 1, 1), ) * sina[:, np.newaxis, np.newaxis] ) @@ -860,13 +882,9 @@ def copy(self, prefix="", scale=None): 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 + 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 index 5bc0714..8175689 100644 --- a/urchin/link.py +++ b/urchin/link.py @@ -1,6 +1,10 @@ +from __future__ import annotations + import os +from typing import Sequence import numpy as np +import numpy.typing as npt import trimesh from lxml import etree as ET @@ -16,6 +20,7 @@ class Box(URDFType): + _meshes: list[trimesh.Trimesh] """A rectangular prism whose center is at the local origin. Parameters @@ -27,22 +32,22 @@ class Box(URDFType): _ATTRIBS = {"size": (np.ndarray, True)} _TAG = "box" - def __init__(self, size): + def __init__(self, size: npt.ArrayLike): self.size = size self._meshes = [] @property - def size(self): + 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): + def size(self, value: npt.ArrayLike) -> None: self._size = np.asanyarray(value).astype(np.float64) self._meshes = [] @property - def meshes(self): + def meshes(self) -> list[trimesh.Trimesh]: """list of :class:`~trimesh.base.Trimesh` : The triangular meshes that represent this object. """ @@ -50,13 +55,15 @@ def meshes(self): self._meshes = [trimesh.creation.box(extents=self.size)] return self._meshes - def copy(self, prefix="", scale=None): + def copy(self, prefix: str = "", scale: 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 ------- @@ -72,6 +79,7 @@ def copy(self, prefix="", scale=None): class Cylinder(URDFType): + _meshes: list[trimesh.Trimesh] """A cylinder whose center is at the local origin. Parameters @@ -88,49 +96,50 @@ class Cylinder(URDFType): } _TAG = "cylinder" - def __init__(self, radius, length): + def __init__(self, radius: float, length: float): self.radius = radius self.length = length self._meshes = [] @property - def radius(self): + def radius(self) -> float: """float : The radius of the cylinder in meters.""" return self._radius @radius.setter - def radius(self, value): + def radius(self, value: float) -> None: self._radius = float(value) self._meshes = [] @property - def length(self): + def length(self) -> float: """float : The length of the cylinder in meters.""" return self._length @length.setter - def length(self, value): + def length(self, value: float) -> None: self._length = float(value) self._meshes = [] @property - def meshes(self): + 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) - ] + self._meshes = [trimesh.creation.cylinder(radius=self.radius, height=self.length)] return self._meshes - def copy(self, prefix="", scale=None): + def copy(self, prefix: str = "", scale: 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 ------- @@ -139,24 +148,27 @@ def copy(self, prefix="", scale=None): """ 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" - ) + 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 * scale[0], - length=self.length * scale[2], + 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 * scale, - length=self.length * scale, + 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 @@ -170,22 +182,22 @@ class Sphere(URDFType): } _TAG = "sphere" - def __init__(self, radius): + def __init__(self, radius: float): self.radius = radius self._meshes = [] @property - def radius(self): + def radius(self) -> float: """float : The radius of the sphere in meters.""" return self._radius @radius.setter - def radius(self, value): + def radius(self, value: float) -> None: self._radius = float(value) self._meshes = [] @property - def meshes(self): + def meshes(self) -> list[trimesh.Trimesh]: """list of :class:`~trimesh.base.Trimesh` : The triangular meshes that represent this object. """ @@ -196,13 +208,15 @@ def meshes(self): self._meshes = [trimesh.creation.icosphere(radius=self.radius)] return self._meshes - def copy(self, prefix="", scale=None): + def copy(self, prefix: str = "", scale: 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 ------- @@ -211,14 +225,18 @@ def copy(self, prefix="", scale=None): """ if scale is None: scale = 1.0 - if isinstance(scale, (list, np.ndarray)): - if scale[0] != scale[1] or scale[0] != scale[2]: + 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[0] - s = self.__class__( - radius=self.radius * scale, + scale = scale_list[0] + from typing import cast as _cast + + sf: float = float(_cast(float, scale)) + result = self.__class__( + radius=self.radius * sf, ) - return s + return result class Mesh(URDFTypeWithMesh): @@ -229,11 +247,16 @@ class Mesh(URDFTypeWithMesh): 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` - A list of meshes that compose this mesh. + 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. @@ -242,7 +265,14 @@ class Mesh(URDFTypeWithMesh): _ATTRIBS = {"filename": (str, True), "scale": (np.ndarray, False)} _TAG = "mesh" - def __init__(self, filename, combine, scale=None, meshes=None, lazy_filename=None): + def __init__( + self, + filename: str, + combine: bool, + scale: npt.ArrayLike | None = None, + meshes: list[trimesh.Trimesh] | trimesh.Trimesh | str | None = None, + lazy_filename: str | None = None, + ): if meshes is None: if lazy_filename is None: meshes = load_meshes(filename) @@ -255,38 +285,37 @@ def __init__(self, filename, combine, scale=None, meshes=None, lazy_filename=Non self.meshes = meshes @property - def filename(self): + def filename(self) -> str: """str : The path to the mesh file for this object.""" return self._filename @filename.setter - def filename(self, value): + def filename(self, value: str) -> None: self._filename = value @property - def scale(self): + def scale(self) -> np.ndarray | None: """(3,) float : A scaling for the mesh along its local XYZ axes.""" return self._scale @scale.setter - def scale(self, value): + def scale(self, value: npt.ArrayLike | None) -> None: if value is not None: value = np.asanyarray(value).astype(np.float64) self._scale = value @property - def meshes(self): + 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 - ) - return self._meshes + 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): + def meshes(self, value: 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): @@ -297,9 +326,7 @@ def meshes(self, value): 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" - ) + raise TypeError("Mesh requires a trimesh.Trimesh or a list of them") elif isinstance(value, trimesh.Trimesh): value = [value] else: @@ -307,83 +334,98 @@ def meshes(self, value): self._meshes = value @classmethod - def _load_and_combine_meshes(cls, fn, combine): + 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) - meshes = [meshes[0] + meshes[1:]] + merged = meshes[0] + for extra in meshes[1:]: + merged = merged + extra + return [merged] 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"]) + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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) - kwargs["lazy_filename"] = None + lazy_filename = None else: meshes = None - kwargs["lazy_filename"] = fn - kwargs["meshes"] = meshes - kwargs["combine"] = combine + lazy_filename = fn - return cls(**kwargs) + return cls( + filename=filename_attr, + combine=combine, + scale=scale_val, + meshes=meshes, + lazy_filename=lazy_filename, + ) - def _to_xml(self, parent, path): + def _to_xml(self, parent: ET._Element | None, 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 = self.meshes - if len(meshes) == 1: - meshes = meshes[0] + meshes_list = self.meshes or [] + if len(meshes_list) == 1: + export_obj: object = meshes_list[0] elif os.path.splitext(fn)[1] == ".glb": - meshes = trimesh.scene.Scene(geometry=meshes) - trimesh.exchange.export.export_mesh(meshes, fn) + 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="", scale=None): + def copy(self, prefix: str = "", scale: 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 = [m.copy() for m in self.meshes] + meshes = [mesh_i.copy() for mesh_i in self.meshes] if scale is not None: sm = np.eye(4) - if isinstance(scale, (list, np.ndarray)): - sm[:3, :3] = np.diag(scale) + if isinstance(scale, (list, tuple, np.ndarray)): + sm[:3, :3] = np.diag(np.asanyarray(scale, dtype=float)) else: - sm[:3, :3] = np.diag(np.repeat(scale, 3)) - for i, m in enumerate(meshes): - meshes[i] = m.apply_transform(sm) + 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) - m = self.__class__( + 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 m + return new_mesh class Geometry(URDFTypeWithMesh): @@ -412,7 +454,13 @@ class Geometry(URDFTypeWithMesh): } _TAG = "geometry" - def __init__(self, box=None, cylinder=None, sphere=None, mesh=None): + def __init__( + self, + box: Box | None = None, + cylinder: Cylinder | None = None, + sphere: Sphere | None = None, + mesh: Mesh | None = 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 @@ -421,51 +469,51 @@ def __init__(self, box=None, cylinder=None, sphere=None, mesh=None): self.mesh = mesh @property - def box(self): + def box(self) -> Box | None: """:class:`.Box` : Box geometry.""" return self._box @box.setter - def box(self, value): + def box(self, value: Box | None) -> None: if value is not None and not isinstance(value, Box): raise TypeError("Expected Box type") self._box = value @property - def cylinder(self): + def cylinder(self) -> Cylinder | None: """:class:`.Cylinder` : Cylinder geometry.""" return self._cylinder @cylinder.setter - def cylinder(self, value): + def cylinder(self, value: Cylinder | None) -> None: if value is not None and not isinstance(value, Cylinder): raise TypeError("Expected Cylinder type") self._cylinder = value @property - def sphere(self): + def sphere(self) -> Sphere | None: """:class:`.Sphere` : Spherical geometry.""" return self._sphere @sphere.setter - def sphere(self, value): + def sphere(self, value: Sphere | None) -> None: if value is not None and not isinstance(value, Sphere): raise TypeError("Expected Sphere type") self._sphere = value @property - def mesh(self): + def mesh(self) -> Mesh | None: """:class:`.Mesh` : Mesh geometry.""" return self._mesh @mesh.setter - def mesh(self, value): + def mesh(self, value: Mesh | None) -> None: if value is not None and not isinstance(value, Mesh): raise TypeError("Expected Mesh type") self._mesh = value @property - def geometry(self): + def geometry(self) -> Box | Cylinder | Sphere | Mesh | None: """:class:`.Box`, :class:`.Cylinder`, :class:`.Sphere`, or :class:`.Mesh` : The valid geometry element. """ @@ -480,19 +528,22 @@ def geometry(self): return None @property - def meshes(self): + 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="", scale=None): + def copy(self, prefix: str = "", scale: 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 ------- @@ -501,14 +552,8 @@ def copy(self, prefix="", scale=None): """ 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 - ), + 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 @@ -534,60 +579,66 @@ class Collision(URDFTypeWithMesh): } _TAG = "collision" - def __init__(self, name, origin, geometry): + def __init__(self, name: str | None, origin: npt.ArrayLike | None, geometry: Geometry): self.geometry = geometry self.name = name self.origin = origin @property - def geometry(self): + def geometry(self) -> Geometry: """:class:`.Geometry` : The geometry of this element.""" return self._geometry @geometry.setter - def geometry(self, value): + 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): + def name(self) -> str | None: """str : The name of this collision element.""" return self._name @name.setter - def name(self, value): + def name(self, value: str | None) -> None: if value is not None: value = str(value) self._name = value @property - def origin(self): + 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): + def origin(self, value: npt.ArrayLike | None) -> None: 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 _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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, path): + def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: node = self._unparse(path) node.append(unparse_origin(self.origin)) return node - def copy(self, prefix="", scale=None): + def copy(self, prefix: str = "", scale: 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 ------- @@ -596,9 +647,13 @@ def copy(self, prefix="", scale=None): """ 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 + 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, @@ -629,73 +684,87 @@ class Visual(URDFTypeWithMesh): } _TAG = "visual" - def __init__(self, geometry, name=None, origin=None, material=None): + def __init__( + self, + geometry: Geometry, + name: str | None = None, + origin: npt.ArrayLike | None = None, + material: Material | None = None, + ): self.geometry = geometry self.name = name self.origin = origin self.material = material @property - def geometry(self): + def geometry(self) -> Geometry: """:class:`.Geometry` : The geometry of this element.""" return self._geometry @geometry.setter - def geometry(self, value): + 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): + def name(self) -> str | None: """str : The name of this visual element.""" return self._name @name.setter - def name(self, value): + def name(self, value: str | None) -> None: if value is not None: value = str(value) self._name = value @property - def origin(self): + 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): + def origin(self, value: npt.ArrayLike | None) -> None: self._origin = configure_origin(value) @property - def material(self): + def material(self) -> Material | None: """:class:`.Material` : The material for this element.""" return self._material @material.setter - def material(self, value): + def material(self, value: Material | None) -> 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, path, lazy_load_meshes): - kwargs = cls._parse(node, path, lazy_load_meshes) - kwargs["origin"] = parse_origin(node) - return cls(**kwargs) + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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, path): + def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: node = self._unparse(path) node.append(unparse_origin(self.origin)) return node - def copy(self, prefix="", scale=None): + def copy(self, prefix: str = "", scale: 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 ------- @@ -704,9 +773,13 @@ def copy(self, prefix="", scale=None): """ 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 + 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), @@ -731,43 +804,43 @@ class Inertial(URDFType): _TAG = "inertial" - def __init__(self, mass, inertia, origin=None): + def __init__(self, mass: float, inertia: npt.ArrayLike, origin: npt.ArrayLike | None = None): self.mass = mass self.inertia = inertia self.origin = origin @property - def mass(self): + def mass(self) -> float: """float : The mass of the link in kilograms.""" return self._mass @mass.setter - def mass(self, value): + def mass(self, value: float) -> None: self._mass = float(value) @property - def inertia(self): + def inertia(self) -> np.ndarray: """(3,3) float : The 3x3 symmetric rotational inertia matrix.""" return self._inertia @inertia.setter - def inertia(self, value): + 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): + 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): + def origin(self, value: npt.ArrayLike | None) -> None: self._origin = configure_origin(value) @classmethod - def _from_xml(cls, node, path): + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): origin = parse_origin(node) mass = float(node.find("mass").attrib["value"]) n = node.find("inertia") @@ -780,7 +853,7 @@ def _from_xml(cls, node, path): 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): + def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: node = ET.Element("inertial") node.append(unparse_origin(self.origin)) mass = ET.Element("mass") @@ -796,7 +869,13 @@ def _to_xml(self, parent, path): node.append(inertia) return node - def copy(self, prefix="", mass=None, origin=None, inertia=None): + def copy( + self, + prefix: str = "", + mass: float | None = None, + origin: np.ndarray | None = None, + inertia: np.ndarray | None = None, + ) -> "Inertial": """Create a deep copy of the visual with the prefix applied to all names. Parameters @@ -847,30 +926,36 @@ class Link(URDFTypeWithMesh): } _TAG = "link" - def __init__(self, name, inertial, visuals, collisions): + def __init__( + self, + name: str, + inertial: Inertial | None, + visuals: Sequence[Visual] | None, + collisions: Sequence[Collision] | None, + ): self.name = name self.inertial = inertial self.visuals = visuals self.collisions = collisions - self._collision_mesh = None + self._collision_mesh: trimesh.Trimesh | None = None @property - def name(self): + def name(self) -> str: """str : The name of this link.""" return self._name @name.setter - def name(self, value): + def name(self, value: object) -> None: self._name = str(value) @property - def inertial(self): + def inertial(self) -> Inertial: """:class:`.Inertial` : Inertial properties of the link.""" return self._inertial @inertial.setter - def inertial(self, value): + def inertial(self, value: Inertial | None) -> None: if value is not None and not isinstance(value, Inertial): raise TypeError("Expected Inertial object") # Set default inertial @@ -879,12 +964,12 @@ def inertial(self, value): self._inertial = value @property - def visuals(self): + def visuals(self) -> list[Visual]: """list of :class:`.Visual` : The visual properties of this link.""" return self._visuals @visuals.setter - def visuals(self, value): + def visuals(self, value: Sequence[Visual] | None) -> None: if value is None: value = [] else: @@ -895,12 +980,12 @@ def visuals(self, value): self._visuals = value @property - def collisions(self): + def collisions(self) -> list[Collision]: """list of :class:`.Collision` : The collision properties of this link.""" return self._collisions @collisions.setter - def collisions(self, value): + def collisions(self, value: Sequence[Collision] | None) -> None: if value is None: value = [] else: @@ -911,7 +996,7 @@ def collisions(self, value): self._collisions = value @property - def collision_mesh(self): + def collision_mesh(self) -> trimesh.Trimesh | None: """:class:`~trimesh.base.Trimesh` : A single collision mesh for the link, specified in the link frame, or None if there isn't one. """ @@ -932,16 +1017,28 @@ def collision_mesh(self): meshes.append(m) if len(meshes) == 0: return None - self._collision_mesh = meshes[0] + meshes[1:] + merged = meshes[0] + for extra in meshes[1:]: + merged = merged + extra + self._collision_mesh = merged return self._collision_mesh - def copy(self, prefix="", scale=None, collision_only=False): + def copy( + self, + prefix: str = "", + scale: 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 ------- @@ -953,9 +1050,13 @@ def copy(self, prefix="", scale=None, collision_only=False): 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) + 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) diff --git a/urchin/material.py b/urchin/material.py index 89028a6..384610b 100644 --- a/urchin/material.py +++ b/urchin/material.py @@ -1,4 +1,7 @@ +from __future__ import annotations + import numpy as np +import numpy.typing as npt import PIL from lxml import etree as ET @@ -14,62 +17,61 @@ class Texture(URDFType): 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. + 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, image=None): + def __init__(self, filename: str, image: PIL.Image.Image | str | np.ndarray | None = None): if image is None: - image = PIL.image.open(filename) + image = PIL.Image.open(filename) self.filename = filename self.image = image @property - def filename(self): + def filename(self) -> str: """str : Path to the image for this texture.""" return self._filename @filename.setter - def filename(self, value): + def filename(self, value: object) -> None: self._filename = str(value) @property - def image(self): + def image(self) -> PIL.Image.Image: """:class:`PIL.Image.Image` : The image for this texture.""" return self._image @image.setter - def image(self, value): + def image(self, value: 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") + 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): + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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: ET._Element | None, 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="", scale=None): + def copy(self, prefix: str = "", scale: float | np.ndarray | None = None) -> "Texture": """Create a deep copy with the prefix applied to all names. Parameters @@ -105,27 +107,32 @@ class Material(URDFType): } _TAG = "material" - def __init__(self, name, color=None, texture=None): + def __init__( + self, + name: str, + color: npt.ArrayLike | None = None, + texture: Texture | str | None = None, + ): self.name = name self.color = color self.texture = texture @property - def name(self): + def name(self) -> str: """str : The name of the material.""" return self._name @name.setter - def name(self, value): + def name(self, value: object) -> None: self._name = str(value) @property - def color(self): + def color(self) -> np.ndarray | None: """(4,) float : The RGBA color of the material, in the range [0,1].""" return self._color @color.setter - def color(self, value): + def color(self, value: npt.ArrayLike | None) -> None: if value is not None: value = np.asanyarray(value).astype(float) value = np.clip(value, 0.0, 1.0) @@ -134,35 +141,32 @@ def color(self, value): self._color = value @property - def texture(self): + def texture(self) -> Texture | None: """:class:`.Texture` : The texture for the material.""" return self._texture @texture.setter - def texture(self, value): + def texture(self, value: 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" - ) + 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): + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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 @@ -182,7 +186,7 @@ def _to_xml(self, parent, path): node.append(color) return node - def copy(self, prefix="", scale=None): + def copy(self, prefix: str = "", scale: float | np.ndarray | None = None) -> "Material": """Create a deep copy of the material with the prefix applied to all names. Parameters diff --git a/urchin/transmission.py b/urchin/transmission.py index ef7f8cb..44bf67a 100644 --- a/urchin/transmission.py +++ b/urchin/transmission.py @@ -1,3 +1,7 @@ +from __future__ import annotations + +from typing import Sequence + from lxml import etree as ET from urchin.base import URDFType @@ -10,9 +14,8 @@ class Actuator(URDFType): ---------- name : str The name of this actuator. - mechanicalReduction : str, optional - A specifier for the mechanical reduction at the joint/actuator - transmission. + mechanicalReduction : float, optional + Mechanical reduction (ratio) at the joint/actuator transmission. hardwareInterfaces : list of str, optional The supported hardware interfaces to the actuator. """ @@ -22,38 +25,43 @@ class Actuator(URDFType): } _TAG = "actuator" - def __init__(self, name, mechanicalReduction=None, hardwareInterfaces=None): + def __init__( + self, + name: str, + mechanicalReduction: float | None = None, + hardwareInterfaces: Sequence[str] | None = None, + ): self.name = name self.mechanicalReduction = mechanicalReduction self.hardwareInterfaces = hardwareInterfaces @property - def name(self): + def name(self) -> str: """str : The name of this actuator.""" return self._name @name.setter - def name(self, value): + def name(self, value: object) -> None: self._name = str(value) @property - def mechanicalReduction(self): - """str : A specifier for the type of mechanical reduction.""" + def mechanicalReduction(self) -> float | None: + """float | None : Mechanical reduction (ratio).""" return self._mechanicalReduction @mechanicalReduction.setter - def mechanicalReduction(self, value): + def mechanicalReduction(self, value: float | str | None) -> None: if value is not None: - value = str(value) + value = float(value) self._mechanicalReduction = value @property - def hardwareInterfaces(self): + def hardwareInterfaces(self) -> list[str]: """list of str : The supported hardware interfaces.""" return self._hardwareInterfaces @hardwareInterfaces.setter - def hardwareInterfaces(self, value): + def hardwareInterfaces(self, value: Sequence[str] | None) -> None: if value is None: value = [] else: @@ -63,19 +71,15 @@ def hardwareInterfaces(self, value): 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): + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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: ET._Element | None, path: str) -> ET._Element: node = self._unparse(path) if self.mechanicalReduction is not None: mr = ET.Element("mechanicalReduction") @@ -88,8 +92,8 @@ def _to_xml(self, parent, path): 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. + def copy(self, prefix: str = "", scale: float | None = None) -> "Actuator": + """Create a deep copy with the prefix applied to all names. Parameters ---------- @@ -124,26 +128,26 @@ class TransmissionJoint(URDFType): } _TAG = "joint" - def __init__(self, name, hardwareInterfaces): + def __init__(self, name: str, hardwareInterfaces: Sequence[str] | None): self.name = name self.hardwareInterfaces = hardwareInterfaces @property - def name(self): + def name(self) -> str: """str : The name of this transmission joint.""" return self._name @name.setter - def name(self, value): + def name(self, value: object) -> None: self._name = str(value) @property - def hardwareInterfaces(self): + def hardwareInterfaces(self) -> list[str]: """list of str : The supported hardware interfaces.""" return self._hardwareInterfaces @hardwareInterfaces.setter - def hardwareInterfaces(self, value): + def hardwareInterfaces(self, value: Sequence[str] | None) -> None: if value is None: value = [] else: @@ -153,15 +157,13 @@ def hardwareInterfaces(self, value): 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): + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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: ET._Element | None, path: str) -> ET._Element: node = self._unparse(path) if len(self.hardwareInterfaces) > 0: for hi in self.hardwareInterfaces: @@ -170,7 +172,7 @@ def _to_xml(self, parent, path): node.append(h) return node - def copy(self, prefix="", scale=None): + def copy(self, prefix: str = "", scale: float | None = None) -> "TransmissionJoint": """Create a deep copy with the prefix applied to all names. Parameters @@ -219,39 +221,45 @@ class Transmission(URDFType): } _TAG = "transmission" - def __init__(self, name, trans_type, joints=None, actuators=None): + def __init__( + self, + name: str, + trans_type: str, + joints: Sequence["TransmissionJoint"] | None = None, + actuators: Sequence[Actuator] | None = None, + ): self.name = name self.trans_type = trans_type self.joints = joints self.actuators = actuators @property - def name(self): + def name(self) -> str: """str : The name of this transmission.""" return self._name @name.setter - def name(self, value): + def name(self, value: object) -> None: self._name = str(value) @property - def trans_type(self): + def trans_type(self) -> str: """str : The type of this transmission.""" return self._trans_type @trans_type.setter - def trans_type(self, value): + def trans_type(self, value: object) -> None: self._trans_type = str(value) @property - def joints(self): + def joints(self) -> list["TransmissionJoint"]: """:class:`.TransmissionJoint` : The joints the transmission is connected to. """ return self._joints @joints.setter - def joints(self, value): + def joints(self, value: Sequence["TransmissionJoint"] | None) -> None: if value is None: value = [] else: @@ -262,12 +270,12 @@ def joints(self, value): self._joints = value @property - def actuators(self): + def actuators(self) -> list[Actuator]: """:class:`.Actuator` : The actuators the transmission is connected to.""" return self._actuators @actuators.setter - def actuators(self, value): + def actuators(self, value: Sequence[Actuator] | None) -> None: if value is None: value = [] else: @@ -278,22 +286,26 @@ def actuators(self, value): 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): + def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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: ET._Element | None, 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="", scale=None): + def copy( + self, prefix: str = "", scale: float | Sequence[float] | None = None + ) -> "Transmission": """Create a deep copy with the prefix applied to all names. Parameters diff --git a/urchin/urdf.py b/urchin/urdf.py index c6f769a..7691977 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -1,10 +1,13 @@ -import copy +from __future__ import annotations + import os import time from collections import OrderedDict +from typing import IO, Mapping, Sequence, cast import networkx as nx import numpy as np +import numpy.typing as npt import trimesh from lxml import etree as ET @@ -51,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: Sequence[Joint] | None = None, + transmissions: Sequence[Transmission] | None = None, + materials: Sequence[Material] | None = None, + other_xml: bytes | str | None = None, + ) -> None: if joints is None: joints = [] if transmissions is None: @@ -67,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() @@ -137,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: object) -> 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) -> bytes | str | None: """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: bytes | str | None) -> 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. @@ -245,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. @@ -254,7 +255,9 @@ 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: Mapping[str, float] | Sequence[float] | npt.ArrayLike | None + ) -> npt.NDArray[np.float64] | None: """Convert a configuration dictionary into a configuration vector. Parameters @@ -284,7 +287,7 @@ def cfg_to_vector(self, cfg): raise ValueError("Invalid configuration: {}".format(cfg)) @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. @@ -292,7 +295,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. @@ -300,7 +303,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. @@ -309,8 +312,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: @@ -320,11 +323,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 @@ -333,7 +336,17 @@ 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: Mapping[str, float] | Sequence[float] | npt.ArrayLike | None = None, + link: str | Link | None = None, + links: Sequence[str | Link] | None = None, + use_names: bool = False, + ) -> ( + 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 @@ -365,7 +378,7 @@ 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, str): link_set.add(self._link_map[link]) @@ -378,19 +391,17 @@ def link_fk(self, cfg=None, link=None, links=None, use_names=False): 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] @@ -464,9 +475,7 @@ def link_fk_batch(self, cfgs=None, link=None, links=None, use_names=False): 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 @@ -487,9 +496,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) @@ -508,7 +515,11 @@ 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: Mapping[str, float] | Sequence[float] | npt.ArrayLike | None = None, + links: Sequence[str | Link] | None = None, + ) -> dict: """Computes the poses of the URDF's visual geometries using fk. Parameters @@ -531,7 +542,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: @@ -539,7 +550,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: ( + Mapping[str, Sequence[float]] + | Sequence[Mapping[str, float] | None] + | npt.ArrayLike + | None + ) = None, + links: Sequence[str | Link] | None = None, + ) -> dict: """Computes the poses of the URDF's visual geometries using fk. Parameters @@ -561,7 +581,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: @@ -569,7 +589,11 @@ 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: Mapping[str, float] | Sequence[float] | npt.ArrayLike | None = None, + links: Sequence[str | Link] | None = None, + ) -> dict[trimesh.Trimesh, npt.NDArray[np.float64]]: """Computes the poses of the URDF's visual trimeshes using fk. Parameters @@ -593,7 +617,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: @@ -605,21 +629,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( @@ -630,12 +648,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 @@ -649,7 +663,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: ( + Mapping[str, Sequence[float]] + | Sequence[Mapping[str, float] | None] + | npt.ArrayLike + | None + ) = None, + links: Sequence[str | Link] | None = None, + ) -> dict[trimesh.Trimesh, npt.NDArray[np.float64]]: """Computes the poses of the URDF's visual trimeshes using fk. Parameters @@ -840,20 +863,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) @@ -893,9 +916,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: @@ -919,10 +940,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: @@ -965,7 +983,11 @@ 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: Mapping[str, float] | Sequence[float] | npt.ArrayLike | None = None, + use_collision: bool = False, + ) -> None: """Visualize the URDF in a given configuration. Parameters @@ -994,7 +1016,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: str | None = None, + prefix: str = "", + scale: float | Sequence[float] | None = None, + collision_only: bool = False, + ) -> "URDF": """Make a deep copy of the URDF. Parameters @@ -1018,11 +1046,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: str | IO[bytes] | IO[str]) -> None: """Save this URDF to a file. Parameters @@ -1035,8 +1063,8 @@ 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, str): path, _ = os.path.split(file_obj) @@ -1047,7 +1075,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: Link | str, + origin: npt.ArrayLike | None = None, + name: str | None = None, + prefix: str = "", + ) -> "URDF": """Join another URDF to this one by rigidly fixturing the two at a link. Parameters @@ -1111,7 +1146,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: @@ -1124,7 +1159,7 @@ 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: str | IO[bytes] | IO[str], lazy_load_meshes: bool = False) -> "URDF": """Load a URDF from a file. Parameters @@ -1177,20 +1212,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( @@ -1199,16 +1228,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 @@ -1228,7 +1255,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: @@ -1239,11 +1266,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: @@ -1268,7 +1294,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: @@ -1280,27 +1306,29 @@ 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: Link | None = 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: 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): @@ -1312,8 +1340,7 @@ def _process_cfg(self, cfg): 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 @@ -1321,14 +1348,24 @@ def _process_cfg(self, cfg): raise TypeError("Invalid type for config") return joint_cfg - def _process_cfgs(self, cfgs): + def _process_cfgs( + self, + cfgs: ( + Mapping[str, Sequence[float]] + | Sequence[Mapping[str, float] | None] + | npt.ArrayLike + | None + ), + ) -> tuple[dict[Joint, Sequence[float] | npt.NDArray[np.float64] | None], int | None]: """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, 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: @@ -1344,41 +1381,68 @@ def _process_cfgs(self, cfgs): for cfg in cfgs: for joint in cfg: if isinstance(joint, str): - joint_cfg[self._joint_map[joint]].append(cfg[joint]) + 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, Sequence[float] | npt.NDArray[np.float64] | None], joint_cfg + ), n_cfgs + + @classmethod + def _from_xml( + cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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: ET._Element | None, 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 3e05626..610de46 100644 --- a/urchin/utils.py +++ b/urchin/utils.py @@ -1,13 +1,17 @@ """Utilities for URDF parsing.""" +from __future__ import annotations + import os +from typing import Sequence 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 @@ -42,7 +46,7 @@ def rpy_to_matrix(coords): ) -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 @@ -93,7 +97,7 @@ def matrix_to_rpy(R, solution=1): 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 @@ -106,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 @@ -125,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. @@ -158,7 +164,7 @@ def parse_origin(node): 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 @@ -178,12 +184,13 @@ def unparse_origin(matrix): 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])) + 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 @@ -212,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 @@ -225,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: object = 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") 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] else: raise ValueError("Unable to load mesh from file") return meshes -def configure_origin(value): +def configure_origin( + value: None | Sequence[float] | npt.ArrayLike, +) -> npt.NDArray[np.float64]: """Convert a value into a 4x4 transform matrix. Parameters @@ -258,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) @@ -268,9 +276,7 @@ def configure_origin(value): 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" - ) + raise ValueError("Origin must be specified as a 4x4 homogenous transformation matrix") else: raise TypeError("Invalid type for origin, expect 4x4 matrix") return value From 14d117ef7c0c1e924d560643219a395a87406b88 Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Wed, 8 Oct 2025 09:09:11 -0700 Subject: [PATCH 08/13] Improved typing to not use object or Any --- docs/source/conf.py | 6 +- pyproject.toml | 4 +- urchin/base.py | 208 ++++++++++++++++++++++++----------------- urchin/joint.py | 2 +- urchin/link.py | 5 +- urchin/material.py | 8 +- urchin/transmission.py | 8 +- urchin/urdf.py | 2 +- urchin/utils.py | 2 +- 9 files changed, 141 insertions(+), 104 deletions(-) diff --git a/docs/source/conf.py b/docs/source/conf.py index 00c9cce..856896d 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', @@ -136,7 +136,7 @@ # 'preamble': '', # Latex figure (float) alignment # - # 'figure_align': 'htbp', +# 'figure_align': 'htbp', } # Grouping the document tree into LaTeX files. List of tuples diff --git a/pyproject.toml b/pyproject.toml index e6f138f..58bd40b 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -28,7 +28,7 @@ classifiers = [ dependencies = [ "lxml", # For XML DOM Tree "networkx", # For joint graph - "numpy", # Numpy + "numpy>=1.20", # Numpy "pillow", # For texture image loading "pycollada>=0.6", # COLLADA (.dae) mesh loading via trimesh "pyribbit>=0.1.46", # For visualization @@ -56,7 +56,7 @@ strict = false warn_unused_ignores = true [[tool.mypy.overrides]] -module = ["lxml.*", "trimesh.*", "networkx.*", "PIL.*", "pyribbit.*"] +module = ["lxml.*", "trimesh.*", "networkx.*", "PIL.*", "pyribbit.*", "sphinx.*", "sphinx_rtd_theme"] ignore_missing_imports = true [tool.ruff] diff --git a/urchin/base.py b/urchin/base.py index e3113eb..c72240c 100644 --- a/urchin/base.py +++ b/urchin/base.py @@ -1,10 +1,19 @@ from __future__ import annotations -from typing import Dict, Type, cast +from typing import 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. @@ -39,29 +48,28 @@ def __init__(self): pass @classmethod - def _parse_attrib(cls, val_type: type, val: str) -> object: + 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 : :class:`object` - The value to parse. + val : str + The string value to parse. Returns ------- - val : :class:`object` - The parsed attribute. + val : ParsedAttribute + The parsed attribute value. """ if val_type == np.ndarray: - parsed: object = np.fromstring(val, sep=" ") - else: - parsed = val_type(val) - return parsed + 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) -> Dict[str, object]: + def _parse_simple_attribs(cls, node: ET._Element) -> ParsedAttributeDict: """Parse all attributes in the _ATTRIBS array for this class. Parameters @@ -71,34 +79,33 @@ def _parse_simple_attribs(cls, node: ET._Element) -> Dict[str, object]: Returns ------- - kwargs : dict + 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: Dict[str, object] = {} - for a in cls._ATTRIBS: - t, r = cls._ATTRIBS[a] # t = type, r = required (bool) - if r: + kwargs: ParsedAttributeDict = {} + for attrib_name, (val_type, required) in cls._ATTRIBS.items(): + if required: try: - v = cls._parse_attrib(t, node.attrib[a]) + value = cls._parse_attrib(val_type, node.attrib[attrib_name]) except Exception: raise ValueError( "Missing required attribute {} when parsing an object of type {}".format( - a, cls.__name__ + attrib_name, cls.__name__ ) ) else: - v = None - if a in node.attrib: - v = cls._parse_attrib(t, node.attrib[a]) - kwargs[a] = v + 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: bool | None = None - ) -> Dict[str, object]: + ) -> ParsedElementDict: """Parse all elements in the _ELEMENTS array from the children of this node. @@ -115,32 +122,40 @@ def _parse_simple_elements( Returns ------- - kwargs : dict + kwargs : ParsedElementDict Map from element names to the :class:`URDFType` subclass (or list, if ``multiple`` was set) created for that element. """ - kwargs: Dict[str, object] = {} - 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) + 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: - vs = node.findall(t._TAG) - if len(vs) == 0 and r: + element_nodes = node.findall(element_type._TAG) + if len(element_nodes) == 0 and required: print( - f"Missing required subelement(s) of type {t.__name__} when " + f"Missing required subelement(s) of type {element_type.__name__} when " f"parsing an object of type {cls.__name__}." ) - v = [t._from_xml(n, path) for n in vs] - kwargs[a] = v + 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: bool | None = None - ) -> Dict[str, object]: + ) -> ParsedValueDict: """Parse all elements and attributes in the _ELEMENTS and _ATTRIBS arrays for a node. @@ -154,16 +169,19 @@ def _parse( Returns ------- - kwargs : dict - Map from names to Python classes created from the attributes + kwargs : ParsedValueDict + Map from names to Python values created from the attributes and elements in the class arrays. """ - kwargs: Dict[str, object] = cls._parse_simple_attribs(node) + 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, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): + def _from_xml( + cls: type[T], node: ET._Element, path: str, lazy_load_meshes: bool | None = None + ) -> T: """Create an instance of this class from an XML node. Parameters @@ -179,9 +197,9 @@ def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = obj : :class:`URDFType` An instance of this class parsed from the node. """ - return cls(**cls._parse(node, path)) + return cls(**cls._parse(node, path, lazy_load_meshes)) - def _unparse_attrib(self, val_type: type, val: object) -> str: + def _unparse_attrib(self, val_type: type, val: ParsedAttribute) -> str: """Convert a Python value into a string for storage in an XML attribute. @@ -189,7 +207,7 @@ def _unparse_attrib(self, val_type: type, val: object) -> str: ---------- val_type : :class:`type` The type of the Python object. - val : :class:`object` + val : ParsedAttribute The actual value. Returns @@ -198,10 +216,9 @@ def _unparse_attrib(self, val_type: type, val: object) -> str: The attribute string. """ if val_type == np.ndarray: - val = np.array2string(cast(np.ndarray, val))[1:-1] - else: - val = str(val) - return val + 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 @@ -209,14 +226,13 @@ def _unparse_simple_attribs(self, node: ET._Element) -> None: Parameters ---------- - node : :class:`object` + node : :class:`lxml.etree.Element` 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) + 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 @@ -224,23 +240,23 @@ def _unparse_simple_elements(self, node: ET._Element, path: str) -> None: Parameters ---------- - node : :class:`object` + 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 a in self._ELEMENTS: - t, r, m = self._ELEMENTS[a] - v = getattr(self, a, None) - if not m: - if v is not None: - node.append(v._to_xml(node, path)) + for element_name, (element_type, _required, multiple) in self._ELEMENTS.items(): + value = getattr(self, element_name, None) + if not multiple: + element_value = cast(URDFType | None, value) + if element_value is not None: + node.append(element_value._to_xml(node, path)) else: - vs = v or [] - for v in vs: - node.append(v._to_xml(node, path)) + element_values = cast(list[URDFType] | None, 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 @@ -287,7 +303,7 @@ class URDFTypeWithMesh(URDFType): @classmethod def _parse_simple_elements( cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None - ) -> Dict[str, object]: + ) -> ParsedElementDict: """Parse all elements in the _ELEMENTS array from the children of this node. @@ -304,38 +320,53 @@ def _parse_simple_elements( Returns ------- - kwargs : dict + kwargs : ParsedElementDict Map from element names to the :class:`URDFType` subclass (or list, if ``multiple`` was set) created for that element. """ - kwargs: Dict[str, object] = {} - 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) + 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: - v = t._from_xml(v, path) + value = cast( + ParsedElement, + element_type._from_xml(element_node, path), + ) + else: + value = None else: - vs = node.findall(t._TAG) - if len(vs) == 0 and r: + 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(t.__name__, cls.__name__) + "parsing an object of type {}".format( + element_type.__name__, cls.__name__ + ) ) - if issubclass(t, URDFTypeWithMesh): - v = [t._from_xml(n, path, lazy_load_meshes) for n in vs] + if issubclass(element_type, URDFTypeWithMesh): + value = [ + element_type._from_xml(child, path, lazy_load_meshes) + for child in element_nodes + ] else: - v = [t._from_xml(n, path) for n in vs] - kwargs[a] = v + 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: bool | None = None - ) -> Dict[str, object]: + ) -> ParsedValueDict: """Parse all elements and attributes in the _ELEMENTS and _ATTRIBS arrays for a node. @@ -351,16 +382,19 @@ def _parse( Returns ------- - kwargs : dict - Map from names to Python classes created from the attributes + kwargs : ParsedValueDict + Map from names to Python values created from the attributes and elements in the class arrays. """ - kwargs: Dict[str, object] = cls._parse_simple_attribs(node) + 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, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): + def _from_xml( + cls: type[T], node: ET._Element, path: str, lazy_load_meshes: bool | None = None + ) -> T: """Create an instance of this class from an XML node. Parameters diff --git a/urchin/joint.py b/urchin/joint.py index 43fd9f9..268926a 100644 --- a/urchin/joint.py +++ b/urchin/joint.py @@ -382,7 +382,7 @@ def joint(self) -> str: return self._joint @joint.setter - def joint(self, value: object) -> None: + def joint(self, value: str) -> None: self._joint = str(value) @property diff --git a/urchin/link.py b/urchin/link.py index 8175689..43f4575 100644 --- a/urchin/link.py +++ b/urchin/link.py @@ -378,8 +378,9 @@ def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: # Export the meshes as a single file if self._meshes is not None: meshes_list = self.meshes or [] + export_obj: trimesh.Trimesh | trimesh.Scene | list[trimesh.Trimesh] if len(meshes_list) == 1: - export_obj: object = meshes_list[0] + export_obj = meshes_list[0] elif os.path.splitext(fn)[1] == ".glb": export_obj = trimesh.scene.Scene(geometry=meshes_list) else: @@ -946,7 +947,7 @@ def name(self) -> str: return self._name @name.setter - def name(self, value: object) -> None: + def name(self, value: str) -> None: self._name = str(value) @property diff --git a/urchin/material.py b/urchin/material.py index 384610b..9d8e9aa 100644 --- a/urchin/material.py +++ b/urchin/material.py @@ -1,5 +1,7 @@ from __future__ import annotations +from os import PathLike, fspath + import numpy as np import numpy.typing as npt import PIL @@ -38,8 +40,8 @@ def filename(self) -> str: return self._filename @filename.setter - def filename(self, value: object) -> None: - self._filename = str(value) + def filename(self, value: str | PathLike[str]) -> None: + self._filename = fspath(value) @property def image(self) -> PIL.Image.Image: @@ -123,7 +125,7 @@ def name(self) -> str: return self._name @name.setter - def name(self, value: object) -> None: + def name(self, value: str) -> None: self._name = str(value) @property diff --git a/urchin/transmission.py b/urchin/transmission.py index 44bf67a..7fdbda9 100644 --- a/urchin/transmission.py +++ b/urchin/transmission.py @@ -41,7 +41,7 @@ def name(self) -> str: return self._name @name.setter - def name(self, value: object) -> None: + def name(self, value: str) -> None: self._name = str(value) @property @@ -138,7 +138,7 @@ def name(self) -> str: return self._name @name.setter - def name(self, value: object) -> None: + def name(self, value: str) -> None: self._name = str(value) @property @@ -239,7 +239,7 @@ def name(self) -> str: return self._name @name.setter - def name(self, value: object) -> None: + def name(self, value: str) -> None: self._name = str(value) @property @@ -248,7 +248,7 @@ def trans_type(self) -> str: return self._trans_type @trans_type.setter - def trans_type(self, value: object) -> None: + def trans_type(self, value: str) -> None: self._trans_type = str(value) @property diff --git a/urchin/urdf.py b/urchin/urdf.py index 7691977..f73f68b 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -143,7 +143,7 @@ def name(self) -> str: return self._name @name.setter - def name(self, value: object) -> None: + def name(self, value: str) -> None: self._name = str(value) @property diff --git a/urchin/utils.py b/urchin/utils.py index 610de46..63d4025 100644 --- a/urchin/utils.py +++ b/urchin/utils.py @@ -232,7 +232,7 @@ def load_meshes(filename: str) -> list[trimesh.Trimesh]: meshes : list of :class:`~trimesh.base.Trimesh` The meshes loaded from the file. """ - meshes_obj: object = trimesh.load(filename) + meshes_obj: trimesh.Geometry = trimesh.load(filename) # If we got a scene, dump the meshes if isinstance(meshes_obj, trimesh.Scene): From 56ba4768abf2a1805fc0086f51c51dd576556883 Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Wed, 8 Oct 2025 09:12:26 -0700 Subject: [PATCH 09/13] Fixed 3.9 Union issues --- urchin/urdf.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/urchin/urdf.py b/urchin/urdf.py index f73f68b..8bef451 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -3,7 +3,7 @@ import os import time from collections import OrderedDict -from typing import IO, Mapping, Sequence, cast +from typing import IO, Mapping, Sequence, Union, cast import networkx as nx import numpy as np @@ -1411,7 +1411,11 @@ def _process_cfgs( from typing import cast as _cast return _cast( - dict[Joint, Sequence[float] | npt.NDArray[np.float64] | None], joint_cfg + dict[ + Joint, + Union[Sequence[float], npt.NDArray[np.float64], None], + ], + joint_cfg, ), n_cfgs @classmethod From 2cb4985864f2d33f0345581a931cf2156a7b5349 Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Wed, 8 Oct 2025 09:25:56 -0700 Subject: [PATCH 10/13] Changed union specification to support python 3.9 --- urchin/base.py | 26 ++++--- urchin/joint.py | 82 +++++++++++---------- urchin/link.py | 157 +++++++++++++++++++++++++---------------- urchin/material.py | 41 +++++++---- urchin/transmission.py | 48 +++++++------ urchin/urdf.py | 150 ++++++++++++++++++++++++--------------- urchin/utils.py | 4 +- 7 files changed, 308 insertions(+), 200 deletions(-) diff --git a/urchin/base.py b/urchin/base.py index c72240c..5ccf768 100644 --- a/urchin/base.py +++ b/urchin/base.py @@ -1,6 +1,6 @@ from __future__ import annotations -from typing import Type, TypeVar, Union, cast +from typing import Optional, Type, TypeVar, Union, cast import numpy as np import numpy.typing as npt @@ -104,7 +104,7 @@ def _parse_simple_attribs(cls, node: ET._Element) -> ParsedAttributeDict: @classmethod def _parse_simple_elements( - cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None + 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. @@ -154,7 +154,7 @@ def _parse_simple_elements( @classmethod def _parse( - cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None + 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. @@ -180,7 +180,10 @@ def _parse( @classmethod def _from_xml( - cls: type[T], node: ET._Element, path: str, lazy_load_meshes: bool | None = None + 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. @@ -250,11 +253,11 @@ def _unparse_simple_elements(self, node: ET._Element, path: str) -> None: for element_name, (element_type, _required, multiple) in self._ELEMENTS.items(): value = getattr(self, element_name, None) if not multiple: - element_value = cast(URDFType | None, value) + element_value = cast(Optional[URDFType], value) if element_value is not None: node.append(element_value._to_xml(node, path)) else: - element_values = cast(list[URDFType] | None, value) + element_values = cast(Optional[list[URDFType]], value) for child in element_values or []: node.append(child._to_xml(node, path)) @@ -278,7 +281,7 @@ def _unparse(self, path: str) -> ET._Element: self._unparse_simple_elements(node, path) return node - def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: + def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: """Create and return an XML node for this object. Parameters @@ -302,7 +305,7 @@ def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: class URDFTypeWithMesh(URDFType): @classmethod def _parse_simple_elements( - cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None + 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. @@ -365,7 +368,7 @@ def _parse_simple_elements( @classmethod def _parse( - cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None + 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. @@ -393,7 +396,10 @@ def _parse( @classmethod def _from_xml( - cls: type[T], node: ET._Element, path: str, lazy_load_meshes: bool | None = None + 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. diff --git a/urchin/joint.py b/urchin/joint.py index 268926a..820d358 100644 --- a/urchin/joint.py +++ b/urchin/joint.py @@ -1,6 +1,6 @@ from __future__ import annotations -from typing import Sequence +from typing import Optional, Sequence, Union import numpy as np import numpy.typing as npt @@ -41,9 +41,9 @@ class SafetyController(URDFType): def __init__( self, k_velocity: float, - k_position: float | None = None, - soft_lower_limit: float | None = None, - soft_upper_limit: float | None = None, + 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 @@ -56,7 +56,7 @@ def soft_lower_limit(self) -> float: return self._soft_lower_limit @soft_lower_limit.setter - def soft_lower_limit(self, value: float | None) -> None: + def soft_lower_limit(self, value: Optional[float]) -> None: if value is not None: value = float(value) else: @@ -69,7 +69,7 @@ def soft_upper_limit(self) -> float: return self._soft_upper_limit @soft_upper_limit.setter - def soft_upper_limit(self, value: float | None) -> None: + def soft_upper_limit(self, value: Optional[float]) -> None: if value is not None: value = float(value) else: @@ -82,7 +82,7 @@ def k_position(self) -> float: return self._k_position @k_position.setter - def k_position(self, value: float | None) -> None: + def k_position(self, value: Optional[float]) -> None: if value is not None: value = float(value) else: @@ -99,7 +99,7 @@ def k_velocity(self, value: float) -> None: self._k_velocity = float(value) def copy( - self, prefix: str = "", scale: float | Sequence[float] | None = None + 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. @@ -137,34 +137,34 @@ class JointCalibration(URDFType): _ATTRIBS = {"rising": (float, False), "falling": (float, False)} _TAG = "calibration" - def __init__(self, rising: float | None = None, falling: float | None = None): + def __init__(self, rising: Optional[float] = None, falling: Optional[float] = None): self.rising = rising self.falling = falling @property - def rising(self) -> float | None: + def rising(self) -> Optional[float]: """float : description.""" return self._rising @rising.setter - def rising(self, value: float | None) -> None: + def rising(self, value: Optional[float]) -> None: if value is not None: value = float(value) self._rising = value @property - def falling(self) -> float | None: + def falling(self) -> Optional[float]: """float : description.""" return self._falling @falling.setter - def falling(self, value: float | None) -> None: + def falling(self, value: Optional[float]) -> None: if value is not None: value = float(value) self._falling = value def copy( - self, prefix: str = "", scale: float | Sequence[float] | None = None + 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. @@ -203,34 +203,34 @@ class JointDynamics(URDFType): } _TAG = "dynamics" - def __init__(self, damping: float | None, friction: float | None): + def __init__(self, damping: Optional[float], friction: Optional[float]): self.damping = damping self.friction = friction @property - def damping(self) -> float | None: + def damping(self) -> Optional[float]: """float : The damping value of the joint.""" return self._damping @damping.setter - def damping(self, value: float | None) -> None: + def damping(self, value: Optional[float]) -> None: if value is not None: value = float(value) self._damping = value @property - def friction(self) -> float | None: + def friction(self) -> Optional[float]: """float : The static friction value of the joint.""" return self._friction @friction.setter - def friction(self, value: float | None) -> None: + def friction(self, value: Optional[float]) -> None: if value is not None: value = float(value) self._friction = value def copy( - self, prefix: str = "", scale: float | Sequence[float] | None = None + self, prefix: str = "", scale: Union[float, Sequence[float], None] = None ) -> "JointDynamics": """Create a deep copy with the prefix applied to all names. @@ -278,8 +278,8 @@ def __init__( self, effort: float, velocity: float, - lower: float | None = None, - upper: float | None = None, + lower: Optional[float] = None, + upper: Optional[float] = None, ): self.effort = effort self.velocity = velocity @@ -305,28 +305,30 @@ def velocity(self, value: float) -> None: self._velocity = float(value) @property - def lower(self) -> float | None: + def lower(self) -> Optional[float]: """float : The lower joint limit.""" return self._lower @lower.setter - def lower(self, value: float | None) -> None: + def lower(self, value: Optional[float]) -> None: if value is not None: value = float(value) self._lower = value @property - def upper(self) -> float | None: + def upper(self) -> Optional[float]: """float : The upper joint limit.""" return self._upper @upper.setter - def upper(self, value: float | None) -> None: + def upper(self, value: Optional[float]) -> None: if value is not None: value = float(value) self._upper = value - def copy(self, prefix: str = "", scale: float | Sequence[float] | None = None) -> "JointLimit": + 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 @@ -371,7 +373,9 @@ class JointMimic(URDFType): } _TAG = "mimic" - def __init__(self, joint: str, multiplier: float | None = None, offset: float | None = None): + def __init__( + self, joint: str, multiplier: Optional[float] = None, offset: Optional[float] = None + ): self.joint = joint self.multiplier = multiplier self.offset = offset @@ -391,7 +395,7 @@ def multiplier(self) -> float: return self._multiplier @multiplier.setter - def multiplier(self, value: float | None) -> None: + def multiplier(self, value: Optional[float]) -> None: if value is not None: value = float(value) else: @@ -404,14 +408,16 @@ def offset(self) -> float: return self._offset @offset.setter - def offset(self, value: float | None) -> None: + 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: float | Sequence[float] | None = None) -> "JointMimic": + 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 @@ -645,7 +651,7 @@ def mimic(self, value): raise TypeError("Expected JointMimic type") self._mimic = value - def is_valid(self, cfg: float | npt.ArrayLike) -> bool: + def is_valid(self, cfg: Union[float, npt.ArrayLike]) -> bool: """Check if the provided configuration value is valid for this joint. Parameters @@ -671,7 +677,9 @@ def is_valid(self, cfg: float | npt.ArrayLike) -> bool: upper = self.limit.upper return cfg_val >= lower and cfg_val <= upper - def get_child_pose(self, cfg: float | npt.ArrayLike | None = None) -> np.ndarray: + 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. @@ -736,7 +744,7 @@ def get_child_pose(self, cfg: float | npt.ArrayLike | None = None) -> np.ndarray else: raise ValueError("Invalid configuration") - def get_child_poses(self, cfg: npt.ArrayLike | None, n_cfgs: int) -> np.ndarray: + 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. @@ -784,7 +792,9 @@ def get_child_poses(self, cfg: npt.ArrayLike | None, n_cfgs: int) -> np.ndarray: raise ValueError("Invalid configuration") @classmethod - def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): + 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"] @@ -796,7 +806,7 @@ def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = kwargs["origin"] = parse_origin(node) return cls(**kwargs) - def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: + 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 diff --git a/urchin/link.py b/urchin/link.py index 43f4575..6c8c93c 100644 --- a/urchin/link.py +++ b/urchin/link.py @@ -1,7 +1,7 @@ from __future__ import annotations import os -from typing import Sequence +from typing import Optional, Sequence, Union import numpy as np import numpy.typing as npt @@ -55,7 +55,9 @@ def meshes(self) -> list[trimesh.Trimesh]: self._meshes = [trimesh.creation.box(extents=self.size)] return self._meshes - def copy(self, prefix: str = "", scale: float | Sequence[float] | None = None) -> "Box": + 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 @@ -130,7 +132,9 @@ def meshes(self) -> list[trimesh.Trimesh]: self._meshes = [trimesh.creation.cylinder(radius=self.radius, height=self.length)] return self._meshes - def copy(self, prefix: str = "", scale: float | Sequence[float] | None = None) -> "Cylinder": + 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 @@ -208,7 +212,9 @@ def meshes(self) -> list[trimesh.Trimesh]: self._meshes = [trimesh.creation.icosphere(radius=self.radius)] return self._meshes - def copy(self, prefix: str = "", scale: float | Sequence[float] | None = None) -> "Sphere": + 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 @@ -269,9 +275,11 @@ def __init__( self, filename: str, combine: bool, - scale: npt.ArrayLike | None = None, - meshes: list[trimesh.Trimesh] | trimesh.Trimesh | str | None = None, - lazy_filename: str | None = None, + 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: @@ -294,12 +302,12 @@ def filename(self, value: str) -> None: self._filename = value @property - def scale(self) -> np.ndarray | None: + 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: npt.ArrayLike | None) -> None: + def scale(self, value: Optional[npt.ArrayLike]) -> None: if value is not None: value = np.asanyarray(value).astype(np.float64) self._scale = value @@ -315,7 +323,10 @@ def meshes(self) -> list[trimesh.Trimesh]: return self._meshes or [] @meshes.setter - def meshes(self, value: list[trimesh.Trimesh] | trimesh.Trimesh | str | None) -> None: + 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): @@ -347,7 +358,9 @@ def _load_and_combine_meshes(cls, fn: str, combine: bool) -> list[trimesh.Trimes return meshes @classmethod - def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): + 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") @@ -371,14 +384,16 @@ def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = lazy_filename=lazy_filename, ) - def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: + 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: trimesh.Trimesh | trimesh.Scene | list[trimesh.Trimesh] + 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": @@ -391,7 +406,9 @@ def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: node = self._unparse(path) return node - def copy(self, prefix: str = "", scale: float | Sequence[float] | None = None) -> "Mesh": + 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 @@ -457,10 +474,10 @@ class Geometry(URDFTypeWithMesh): def __init__( self, - box: Box | None = None, - cylinder: Cylinder | None = None, - sphere: Sphere | None = None, - mesh: Mesh | None = None, + 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") @@ -470,51 +487,51 @@ def __init__( self.mesh = mesh @property - def box(self) -> Box | None: + def box(self) -> Optional[Box]: """:class:`.Box` : Box geometry.""" return self._box @box.setter - def box(self, value: Box | None) -> None: + 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) -> Cylinder | None: + def cylinder(self) -> Optional[Cylinder]: """:class:`.Cylinder` : Cylinder geometry.""" return self._cylinder @cylinder.setter - def cylinder(self, value: Cylinder | None) -> None: + 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) -> Sphere | None: + def sphere(self) -> Optional[Sphere]: """:class:`.Sphere` : Spherical geometry.""" return self._sphere @sphere.setter - def sphere(self, value: Sphere | None) -> None: + 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) -> Mesh | None: + def mesh(self) -> Optional[Mesh]: """:class:`.Mesh` : Mesh geometry.""" return self._mesh @mesh.setter - def mesh(self, value: Mesh | None) -> None: + 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) -> Box | Cylinder | Sphere | Mesh | None: + def geometry(self) -> Union[Box, Cylinder, Sphere, Mesh, None]: """:class:`.Box`, :class:`.Cylinder`, :class:`.Sphere`, or :class:`.Mesh` : The valid geometry element. """ @@ -536,7 +553,9 @@ def meshes(self) -> list[trimesh.Trimesh]: assert self.geometry is not None return self.geometry.meshes - def copy(self, prefix: str = "", scale: float | Sequence[float] | None = None) -> "Geometry": + 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 @@ -580,7 +599,9 @@ class Collision(URDFTypeWithMesh): } _TAG = "collision" - def __init__(self, name: str | None, origin: npt.ArrayLike | None, geometry: Geometry): + def __init__( + self, name: Optional[str], origin: Optional[npt.ArrayLike], geometry: Geometry + ): self.geometry = geometry self.name = name self.origin = origin @@ -597,12 +618,12 @@ def geometry(self, value: Geometry) -> None: self._geometry = value @property - def name(self) -> str | None: + def name(self) -> Optional[str]: """str : The name of this collision element.""" return self._name @name.setter - def name(self, value: str | None) -> None: + def name(self, value: Optional[str]) -> None: if value is not None: value = str(value) self._name = value @@ -613,11 +634,13 @@ def origin(self) -> np.ndarray: return self._origin @origin.setter - def origin(self, value: npt.ArrayLike | None) -> None: + 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: bool | None = None): + 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: @@ -626,12 +649,14 @@ def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = origin = parse_origin(node) return cls(name=name, origin=origin, geometry=geometry) - def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: + 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: float | Sequence[float] | None = None) -> "Collision": + 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 @@ -688,9 +713,9 @@ class Visual(URDFTypeWithMesh): def __init__( self, geometry: Geometry, - name: str | None = None, - origin: npt.ArrayLike | None = None, - material: Material | None = None, + name: Optional[str] = None, + origin: Optional[npt.ArrayLike] = None, + material: Optional[Material] = None, ): self.geometry = geometry self.name = name @@ -709,12 +734,12 @@ def geometry(self, value: Geometry) -> None: self._geometry = value @property - def name(self) -> str | None: + def name(self) -> Optional[str]: """str : The name of this visual element.""" return self._name @name.setter - def name(self, value: str | None) -> None: + def name(self, value: Optional[str]) -> None: if value is not None: value = str(value) self._name = value @@ -725,23 +750,25 @@ def origin(self) -> np.ndarray: return self._origin @origin.setter - def origin(self, value: npt.ArrayLike | None) -> None: + def origin(self, value: Optional[npt.ArrayLike]) -> None: self._origin = configure_origin(value) @property - def material(self) -> Material | None: + def material(self) -> Optional[Material]: """:class:`.Material` : The material for this element.""" return self._material @material.setter - def material(self, value: Material | None) -> None: + 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: bool | None = None): + 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") @@ -752,12 +779,14 @@ def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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: ET._Element | None, path: str) -> ET._Element: + 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: float | Sequence[float] | None = None) -> "Visual": + 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 @@ -805,7 +834,9 @@ class Inertial(URDFType): _TAG = "inertial" - def __init__(self, mass: float, inertia: npt.ArrayLike, origin: npt.ArrayLike | None = None): + def __init__( + self, mass: float, inertia: npt.ArrayLike, origin: Optional[npt.ArrayLike] = None + ): self.mass = mass self.inertia = inertia self.origin = origin @@ -837,11 +868,13 @@ def origin(self) -> np.ndarray: return self._origin @origin.setter - def origin(self, value: npt.ArrayLike | None) -> None: + 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: bool | None = None): + 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") @@ -854,7 +887,7 @@ def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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: ET._Element | None, path: str) -> ET._Element: + 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") @@ -873,9 +906,9 @@ def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: def copy( self, prefix: str = "", - mass: float | None = None, - origin: np.ndarray | None = None, - inertia: np.ndarray | None = None, + 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. @@ -930,16 +963,16 @@ class Link(URDFTypeWithMesh): def __init__( self, name: str, - inertial: Inertial | None, - visuals: Sequence[Visual] | None, - collisions: Sequence[Collision] | None, + 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: trimesh.Trimesh | None = None + self._collision_mesh: Optional[trimesh.Trimesh] = None @property def name(self) -> str: @@ -956,7 +989,7 @@ def inertial(self) -> Inertial: return self._inertial @inertial.setter - def inertial(self, value: Inertial | None) -> None: + 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 @@ -970,7 +1003,7 @@ def visuals(self) -> list[Visual]: return self._visuals @visuals.setter - def visuals(self, value: Sequence[Visual] | None) -> None: + def visuals(self, value: Optional[Sequence[Visual]]) -> None: if value is None: value = [] else: @@ -986,7 +1019,7 @@ def collisions(self) -> list[Collision]: return self._collisions @collisions.setter - def collisions(self, value: Sequence[Collision] | None) -> None: + def collisions(self, value: Optional[Sequence[Collision]]) -> None: if value is None: value = [] else: @@ -997,7 +1030,7 @@ def collisions(self, value: Sequence[Collision] | None) -> None: self._collisions = value @property - def collision_mesh(self) -> trimesh.Trimesh | None: + 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. """ @@ -1027,7 +1060,7 @@ def collision_mesh(self) -> trimesh.Trimesh | None: def copy( self, prefix: str = "", - scale: float | Sequence[float] | None = None, + scale: Union[float, Sequence[float], None] = None, collision_only: bool = False, ) -> "Link": """Create a deep copy of the link. diff --git a/urchin/material.py b/urchin/material.py index 9d8e9aa..ad4d517 100644 --- a/urchin/material.py +++ b/urchin/material.py @@ -1,6 +1,7 @@ from __future__ import annotations from os import PathLike, fspath +from typing import Optional, Union import numpy as np import numpy.typing as npt @@ -28,7 +29,11 @@ class Texture(URDFType): _ATTRIBS = {"filename": (str, True)} _TAG = "texture" - def __init__(self, filename: str, image: PIL.Image.Image | str | np.ndarray | None = None): + 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 @@ -40,7 +45,7 @@ def filename(self) -> str: return self._filename @filename.setter - def filename(self, value: str | PathLike[str]) -> None: + def filename(self, value: Union[str, PathLike[str]]) -> None: self._filename = fspath(value) @property @@ -49,7 +54,7 @@ def image(self) -> PIL.Image.Image: return self._image @image.setter - def image(self, value: PIL.Image.Image | str | np.ndarray) -> None: + 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): @@ -59,21 +64,25 @@ def image(self, value: PIL.Image.Image | str | np.ndarray) -> None: self._image = value @classmethod - def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): + 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: ET._Element | None, path: str) -> ET._Element: + 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: float | np.ndarray | None = None) -> "Texture": + 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 @@ -112,8 +121,8 @@ class Material(URDFType): def __init__( self, name: str, - color: npt.ArrayLike | None = None, - texture: Texture | str | None = None, + color: Optional[npt.ArrayLike] = None, + texture: Union[Texture, str, None] = None, ): self.name = name self.color = color @@ -129,12 +138,12 @@ def name(self, value: str) -> None: self._name = str(value) @property - def color(self) -> np.ndarray | None: + 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: npt.ArrayLike | None) -> None: + 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) @@ -143,12 +152,12 @@ def color(self, value: npt.ArrayLike | None) -> None: self._color = value @property - def texture(self) -> Texture | None: + def texture(self) -> Optional[Texture]: """:class:`.Texture` : The texture for the material.""" return self._texture @texture.setter - def texture(self, value: Texture | str | None) -> None: + def texture(self, value: Union[Texture, str, None]) -> None: if value is not None: if isinstance(value, str): image = PIL.Image.open(value) @@ -158,7 +167,9 @@ def texture(self, value: Texture | str | None) -> None: self._texture = value @classmethod - def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): + 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") @@ -188,7 +199,9 @@ def _to_xml(self, parent: ET._Element, path: str) -> ET._Element: node.append(color) return node - def copy(self, prefix: str = "", scale: float | np.ndarray | None = None) -> "Material": + 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 diff --git a/urchin/transmission.py b/urchin/transmission.py index 7fdbda9..aaa0225 100644 --- a/urchin/transmission.py +++ b/urchin/transmission.py @@ -1,6 +1,6 @@ from __future__ import annotations -from typing import Sequence +from typing import Optional, Sequence, Union from lxml import etree as ET @@ -28,8 +28,8 @@ class Actuator(URDFType): def __init__( self, name: str, - mechanicalReduction: float | None = None, - hardwareInterfaces: Sequence[str] | None = None, + mechanicalReduction: Optional[float] = None, + hardwareInterfaces: Optional[Sequence[str]] = None, ): self.name = name self.mechanicalReduction = mechanicalReduction @@ -45,12 +45,12 @@ def name(self, value: str) -> None: self._name = str(value) @property - def mechanicalReduction(self) -> float | None: + def mechanicalReduction(self) -> Optional[float]: """float | None : Mechanical reduction (ratio).""" return self._mechanicalReduction @mechanicalReduction.setter - def mechanicalReduction(self, value: float | str | None) -> None: + def mechanicalReduction(self, value: Union[float, str, None]) -> None: if value is not None: value = float(value) self._mechanicalReduction = value @@ -61,7 +61,7 @@ def hardwareInterfaces(self) -> list[str]: return self._hardwareInterfaces @hardwareInterfaces.setter - def hardwareInterfaces(self, value: Sequence[str] | None) -> None: + def hardwareInterfaces(self, value: Optional[Sequence[str]]) -> None: if value is None: value = [] else: @@ -71,7 +71,9 @@ def hardwareInterfaces(self, value: Sequence[str] | None) -> None: self._hardwareInterfaces = value @classmethod - def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): + 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 @@ -79,7 +81,7 @@ def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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: ET._Element | None, path: str) -> ET._Element: + 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") @@ -92,7 +94,7 @@ def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: node.append(h) return node - def copy(self, prefix: str = "", scale: float | None = None) -> "Actuator": + def copy(self, prefix: str = "", scale: Optional[float] = None) -> "Actuator": """Create a deep copy with the prefix applied to all names. Parameters @@ -128,7 +130,7 @@ class TransmissionJoint(URDFType): } _TAG = "joint" - def __init__(self, name: str, hardwareInterfaces: Sequence[str] | None): + def __init__(self, name: str, hardwareInterfaces: Optional[Sequence[str]]): self.name = name self.hardwareInterfaces = hardwareInterfaces @@ -147,7 +149,7 @@ def hardwareInterfaces(self) -> list[str]: return self._hardwareInterfaces @hardwareInterfaces.setter - def hardwareInterfaces(self, value: Sequence[str] | None) -> None: + def hardwareInterfaces(self, value: Optional[Sequence[str]]) -> None: if value is None: value = [] else: @@ -157,13 +159,15 @@ def hardwareInterfaces(self, value: Sequence[str] | None) -> None: self._hardwareInterfaces = value @classmethod - def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): + 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: ET._Element | None, path: str) -> ET._Element: + 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: @@ -172,7 +176,7 @@ def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: node.append(h) return node - def copy(self, prefix: str = "", scale: float | None = None) -> "TransmissionJoint": + def copy(self, prefix: str = "", scale: Optional[float] = None) -> "TransmissionJoint": """Create a deep copy with the prefix applied to all names. Parameters @@ -225,8 +229,8 @@ def __init__( self, name: str, trans_type: str, - joints: Sequence["TransmissionJoint"] | None = None, - actuators: Sequence[Actuator] | None = None, + joints: Optional[Sequence["TransmissionJoint"]] = None, + actuators: Optional[Sequence[Actuator]] = None, ): self.name = name self.trans_type = trans_type @@ -259,7 +263,7 @@ def joints(self) -> list["TransmissionJoint"]: return self._joints @joints.setter - def joints(self, value: Sequence["TransmissionJoint"] | None) -> None: + def joints(self, value: Optional[Sequence["TransmissionJoint"]]) -> None: if value is None: value = [] else: @@ -275,7 +279,7 @@ def actuators(self) -> list[Actuator]: return self._actuators @actuators.setter - def actuators(self, value: Sequence[Actuator] | None) -> None: + def actuators(self, value: Optional[Sequence[Actuator]]) -> None: if value is None: value = [] else: @@ -286,7 +290,9 @@ def actuators(self, value: Sequence[Actuator] | None) -> None: self._actuators = value @classmethod - def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None): + 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: @@ -296,7 +302,7 @@ def _from_xml(cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = 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: ET._Element | None, path: str) -> ET._Element: + 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 @@ -304,7 +310,7 @@ def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: return node def copy( - self, prefix: str = "", scale: float | Sequence[float] | None = None + self, prefix: str = "", scale: Union[float, Sequence[float], None] = None ) -> "Transmission": """Create a deep copy with the prefix applied to all names. diff --git a/urchin/urdf.py b/urchin/urdf.py index 8bef451..68c975c 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -3,7 +3,7 @@ import os import time from collections import OrderedDict -from typing import IO, Mapping, Sequence, Union, cast +from typing import IO, Mapping, Optional, Sequence, Union, cast import networkx as nx import numpy as np @@ -56,10 +56,10 @@ def __init__( self, name: str, links: Sequence[Link], - joints: Sequence[Joint] | None = None, - transmissions: Sequence[Transmission] | None = None, - materials: Sequence[Material] | None = None, - other_xml: bytes | str | None = None, + 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 = [] @@ -227,12 +227,12 @@ def material_map(self) -> dict[str, Material]: return dict(self._material_map) @property - def other_xml(self) -> bytes | str | None: + 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: bytes | str | None) -> None: + def other_xml(self, value: Optional[Union[bytes, str]]) -> None: self._other_xml = value @property @@ -256,8 +256,14 @@ def actuated_joint_names(self) -> list[str]: return [j.name for j in self._actuated_joints] def cfg_to_vector( - self, cfg: Mapping[str, float] | Sequence[float] | npt.ArrayLike | None - ) -> npt.NDArray[np.float64] | None: + 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 @@ -338,15 +344,20 @@ def joint_limits(self) -> npt.NDArray[np.float64]: def link_fk( self, - cfg: Mapping[str, float] | Sequence[float] | npt.ArrayLike | None = None, - link: str | Link | None = None, - links: Sequence[str | Link] | None = None, + 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, - ) -> ( - dict[Link, npt.NDArray[np.float64]] - | dict[str, npt.NDArray[np.float64]] - | npt.NDArray[np.float64] - ): + ) -> 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 @@ -517,8 +528,13 @@ def link_fk_batch(self, cfgs=None, link=None, links=None, use_names=False): def visual_geometry_fk( self, - cfg: Mapping[str, float] | Sequence[float] | npt.ArrayLike | None = None, - links: Sequence[str | Link] | None = None, + 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. @@ -552,13 +568,13 @@ def visual_geometry_fk( def visual_geometry_fk_batch( self, - cfgs: ( - Mapping[str, Sequence[float]] - | Sequence[Mapping[str, float] | None] - | npt.ArrayLike - | None - ) = None, - links: Sequence[str | Link] | None = None, + 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. @@ -591,8 +607,13 @@ def visual_geometry_fk_batch( def visual_trimesh_fk( self, - cfg: Mapping[str, float] | Sequence[float] | npt.ArrayLike | None = None, - links: Sequence[str | Link] | None = None, + 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. @@ -665,13 +686,13 @@ def visual_trimesh_fk( def visual_trimesh_fk_batch( self, - cfgs: ( - Mapping[str, Sequence[float]] - | Sequence[Mapping[str, float] | None] - | npt.ArrayLike - | None - ) = None, - links: Sequence[str | Link] | None = None, + 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. @@ -985,7 +1006,12 @@ def animate(self, cfg_trajectory=None, loop_time=3.0, use_collision=False): def show( self, - cfg: Mapping[str, float] | Sequence[float] | npt.ArrayLike | None = None, + cfg: Union[ + Mapping[str, float], + Sequence[float], + npt.ArrayLike, + None, + ] = None, use_collision: bool = False, ) -> None: """Visualize the URDF in a given configuration. @@ -1018,9 +1044,9 @@ def show( def copy( self, - name: str | None = None, + name: Optional[str] = None, prefix: str = "", - scale: float | Sequence[float] | None = None, + scale: Union[float, Sequence[float], None] = None, collision_only: bool = False, ) -> "URDF": """Make a deep copy of the URDF. @@ -1050,7 +1076,7 @@ def copy( other_xml=self.other_xml, ) - def save(self, file_obj: str | IO[bytes] | IO[str]) -> None: + def save(self, file_obj: Union[str, IO[bytes], IO[str]]) -> None: """Save this URDF to a file. Parameters @@ -1078,9 +1104,9 @@ def save(self, file_obj: str | IO[bytes] | IO[str]) -> None: def join( self, other: "URDF", - link: Link | str, - origin: npt.ArrayLike | None = None, - name: str | None = None, + 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. @@ -1159,7 +1185,9 @@ def _merge_materials(self) -> None: self._material_map[v.material.name] = v.material @classmethod - def load(cls, file_obj: str | IO[bytes] | IO[str], lazy_load_meshes: bool = False) -> "URDF": + def load( + cls, file_obj: Union[str, IO[bytes], IO[str]], lazy_load_meshes: bool = False + ) -> "URDF": """Load a URDF from a file. Parameters @@ -1306,7 +1334,7 @@ def _validate_graph(self) -> tuple[Link, list[Link]]: raise ValueError("There are cycles in the link graph") # Ensure that there is exactly one base link, which has no parent - base_link: Link | None = None + base_link: Optional[Link] = None end_links: list[Link] = [] for n in self._G: if len(nx.descendants(self._G, n)) == 0: @@ -1323,7 +1351,13 @@ def _validate_graph(self) -> tuple[Link, list[Link]]: return base_link, end_links def _process_cfg( - self, cfg: Mapping[str, float] | Sequence[float] | npt.ArrayLike | None + 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. @@ -1350,20 +1384,26 @@ def _process_cfg( def _process_cfgs( self, - cfgs: ( - Mapping[str, Sequence[float]] - | Sequence[Mapping[str, float] | None] - | npt.ArrayLike - | None - ), - ) -> tuple[dict[Joint, Sequence[float] | npt.NDArray[np.float64] | None], int | None]: + 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: dict[Joint, list[float] | npt.NDArray[np.float64] | None] = { + joint_cfg: dict[ + Joint, + Union[list[float], npt.NDArray[np.float64], None], + ] = { j: [] for j in self.actuated_joints } n_cfgs = None @@ -1420,7 +1460,7 @@ def _process_cfgs( @classmethod def _from_xml( - cls, node: ET._Element, path: str, lazy_load_meshes: bool | None = None + 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", "")) @@ -1446,7 +1486,7 @@ def _from_xml( other_xml=other_xml, ) - def _to_xml(self, parent: ET._Element | None, path: str) -> ET._Element: + 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 63d4025..36be7c1 100644 --- a/urchin/utils.py +++ b/urchin/utils.py @@ -3,7 +3,7 @@ from __future__ import annotations import os -from typing import Sequence +from typing import Sequence, Union import numpy as np import numpy.typing as npt @@ -254,7 +254,7 @@ def load_meshes(filename: str) -> list[trimesh.Trimesh]: def configure_origin( - value: None | Sequence[float] | npt.ArrayLike, + value: Union[None, Sequence[float], npt.ArrayLike], ) -> npt.NDArray[np.float64]: """Convert a value into a 4x4 transform matrix. From 9d2ff2e9210982425f3fa087f58d85bdfc6e1cb3 Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Wed, 8 Oct 2025 09:28:39 -0700 Subject: [PATCH 11/13] Fixed ruff --- docs/source/conf.py | 2 +- urchin/base.py | 13 +++--------- urchin/joint.py | 8 ++----- urchin/link.py | 48 +++++++++++------------------------------- urchin/material.py | 16 ++++---------- urchin/transmission.py | 12 +++-------- urchin/urdf.py | 4 +--- 7 files changed, 26 insertions(+), 77 deletions(-) diff --git a/docs/source/conf.py b/docs/source/conf.py index 856896d..e103309 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -136,7 +136,7 @@ # 'preamble': '', # Latex figure (float) alignment # -# 'figure_align': 'htbp', + # 'figure_align': 'htbp', } # Grouping the document tree into LaTeX files. List of tuples diff --git a/urchin/base.py b/urchin/base.py index 5ccf768..8b8a3b1 100644 --- a/urchin/base.py +++ b/urchin/base.py @@ -145,10 +145,7 @@ def _parse_simple_elements( 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 - ] + value = [element_type._from_xml(child, path) for child in element_nodes] kwargs[element_name] = value return kwargs @@ -350,9 +347,7 @@ def _parse_simple_elements( 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__ - ) + "parsing an object of type {}".format(element_type.__name__, cls.__name__) ) if issubclass(element_type, URDFTypeWithMesh): value = [ @@ -360,9 +355,7 @@ def _parse_simple_elements( for child in element_nodes ] else: - value = [ - element_type._from_xml(child, path) for child in element_nodes - ] + value = [element_type._from_xml(child, path) for child in element_nodes] kwargs[element_name] = value return kwargs diff --git a/urchin/joint.py b/urchin/joint.py index 820d358..af4efeb 100644 --- a/urchin/joint.py +++ b/urchin/joint.py @@ -677,9 +677,7 @@ def is_valid(self, cfg: Union[float, npt.ArrayLike]) -> bool: 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: + 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. @@ -792,9 +790,7 @@ def get_child_poses(self, cfg: Optional[npt.ArrayLike], n_cfgs: int) -> np.ndarr raise ValueError("Invalid configuration") @classmethod - def _from_xml( - cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None - ): + 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"] diff --git a/urchin/link.py b/urchin/link.py index 6c8c93c..e1c6dec 100644 --- a/urchin/link.py +++ b/urchin/link.py @@ -55,9 +55,7 @@ def meshes(self) -> list[trimesh.Trimesh]: self._meshes = [trimesh.creation.box(extents=self.size)] return self._meshes - def copy( - self, prefix: str = "", scale: Union[float, Sequence[float], None] = None - ) -> "Box": + 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 @@ -212,9 +210,7 @@ def meshes(self) -> list[trimesh.Trimesh]: self._meshes = [trimesh.creation.icosphere(radius=self.radius)] return self._meshes - def copy( - self, prefix: str = "", scale: Union[float, Sequence[float], None] = None - ) -> "Sphere": + 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 @@ -276,9 +272,7 @@ def __init__( filename: str, combine: bool, scale: Optional[npt.ArrayLike] = None, - meshes: Union[ - list[trimesh.Trimesh], trimesh.Trimesh, str, None - ] = None, + meshes: Union[list[trimesh.Trimesh], trimesh.Trimesh, str, None] = None, lazy_filename: Optional[str] = None, ): if meshes is None: @@ -358,9 +352,7 @@ def _load_and_combine_meshes(cls, fn: str, combine: bool) -> list[trimesh.Trimes return meshes @classmethod - def _from_xml( - cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None - ): + 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") @@ -391,9 +383,7 @@ def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: # 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] - ] + 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": @@ -406,9 +396,7 @@ def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: node = self._unparse(path) return node - def copy( - self, prefix: str = "", scale: Union[float, Sequence[float], None] = None - ) -> "Mesh": + 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 @@ -599,9 +587,7 @@ class Collision(URDFTypeWithMesh): } _TAG = "collision" - def __init__( - self, name: Optional[str], origin: Optional[npt.ArrayLike], geometry: Geometry - ): + def __init__(self, name: Optional[str], origin: Optional[npt.ArrayLike], geometry: Geometry): self.geometry = geometry self.name = name self.origin = origin @@ -638,9 +624,7 @@ 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 - ): + 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: @@ -766,9 +750,7 @@ def material(self, value: Optional[Material]) -> None: self._material = value @classmethod - def _from_xml( - cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None - ): + 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") @@ -784,9 +766,7 @@ def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: node.append(unparse_origin(self.origin)) return node - def copy( - self, prefix: str = "", scale: Union[float, Sequence[float], None] = None - ) -> "Visual": + 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 @@ -834,9 +814,7 @@ class Inertial(URDFType): _TAG = "inertial" - def __init__( - self, mass: float, inertia: npt.ArrayLike, origin: Optional[npt.ArrayLike] = None - ): + def __init__(self, mass: float, inertia: npt.ArrayLike, origin: Optional[npt.ArrayLike] = None): self.mass = mass self.inertia = inertia self.origin = origin @@ -872,9 +850,7 @@ 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 - ): + 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") diff --git a/urchin/material.py b/urchin/material.py index ad4d517..3342e0f 100644 --- a/urchin/material.py +++ b/urchin/material.py @@ -64,9 +64,7 @@ def image(self, value: Union[PIL.Image.Image, str, np.ndarray]) -> None: self._image = value @classmethod - def _from_xml( - cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None - ): + 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) @@ -80,9 +78,7 @@ def _to_xml(self, parent: Optional[ET._Element], path: str) -> ET._Element: return self._unparse(path) - def copy( - self, prefix: str = "", scale: Union[float, np.ndarray, None] = None - ) -> "Texture": + 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 @@ -167,9 +163,7 @@ def texture(self, value: Union[Texture, str, None]) -> None: self._texture = value @classmethod - def _from_xml( - cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None - ): + 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") @@ -199,9 +193,7 @@ def _to_xml(self, parent: ET._Element, path: str) -> ET._Element: node.append(color) return node - def copy( - self, prefix: str = "", scale: Union[float, np.ndarray, None] = None - ) -> "Material": + 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 diff --git a/urchin/transmission.py b/urchin/transmission.py index aaa0225..b13f66c 100644 --- a/urchin/transmission.py +++ b/urchin/transmission.py @@ -71,9 +71,7 @@ def hardwareInterfaces(self, value: Optional[Sequence[str]]) -> None: self._hardwareInterfaces = value @classmethod - def _from_xml( - cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None - ): + 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 @@ -159,9 +157,7 @@ def hardwareInterfaces(self, value: Optional[Sequence[str]]) -> None: self._hardwareInterfaces = value @classmethod - def _from_xml( - cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None - ): + 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] @@ -290,9 +286,7 @@ def actuators(self, value: Optional[Sequence[Actuator]]) -> None: self._actuators = value @classmethod - def _from_xml( - cls, node: ET._Element, path: str, lazy_load_meshes: Optional[bool] = None - ): + 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: diff --git a/urchin/urdf.py b/urchin/urdf.py index 68c975c..172e742 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -1403,9 +1403,7 @@ def _process_cfgs( joint_cfg: dict[ Joint, Union[list[float], npt.NDArray[np.float64], None], - ] = { - j: [] for j in self.actuated_joints - } + ] = {j: [] for j in self.actuated_joints} n_cfgs = None if isinstance(cfgs, dict): for joint in cfgs: From 503ec99ee798a32e9db8474eba4236a104fe462e Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Wed, 8 Oct 2025 09:33:49 -0700 Subject: [PATCH 12/13] Fixed mypy --- urchin/link.py | 2 +- urchin/urdf.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/urchin/link.py b/urchin/link.py index e1c6dec..232ec61 100644 --- a/urchin/link.py +++ b/urchin/link.py @@ -1072,7 +1072,7 @@ def copy( 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) + inertial = Inertial(mass=float(cm.mass), inertia=cm.moment_inertia, origin=cmm) visuals = None if not collision_only: diff --git a/urchin/urdf.py b/urchin/urdf.py index 172e742..a7625c7 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -290,7 +290,7 @@ def cfg_to_vector( 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) -> Link: From 34206d35c5e8ee722c9c93cec787ab9c2bf4fec4 Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Tue, 21 Oct 2025 09:47:45 -0700 Subject: [PATCH 13/13] Pinned ruff and mypy at newer versions --- .github/workflows/ci.yml | 3 +-- .pre-commit-config.yaml | 52 ++++++++++++++++++++-------------------- 2 files changed, 27 insertions(+), 28 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 6d75cb1..85f8593 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -27,7 +27,7 @@ jobs: python -m pip install --upgrade pip pip install -r requirements.txt pip install .[dev] - pip install mypy ruff + pip install mypy==1.18.2 ruff==0.14.1 - name: Ruff Lint run: | @@ -41,4 +41,3 @@ jobs: - name: Pytest run: | pytest -q - diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1f21f23..f15b043 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,27 +1,27 @@ repos: -- 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.6.5 - 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.7.1 - 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 + - 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