jaxsim 0.4.3.dev364__py3-none-any.whl → 0.4.3.dev365__py3-none-any.whl

jaxsim/_version.py

@@ -12,5 +12,5 @@ __version__: str
 __version_tuple__: VERSION_TUPLE
 version_tuple: VERSION_TUPLE
 
-__version__ = version = '0.4.3.dev364'
-__version_tuple__ = version_tuple = (0, 4, 3, 'dev364')
+__version__ = version = '0.4.3.dev365'
+__version_tuple__ = version_tuple = (0, 4, 3, 'dev365')

jaxsim/parsers/descriptions/__init__.py

@@ -1,4 +1,10 @@
-from .collision import BoxCollision, CollidablePoint, CollisionShape, SphereCollision
+from .collision import (
+    BoxCollision,
+    CollidablePoint,
+    CollisionShape,
+    MeshCollision,
+    SphereCollision,
+)
 from .joint import JointDescription, JointGenericAxis, JointType
 from .link import LinkDescription
 from .model import ModelDescription

jaxsim/parsers/descriptions/collision.py

@@ -154,3 +154,22 @@ class SphereCollision(CollisionShape):
             return False
 
         return hash(self) == hash(other)
+
+
+@dataclasses.dataclass
+class MeshCollision(CollisionShape):
+    center: jtp.VectorLike
+
+    def __hash__(self) -> int:
+        return hash(
+            (
+                hash(tuple(self.center.tolist())),
+                hash(self.collidable_points),
+            )
+        )
+
+    def __eq__(self, other: MeshCollision) -> bool:
+        if not isinstance(other, MeshCollision):
+            return False
+
+        return hash(self) == hash(other)

jaxsim/parsers/rod/meshes.py

@@ -0,0 +1,104 @@
+import numpy as np
+import trimesh
+
+VALID_AXIS = {"x": 0, "y": 1, "z": 2}
+
+
+def extract_points_vertices(mesh: trimesh.Trimesh) -> np.ndarray:
+    """
+    Extracts the vertices of a mesh as points.
+    """
+    return mesh.vertices
+
+
+def extract_points_random_surface_sampling(mesh: trimesh.Trimesh, n) -> np.ndarray:
+    """
+    Extracts N random points from the surface of a mesh.
+
+    Args:
+        mesh: The mesh from which to extract points.
+        n: The number of points to extract.
+
+    Returns:
+        The extracted points (N x 3 array).
+    """
+
+    return mesh.sample(n)
+
+
+def extract_points_uniform_surface_sampling(
+    mesh: trimesh.Trimesh, n: int
+) -> np.ndarray:
+    """
+    Extracts N uniformly sampled points from the surface of a mesh.
+
+    Args:
+        mesh: The mesh from which to extract points.
+        n: The number of points to extract.
+
+    Returns:
+        The extracted points (N x 3 array).
+    """
+
+    return trimesh.sample.sample_surface_even(mesh=mesh, count=n)[0]
+
+
+def extract_points_select_points_over_axis(
+    mesh: trimesh.Trimesh, axis: str, direction: str, n: int
+) -> np.ndarray:
+    """
+    Extracts N points from a mesh along a specified axis. The points are selected based on their position along the axis.
+
+    Args:
+        mesh: The mesh from which to extract points.
+        axis: The axis along which to extract points.
+        direction: The direction along the axis from which to extract points. Valid values are "higher" and "lower".
+        n: The number of points to extract.
+
+    Returns:
+        The extracted points (N x 3 array).
+    """
+
+    dirs = {"higher": np.s_[-n:], "lower": np.s_[:n]}
+    arr = mesh.vertices
+
+    # Sort rows lexicographically first, then columnar.
+    arr.sort(axis=0)
+    sorted_arr = arr[dirs[direction]]
+    return sorted_arr
+
+
+def extract_points_aap(
+    mesh: trimesh.Trimesh,
+    axis: str,
+    upper: float | None = None,
+    lower: float | None = None,
+) -> np.ndarray:
+    """
+    Extracts points from a mesh along a specified axis within a specified range. The points are selected based on their position along the axis.
+
+    Args:
+        mesh: The mesh from which to extract points.
+        axis: The axis along which to extract points.
+        upper: The upper bound of the range.
+        lower: The lower bound of the range.
+
+    Returns:
+        The extracted points (N x 3 array).
+
+    Raises:
+        AssertionError: If the lower bound is greater than the upper bound.
+    """
+
+    # Check bounds.
+    upper = upper if upper is not None else np.inf
+    lower = lower if lower is not None else -np.inf
+    assert lower < upper, "Invalid bounds for axis-aligned plane"
+
+    # Logic.
+    points = mesh.vertices[
+        (mesh.vertices[:, VALID_AXIS[axis]] >= lower)
+        & (mesh.vertices[:, VALID_AXIS[axis]] <= upper)
+    ]
+
+    return points

