Merge pull request #38 from ami-iit/update_mesh_builder

Update `MeshBuilder` class

src/rod/builder/primitive_builder.py

@@ -12,8 +12,9 @@
 
 @dataclasses.dataclass
 class PrimitiveBuilder(abc.ABC):
-    name: str
-    mass: float
+
+    name: str = dataclasses.field(kw_only=True)
+    mass: float = dataclasses.field(kw_only=True)
 
     element: rod.Model | rod.Link | rod.Inertial | rod.Collision | rod.Visual = (
         dataclasses.field(

src/rod/builder/primitives.py

@@ -1,10 +1,13 @@
 import dataclasses
 import pathlib
 
+import numpy as np
+import numpy.typing as npt
+import resolve_robotics_uri_py
 import trimesh
-from numpy.typing import NDArray
 
 import rod
+from rod import logging
 from rod.builder.primitive_builder import PrimitiveBuilder
 
 
@@ -62,8 +65,13 @@ def _geometry(self) -> rod.Geometry:
 
 @dataclasses.dataclass
 class MeshBuilder(PrimitiveBuilder):
-    mesh_path: str | pathlib.Path
-    scale: NDArray
+
+    mesh_uri: str | pathlib.Path
+
+    scale: npt.NDArray = dataclasses.field(default_factory=lambda: np.ones(3))
+
+    mass: float | None = dataclasses.field(default=None, kw_only=True)
+    inertia_tensor: npt.NDArray | None = dataclasses.field(default=None, kw_only=True)
 
     def __post_init__(self) -> None:
         """
@@ -74,34 +82,46 @@ def __post_init__(self) -> None:
             AssertionError: If the scale is not a 3D vector.
             TypeError: If the mesh_path is not a str or pathlib.Path.
         """
+        # Adjust the shape of the scale.
+        self.scale = self.scale.squeeze()
 
-        mesh_path = (
-            self.mesh_path
-            if isinstance(self.mesh_path, pathlib.Path)
-            else pathlib.Path(self.mesh_path)
-        )
+        if self.scale.shape != (3,):
+            raise RuntimeError(f"Scale must be a 3D vector, got '{self.scale.shape}'")
 
-        if not mesh_path.is_file():
-            raise FileNotFoundError(f"Mesh file not found at {self.mesh_path}")
+        # Resolve the mesh URI.
+        mesh_path = resolve_robotics_uri_py.resolve_robotics_uri(uri=str(self.mesh_uri))
 
-        self.mesh: trimesh.base.Trimesh = trimesh.load_mesh(
-            file_obj=mesh_path,
-        )
+        # Build the trimesh object from the mesh path.
+        self.mesh: trimesh.base.Trimesh = trimesh.load_mesh(file_obj=mesh_path)
+
+        # Populate the mass from the mesh if it was not provided externally.
+        if self.mass is None:
+
+            if self.mesh.is_watertight:
+                self.mass = self.mesh.mass
+
+            else:
+                msg = "Mesh '{}' is not watertight. Using a dummy mass value."
+                logging.warning(msg.format(self.mesh_uri))
+                self.mass = 1.0
 
-        assert self.scale.shape == (
-            3,
-        ), f"Scale must be a 3D vector, got {self.scale.shape}"
+        # Populate the inertia tensor from the mesh if it was not provided externally.
+        if self.inertia_tensor is None:
+
+            if self.mesh.is_watertight:
+                self.inertia_tensor = self.mesh.moment_inertia
+
+            else:
+                msg = "Mesh '{}' is not watertight. Using a dummy inertia tensor."
+                logging.warning(msg.format(self.mesh_uri))
+                self.inertia_tensor = np.eye(3)
 
     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],
-        )
+
+        return rod.Inertia.from_inertia_tensor(inertia_tensor=self.inertia_tensor)
 
     def _geometry(self) -> rod.Geometry:
-        return rod.Geometry(mesh=rod.Mesh(uri=str(self.mesh_path), scale=self.scale))
+
+        return rod.Geometry(
+            mesh=rod.Mesh(uri=str(self.mesh_uri), scale=self.scale.tolist())
+        )

src/rod/sdf/link.py

@@ -1,9 +1,13 @@
+from __future__ import annotations
+
 import dataclasses
 
 import mashumaro
 import numpy as np
 import numpy.typing as npt
 
+from rod import logging
+
 from .collision import Collision
 from .common import Pose
 from .element import Element
@@ -12,6 +16,7 @@
 
 @dataclasses.dataclass
 class Inertia(Element):
+
     ixx: float = dataclasses.field(
         default=1.0,
         metadata=mashumaro.field_options(serialize=Element.serialize_float),
@@ -42,6 +47,42 @@ class Inertia(Element):
         metadata=mashumaro.field_options(serialize=Element.serialize_float),
     )
 
+    @staticmethod
+    def from_inertia_tensor(
+        inertia_tensor: npt.NDArray, validate: bool = True
+    ) -> Inertia:
+
+        inertia_tensor = inertia_tensor.squeeze()
+
+        if inertia_tensor.shape != (3, 3):
+            raise ValueError(f"Expected shape (3, 3), got {inertia_tensor.shape}")
+
+        # Extract the diagonal terms.
+        I1, I2, I3 = np.diag(inertia_tensor)
+
+        # Check if the inertia tensor meets the triangular inequality.
+        valid = True
+        valid = valid and I1 + I2 >= I3
+        valid = valid and I1 + I3 >= I2
+        valid = valid and I2 + I3 >= I1
+
+        if not valid:
+            msg = "Inertia tensor does not meet the triangular inequality"
+
+            if not validate:
+                logging.warning(msg)
+            else:
+                raise ValueError(msg)
+
+        return Inertia(
+            ixx=float(inertia_tensor[0, 0]),
+            ixy=float(inertia_tensor[0, 1]),
+            ixz=float(inertia_tensor[0, 2]),
+            iyy=float(inertia_tensor[1, 1]),
+            iyz=float(inertia_tensor[1, 2]),
+            izz=float(inertia_tensor[2, 2]),
+        )
+
     def matrix(self) -> npt.NDArray:
         return np.array(
             [

tests/test_meshbuilder.py

@@ -1,6 +1,7 @@
 import tempfile
 
 import numpy as np
+import pytest
 import trimesh
 
 from rod.builder.primitives import MeshBuilder
@@ -20,11 +21,18 @@ def test_builder_creation():
 
             builder = MeshBuilder(
                 name="test_mesh",
-                mesh_path=fp.name,
+                mesh_uri=fp.name,
                 mass=1.0,
                 scale=np.array([1.0, 1.0, 1.0]),
             )
 
+    # Check that the builder can build a correct link.
+    # Note that the URI is not valid since it's a temporary file.
+    link = builder.build_link().add_inertial().add_visual().add_collision().build()
+    assert link.collision is not None
+    assert link.collision.geometry.mesh is not None
+    assert link.collision.geometry.mesh.scale == pytest.approx([1, 1, 1])
+
     assert (
         builder.mesh.vertices.shape == mesh.vertices.shape
     ), f"{builder.mesh.vertices.shape} != {mesh.vertices.shape}"
@@ -56,7 +64,7 @@ def test_builder_creation_custom_mesh():
 
             builder = MeshBuilder(
                 name="test_mesh",
-                mesh_path=fp.name,
+                mesh_uri=fp.name,
                 mass=1.0,
                 scale=np.array([1.0, 1.0, 1.0]),
             )