Merge pull request #34 from ami-iit/enhance_sdf_frames

Enhance processing and URDF export of `//frame` elements

Author: diegoferigo

.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'

src/rod/__init__.py

@@ -23,8 +23,16 @@
 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
@@ -43,9 +51,58 @@ def _is_editable():
     return rod_package_dir not in site.getsitepackages()
 
 
-# Initialize the logging verbosity
+# 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

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 @@ def transform(self, name: str) -> npt.NDArray:
 
             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],
+            ]
         )

src/rod/sdf/model.py

@@ -158,13 +158,15 @@ def switch_frame_convention(
         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
 
         switch_frame_convention(
             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)