rod 0.2.dev12__tar.gz → 0.2.1.dev33__tar.gz

{rod-0.2.dev12 → rod-0.2.1.dev33}/.github/workflows/ci_cd.yml

@@ -104,7 +104,11 @@ jobs:
         if: matrix.type == 'apt'
         run: |
           sudo apt-get update
-          sudo apt-get install -y --no-install-recommends gazebo
+          sudo apt-get install lsb-release wget gnupg
+          wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
+          echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
+          sudo apt-get update
+          sudo apt-get install --no-install-recommends libsdformat13 gz-tools2
 
       - name: Install conda dependencies
         if: matrix.type == 'conda'
@@ -122,7 +126,8 @@ jobs:
             pptree \
             idyntree \
             pytest \
-            robot_descriptions
+            robot_descriptions \
+            trimesh
             # pytest-icdiff \  # creates problems on macOS
           mamba install -y gz-sim7 idyntree
 

{rod-0.2.dev12 → rod-0.2.1.dev33}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: rod
-Version: 0.2.dev12
+Version: 0.2.1.dev33
 Summary: The ultimate Python tool for RObot Descriptions processing.
 Home-page: https://github.com/ami-iit/rod
 Author: Diego Ferigo
@@ -38,6 +38,7 @@ Requires-Dist: numpy
 Requires-Dist: packaging
 Requires-Dist: resolve-robotics-uri-py
 Requires-Dist: scipy
+Requires-Dist: trimesh
 Requires-Dist: xmltodict
 Provides-Extra: style
 Requires-Dist: black; extra == "style"

{rod-0.2.dev12 → rod-0.2.1.dev33}/setup.cfg

@@ -58,6 +58,7 @@ install_requires =
 	packaging
 	resolve-robotics-uri-py
 	scipy
+	trimesh
 	xmltodict
 
 [options.extras_require]

rod-0.2.1.dev33/src/rod/__init__.py

@@ -0,0 +1,108 @@
+from . import logging
+from .sdf.collision import Collision
+from .sdf.common import Frame, Pose, Xyz
+from .sdf.geometry import (
+    Box,
+    Capsule,
+    Cylinder,
+    Ellipsoid,
+    Geometry,
+    Heightmap,
+    Mesh,
+    Plane,
+    Sphere,
+)
+from .sdf.joint import Axis, Dynamics, Joint, Limit
+from .sdf.link import Inertia, Inertial, Link
+from .sdf.material import Material
+from .sdf.model import Model
+from .sdf.physics import Physics
+from .sdf.scene import Scene
+from .sdf.sdf import Sdf
+from .sdf.visual import Visual
+from .sdf.world import World
+from .utils.frame_convention import FrameConvention
+
+# ===============================
+# Configure the logging verbosity
+# ===============================
+
+
+def _is_editable():
+    """
+    Check if the rod package is installed in editable mode.
+    """
+
+    import importlib.util
+    import pathlib
+    import site
+
+    # Get the ModuleSpec of rod
+    rod_spec = importlib.util.find_spec(name="rod")
+
+    # This can be None. If it's None, assume non-editable installation.
+    if rod_spec.origin is None:
+        return False
+
+    # Get the folder containing the rod package
+    rod_package_dir = str(pathlib.Path(rod_spec.origin).parent.parent)
+
+    # The installation is editable if the package dir is not in any {site|dist}-packages
+    return rod_package_dir not in site.getsitepackages()
+
+
+# Initialize the logging verbosity depending on the installation mode.
+logging.configure(
+    level=logging.LoggingLevel.DEBUG if _is_editable() else logging.LoggingLevel.WARNING
+)
+
+del _is_editable
+
+# =====================================
+# Check for compatible sdformat version
+# =====================================
+
+
+def check_compatible_sdformat(specification_version: str) -> None:
+    """
+    Check if the installed sdformat version produces SDF files compatible with ROD.
+
+    Args:
+        specification_version: The minimum required SDF specification version.
+
+    Note:
+        This check runs only if sdformat is installed in the system.
+    """
+
+    import os
+
+    import packaging.version
+    import xmltodict
+
+    from rod.utils.gazebo import GazeboHelper
+
+    if os.environ.get("ROD_SKIP_SDFORMAT_CHECK", "0") == "1":
+        return
+
+    if not GazeboHelper.has_gazebo():
+        return
+    else:
+        cmdline = GazeboHelper.get_gazebo_executable()
+        logging.info(f"Calling sdformat through '{cmdline} sdf'")
+
+    output_sdf_version = packaging.version.Version(
+        xmltodict.parse(
+            xml_input=GazeboHelper.process_model_description_with_sdformat(
+                model_description="<sdf version='1.4'/>"
+            )
+        )["sdf"]["@version"]
+    )
+
+    if output_sdf_version < packaging.version.Version(specification_version):
+        msg = "The found sdformat installation only supports the '{}' specification, "
+        msg += "while ROD requires at least the '{}' specification."
+        raise RuntimeError(msg.format(output_sdf_version, specification_version))
+
+
+check_compatible_sdformat(specification_version="1.10")
+del check_compatible_sdformat

