diff --git a/src/rod/builder/primitive_builder.py b/src/rod/builder/primitive_builder.py index eb6e8e4..3ef60c9 100644 --- a/src/rod/builder/primitive_builder.py +++ b/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( diff --git a/src/rod/builder/primitives.py b/src/rod/builder/primitives.py index 014afc6..8ee7883 100644 --- a/src/rod/builder/primitives.py +++ b/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()) + ) diff --git a/src/rod/sdf/link.py b/src/rod/sdf/link.py index e17c99b..dc4b3f4 100644 --- a/src/rod/sdf/link.py +++ b/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( [ diff --git a/tests/test_meshbuilder.py b/tests/test_meshbuilder.py index f5ea8cc..9e39fde 100644 --- a/tests/test_meshbuilder.py +++ b/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]), )