jaxsim/parsers/rod/parser.py

@@ -334,6 +334,18 @@ def extract_model_data(
 
                 collisions.append(sphere_collision)
 
+            if collision.geometry.mesh is not None and int(
+                os.environ.get("JAXSIM_COLLISION_MESH_ENABLED", "0")
+            ):
+                logging.warning("Mesh collision support is still experimental.")
+                mesh_collision = utils.create_mesh_collision(
+                    collision=collision,
+                    link_description=links_dict[link.name],
+                    method=utils.meshes.extract_points_vertices,
+                )
+
+                collisions.append(mesh_collision)
+
     return SDFData(
         model_name=sdf_model.name,
         link_descriptions=links,

jaxsim/parsers/rod/utils.py

@@ -1,12 +1,21 @@
 import os
+import pathlib
+from collections.abc import Callable
+from typing import TypeVar
 
 import numpy as np
 import numpy.typing as npt
 import rod
+import trimesh
+from rod.utils.resolve_uris import resolve_local_uri
 
 import jaxsim.typing as jtp
+from jaxsim import logging
 from jaxsim.math import Adjoint, Inertia
 from jaxsim.parsers import descriptions
+from jaxsim.parsers.rod import meshes
+
+MeshMappingMethod = TypeVar("MeshMappingMethod", bound=Callable[..., npt.NDArray])
 
 
 def from_sdf_inertial(inertial: rod.Inertial) -> jtp.Matrix:
@@ -202,3 +211,47 @@ def create_sphere_collision(
     return descriptions.SphereCollision(
         collidable_points=collidable_points, center=center_wrt_link
     )
+
+
+def create_mesh_collision(
+    collision: rod.Collision,
+    link_description: descriptions.LinkDescription,
+    method: MeshMappingMethod = None,
+) -> descriptions.MeshCollision:
+
+    file = pathlib.Path(resolve_local_uri(uri=collision.geometry.mesh.uri))
+    _file_type = file.suffix.replace(".", "")
+    mesh = trimesh.load_mesh(file, file_type=_file_type)
+
+    if mesh.is_empty:
+        raise RuntimeError(f"Failed to process '{file}' with trimesh")
+
+    mesh.apply_scale(collision.geometry.mesh.scale)
+    logging.info(
+        msg=f"Loading mesh {collision.geometry.mesh.uri} with scale {collision.geometry.mesh.scale}, file type '{_file_type}'"
+    )
+
+    if method is None:
+        method = meshes.VertexExtraction()
+        logging.debug("Using default Vertex Extraction method for mesh wrapping")
+    else:
+        logging.debug(f"Using method {method} for mesh wrapping")
+
+    points = method(mesh=mesh)
+    logging.debug(f"Extracted {len(points)} points from mesh")
+
+    W_H_L = collision.pose.transform() if collision.pose is not None else np.eye(4)
+
+    # Extract translation from transformation matrix
+    W_p_L = W_H_L[:3, 3]
+    mesh_points_wrt_link = points @ W_H_L[:3, :3].T + W_p_L
+    collidable_points = [
+        descriptions.CollidablePoint(
+            parent_link=link_description,
+            position=point,
+            enabled=True,
+        )
+        for point in mesh_points_wrt_link
+    ]
+
+    return descriptions.MeshCollision(collidable_points=collidable_points, center=W_p_L)

{jaxsim-0.4.3.dev364.dist-info → jaxsim-0.4.3.dev365.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: jaxsim
-Version: 0.4.3.dev364
+Version: 0.4.3.dev365
 Summary: A differentiable physics engine and multibody dynamics library for control and robot learning.
 Author-email: Diego Ferigo <dgferigo@gmail.com>, Filippo Luca Ferretti <filippoluca.ferretti@outlook.com>
 Maintainer-email: Filippo Luca Ferretti <filippo.ferretti@iit.it>, Alessandro Croci <alessandro.croci@iit.it>
@@ -69,6 +69,7 @@ Requires-Dist: pptree
 Requires-Dist: optax>=0.2.3
 Requires-Dist: qpax
 Requires-Dist: rod>=0.3.3
+Requires-Dist: trimesh
 Requires-Dist: typing-extensions; python_version < "3.12"
 Provides-Extra: all
 Requires-Dist: jaxsim[style,testing,viz]; extra == "all"

{jaxsim-0.4.3.dev364.dist-info → jaxsim-0.4.3.dev365.dist-info}/RECORD

@@ -1,5 +1,5 @@
 jaxsim/__init__.py,sha256=opgtbhhd1kDsHI4H1vOd3loMPDRi884yQ3tohfFGfNc,3382
-jaxsim/_version.py,sha256=oVKctISvwW5BE5qS8_uvJRJEQ5goquTFM3lihueDyJ0,428
+jaxsim/_version.py,sha256=MAn1zPonARBalCehiaTUL7_k9OAe5L1w_cFS6kmRIWI,428
 jaxsim/exceptions.py,sha256=vSoScaRD4nvh6jltgK9Ry5pKnE0O5hb4_yI_pk_fvR8,2175
 jaxsim/logging.py,sha256=STI-D_upXZYX-ZezLrlJJ0UlD5YspST0vZ_DcIwkzO4,1553
 jaxsim/typing.py,sha256=2HXy9hgazPXjofi1vLQ09ZubPtgVmg80U9NKmZ6NYiI,761
@@ -37,14 +37,15 @@ jaxsim/mujoco/utils.py,sha256=vZ8afASNOSxnxVW9p_1U1J_n-9nVhnBDqlV5k8c1GkM,8256
 jaxsim/mujoco/visualizer.py,sha256=nD6SNWmn-nxjjjIY9oPAHvL2j8q93DJDjZeepzke_DQ,6988
 jaxsim/parsers/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
 jaxsim/parsers/kinematic_graph.py,sha256=MJkJ7AW1TdLZmxibuiVrTfn6jHjh3OVhEF20DqwsCnM,34748
-jaxsim/parsers/descriptions/__init__.py,sha256=PbIlunVfb59pB5jSX97YVpMAANRZPRkJ0X-hS14rzv4,221
-jaxsim/parsers/descriptions/collision.py,sha256=BQeIG-TKi4SVny23w6riDrQ5itC6VRwEMBX6HgAXHxA,3973
+jaxsim/parsers/descriptions/__init__.py,sha256=N_hp8cGI5FyEFiuNx9a-CAGCr0F0QpYEpdMHvwB7_1g,261
+jaxsim/parsers/descriptions/collision.py,sha256=mLcwj61of16MVJf64I95E5SiQZST3uyTEfS0ImhGBSI,4419
 jaxsim/parsers/descriptions/joint.py,sha256=2KWLP4ILPMV8q1X0J7aS3GGFeZn4zXan0dqGOWc7XuQ,4365
 jaxsim/parsers/descriptions/link.py,sha256=Eh0W5qL7_Uw0GV-BkNKXhm9Q2dRTfIWCX5D-87zQkxA,3711
 jaxsim/parsers/descriptions/model.py,sha256=I2Vsbv8Josl4Le7b5rIvhqA2k9Bbv5JxMqwytayxds0,9833
 jaxsim/parsers/rod/__init__.py,sha256=G2vqlLajBLUc4gyzXwsEI2Wsi4TMOIF9bLDFeT6KrGU,92
-jaxsim/parsers/rod/parser.py,sha256=EXcbtr_vMjAaUzQjfQlD1zLLYLAZXrNeFHaiZVlLwFI,13976
-jaxsim/parsers/rod/utils.py,sha256=czQ2Y1_I9zGO0y2XDotHSqDorVH6zEcPhkuelApjs3k,5697
+jaxsim/parsers/rod/meshes.py,sha256=a5qUFkHe3gPXMKRnvPx3BDvlEHT2vEJqcLYLXhJFCdA,2847
+jaxsim/parsers/rod/parser.py,sha256=OJdKM2hVyED9nw7iRpxCRFZx9Z6rKmVflhkItGTzRns,14499
+jaxsim/parsers/rod/utils.py,sha256=-bVIbO8AOnImkVbf8XA3XOweoTny7m85ACLkR0d_p2E,7505
 jaxsim/rbda/__init__.py,sha256=kmy4G9aMkrqPNGdLSaSV3k15dpF52vBEUQXDFDuKIxU,337
 jaxsim/rbda/aba.py,sha256=w7ciyxB0IsmueatT0C7PcBQEl9dyiH9oqJgIi3xeTUE,8983
 jaxsim/rbda/collidable_points.py,sha256=0PFLzxWKtRg8-JtfNhGlSjBMv1J98tiLymOdvlvAak4,5325
@@ -65,8 +66,8 @@ jaxsim/utils/__init__.py,sha256=Y5zyoRevl3EMVQadhZ4EtSwTEkDt2vcnFoRhPJjKTZ0,215
 jaxsim/utils/jaxsim_dataclass.py,sha256=TGmTQV2Lq7Q-2nLoAEaeNtkPa_qj0IKkdBm4COj46Os,11312
 jaxsim/utils/tracing.py,sha256=eEY28MZW0Lm_jJNt1NkFqZz0ek01tvhR46OXZYCo7tc,532
 jaxsim/utils/wrappers.py,sha256=ZY7olSORzZRvSzkdeNLj8yjwUIAt9L0Douwl7wItjpk,4008
-jaxsim-0.4.3.dev364.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
-jaxsim-0.4.3.dev364.dist-info/METADATA,sha256=l0SzDTLIIyjVPsCNDnZQ3UX-L50mUBLD_VfkU0lflSE,17513
-jaxsim-0.4.3.dev364.dist-info/WHEEL,sha256=R06PA3UVYHThwHvxuRWMqaGcr-PuniXahwjmQRFMEkY,91
-jaxsim-0.4.3.dev364.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
-jaxsim-0.4.3.dev364.dist-info/RECORD,,
+jaxsim-0.4.3.dev365.dist-info/LICENSE,sha256=eaYdFmdeMbiIoIiPzEK0MjP1S9wtFXjXNR5er49uLR0,1546
+jaxsim-0.4.3.dev365.dist-info/METADATA,sha256=_oydr6ibnTCcmb-aQwfcEFX1ZlVSnnj7N-rUzv2-C50,17536
+jaxsim-0.4.3.dev365.dist-info/WHEEL,sha256=R06PA3UVYHThwHvxuRWMqaGcr-PuniXahwjmQRFMEkY,91
+jaxsim-0.4.3.dev365.dist-info/top_level.txt,sha256=LxGMA8FLtXjQ6oI7N5gd_R_oSUHxpXxUEOfT1xS_ni0,7
+jaxsim-0.4.3.dev365.dist-info/RECORD,,