rod-0.2.1.dev33/src/rod/builder/primitives.py

@@ -0,0 +1,110 @@
+import dataclasses
+import pathlib
+from typing import Union
+
+import trimesh
+from numpy.typing import NDArray
+
+import rod
+from rod.builder.primitive_builder import PrimitiveBuilder
+
+
+@dataclasses.dataclass
+class SphereBuilder(PrimitiveBuilder):
+    radius: float
+
+    def _inertia(self) -> rod.Inertia:
+        return rod.Inertia(
+            ixx=2 / 5 * self.mass * (self.radius) ** 2,
+            iyy=2 / 5 * self.mass * (self.radius) ** 2,
+            izz=2 / 5 * self.mass * (self.radius) ** 2,
+        )
+
+    def _geometry(self) -> rod.Geometry:
+        return rod.Geometry(sphere=rod.Sphere(radius=self.radius))
+
+
+@dataclasses.dataclass
+class BoxBuilder(PrimitiveBuilder):
+    x: float
+    y: float
+    z: float
+
+    def _inertia(self) -> rod.Inertia:
+        return rod.Inertia(
+            ixx=self.mass / 12 * (self.y**2 + self.z**2),
+            iyy=self.mass / 12 * (self.x**2 + self.z**2),
+            izz=self.mass / 12 * (self.x**2 + self.y**2),
+        )
+
+    def _geometry(self) -> rod.Geometry:
+        return rod.Geometry(box=rod.Box(size=[self.x, self.y, self.z]))
+
+
+@dataclasses.dataclass
+class CylinderBuilder(PrimitiveBuilder):
+    radius: float
+    length: float
+
+    def _inertia(self) -> rod.Inertia:
+        ixx_iyy = self.mass * (3 * self.radius**2 + self.length**2) / 12
+
+        return rod.Inertia(
+            ixx=ixx_iyy,
+            iyy=ixx_iyy,
+            izz=0.5 * self.mass * self.radius**2,
+        )
+
+    def _geometry(self) -> rod.Geometry:
+        return rod.Geometry(
+            cylinder=rod.Cylinder(radius=self.radius, length=self.length)
+        )
+
+
+@dataclasses.dataclass
+class MeshBuilder(PrimitiveBuilder):
+    mesh_path: Union[str, pathlib.Path]
+    scale: NDArray
+
+    def __post_init__(self) -> None:
+        """
+        Post-initialization method for the class.
+        Loads the mesh from the specified file path and performs necessary checks.
+
+        Raises:
+            AssertionError: If the scale is not a 3D vector.
+            TypeError: If the mesh_path is not a str or pathlib.Path.
+        """
+
+        if isinstance(self.mesh_path, str):
+            extension = pathlib.Path(self.mesh_path).suffix
+        elif isinstance(self.mesh_path, pathlib.Path):
+            extension = self.mesh_path.suffix
+        else:
+            raise TypeError(
+                f"Expected str or pathlib.Path for mesh_path, got {type(self.mesh_path)}"
+            )
+
+        self.mesh: trimesh.base.Trimesh = trimesh.load(
+            str(self.mesh_path),
+            force="mesh",
+            file_type=extension,
+        )
+
+        assert self.scale.shape == (
+            3,
+        ), f"Scale must be a 3D vector, got {self.scale.shape}"
+
+    def _inertia(self) -> rod.Inertia:
+        inertia = self.mesh.moment_inertia
+        return rod.Inertia(
+            ixx=inertia[0, 0],
+            ixy=inertia[0, 1],
+            ixz=inertia[0, 2],
+            iyy=inertia[1, 1],
+            iyz=inertia[1, 2],
+            izz=inertia[2, 2],
+        )
+
+    def _geometry(self) -> rod.Geometry:
+        return rod.Geometry(mesh=rod.Mesh(uri=str(self.mesh_path), scale=self.scale))

{rod-0.2.dev12 → rod-0.2.1.dev33}/src/rod/kinematics/tree_transforms.py

@@ -14,18 +14,15 @@ class TreeTransforms:
     kinematic_tree: KinematicTree = dataclasses.dataclass(init=False)
 
     @staticmethod
-    def build(
-        model: "rod.Model",
-        is_top_level: bool = True,
-        prevent_switching_frame_convention: bool = False,
-    ) -> "TreeTransforms":
+    def build(model: "rod.Model", is_top_level: bool = True) -> "TreeTransforms":
+
+        # Operate on a deep copy of the model to avoid side effects.
         model = copy.deepcopy(model)
 
+        # Make sure that all elements have a pose attribute with explicit 'relative_to'.
         model.resolve_frames(is_top_level=is_top_level, explicit_frames=True)
 
-        if not prevent_switching_frame_convention:
-            model.switch_frame_convention(frame_convention=rod.FrameConvention.Urdf)
-
+        # Build the kinematic tree and return the TreeTransforms object.
         return TreeTransforms(
             kinematic_tree=KinematicTree.build(model=model, is_top_level=is_top_level)
         )
@@ -54,30 +51,54 @@ class TreeTransforms:
 
             return W_H_E
 
-        if (
-            name in self.kinematic_tree.link_names()
-            or name in self.kinematic_tree.frame_names()
-        ):
-            element = (
-                self.kinematic_tree.links_dict[name]
-                if name in self.kinematic_tree.link_names()
-                else self.kinematic_tree.frames_dict[name]
-            )
-            assert element.name() == name
+        if name in self.kinematic_tree.link_names():
 
-            # Get the pose of the frame in which the node's pose is expressed
+            element = self.kinematic_tree.links_dict[name]
+
+            assert element.name() == name
             assert element._source.pose.relative_to not in {"", None}
-            x_H_N = element._source.pose.transform()
+
+            # Get the pose of the frame in which the link's pose is expressed.
+            x_H_L = element._source.pose.transform()
             W_H_x = self.transform(name=element._source.pose.relative_to)
 
-            # Compute and cache the world-to-node transform
-            W_H_N = W_H_x @ x_H_N
+            # Compute the world transform of the link.
+            W_H_L = W_H_x @ x_H_L
+            return W_H_L
+
+        if name in self.kinematic_tree.frame_names():
+
+            element = self.kinematic_tree.frames_dict[name]
+
+            assert element.name() == name
+            assert element._source.pose.relative_to not in {"", None}
 
-            return W_H_N
+            # Get the pose of the frame in which the frame's pose is expressed.
+            x_H_F = element._source.pose.transform()
+            W_H_x = self.transform(name=element._source.pose.relative_to)
+
+            # Compute the world transform of the frame.
+            W_H_F = W_H_x @ x_H_F
+            return W_H_F
 
         raise ValueError(name)
 
     def relative_transform(self, relative_to: str, name: str) -> npt.NDArray:
-        return np.linalg.inv(self.transform(name=relative_to)) @ self.transform(
-            name=name
+
+        world_H_name = self.transform(name=name)
+        world_H_relative_to = self.transform(name=relative_to)
+
+        return TreeTransforms.inverse(world_H_relative_to) @ world_H_name
+
+    @staticmethod
+    def inverse(transform: npt.NDArray) -> npt.NDArray:
+
+        R = transform[0:3, 0:3]
+        p = np.vstack(transform[0:3, 3])
+
+        return np.block(
+            [
+                [R.T, -R.T @ p],
+                [0, 0, 0, 1],
+            ]
         )

{rod-0.2.dev12 → rod-0.2.1.dev33}/src/rod/sdf/model.py

@@ -158,6 +158,7 @@ class Model(Element):
         frame_convention: "rod.FrameConvention",
         is_top_level: bool = True,
         explicit_frames: bool = True,
+        attach_frames_to_links: bool = True,
     ) -> None:
         from rod.utils.frame_convention import switch_frame_convention
 
@@ -165,6 +166,7 @@ class Model(Element):
             model=self,
             frame_convention=frame_convention,
             is_top_level=is_top_level,
+            attach_frames_to_links=attach_frames_to_links,
         )
 
         self.resolve_frames(is_top_level=is_top_level, explicit_frames=explicit_frames)