diff --git a/source/isaaclab_newton/config/extension.toml b/source/isaaclab_newton/config/extension.toml index 7f6e09f992a5..5fbc20c99341 100644 --- a/source/isaaclab_newton/config/extension.toml +++ b/source/isaaclab_newton/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.5.10" +version = "0.5.12" # Description title = "Newton simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index 3a7633711b8d..001356c02266 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,46 @@ Changelog --------- +0.5.12 (2026-04-10) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab_newton.physics.SDFCfg` for configuring SDF-based mesh + collisions via Newton's ``mesh.build_sdf()`` API. Supports per-body and per-shape + regex pattern matching, per-pattern resolution overrides, and optional creation of + collision shapes from visual meshes. +* Added hydroelastic shape enablement fields + (:attr:`~isaaclab_newton.physics.SDFCfg.k_hydro`, + :attr:`~isaaclab_newton.physics.SDFCfg.hydroelastic_shape_patterns`) on + :class:`~isaaclab_newton.physics.SDFCfg`. +* Added missing hydroelastic pipeline parameters to + :class:`~isaaclab_newton.physics.HydroelasticSDFCfg`: ``moment_matching``, + ``buffer_mult_broad``, ``buffer_mult_iso``, ``buffer_mult_contact``, ``grid_size``. +* Added SDF pattern skip in the Newton cloner to preserve original triangle + meshes for shapes that will use SDF collision. + + +0.5.11 (2026-04-09) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab_newton.physics.NewtonCollisionPipelineCfg` to expose Newton collision pipeline parameters via + :attr:`~isaaclab_newton.physics.NewtonCfg.collision_cfg`. +* Added :attr:`~isaaclab_newton.physics.MJWarpSolverCfg.tolerance` for solver convergence control. + +Fixed +^^^^^ + +* Fixed truthiness check on hydroelastic config dict in collision pipeline + initialization. An explicit ``is not None`` check is now used so that + :class:`~isaaclab_newton.physics.newton_collision_cfg.HydroelasticSDFCfg` + with all-default values is no longer silently skipped. + + 0.5.10 (2026-04-05) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py index 59e5d2476ccc..0e166eb5afdd 100644 --- a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py +++ b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py @@ -5,20 +5,33 @@ from __future__ import annotations +import re from collections.abc import Callable import torch import warp as wp -from newton import ModelBuilder, solvers +from newton import GeoType, ModelBuilder, solvers from newton._src.usd.schemas import SchemaResolverNewton, SchemaResolverPhysx from pxr import Usd, UsdGeom +from isaaclab.physics import PhysicsManager from isaaclab.physics.scene_data_requirements import VisualizerPrebuiltArtifacts from isaaclab_newton.physics import NewtonManager +def _compile_sdf_patterns(patterns: list[str]) -> list[re.Pattern]: + """Compile regex patterns with validation, raising on invalid regex.""" + compiled = [] + for i, p in enumerate(patterns): + try: + compiled.append(re.compile(p)) + except re.error as e: + raise ValueError(f"Invalid regex in SDFCfg pattern[{i}]: {p!r} — {e}") from e + return compiled + + def _build_newton_builder_from_mapping( stage: Usd.Stage, sources: list[str], @@ -62,6 +75,17 @@ def _build_newton_builder_from_mapping( # The prototype is built from env_0 in absolute world coordinates. # add_builder xforms are deltas from env_0 so positions don't get double-counted. env0_pos = positions[0] + + # SDF collision requires original triangle meshes for mesh.build_sdf(). + # Convex hull approximation destroys the source geometry, so shapes + # matching SDF patterns must be excluded from approximation here. + # _apply_sdf_config() builds the SDF on each prototype after approximation. + cfg = PhysicsManager._cfg + sdf_cfg = cfg.sdf_cfg if cfg is not None else None # type: ignore[union-attr] + body_pats = _compile_sdf_patterns(sdf_cfg.body_patterns) if sdf_cfg and sdf_cfg.body_patterns else None + shape_pats = _compile_sdf_patterns(sdf_cfg.shape_patterns) if sdf_cfg and sdf_cfg.shape_patterns else None + has_sdf_patterns = body_pats is not None or shape_pats is not None + protos: dict[str, ModelBuilder] = {} for src_path in sources: p = ModelBuilder(up_axis=up_axis) @@ -74,7 +98,33 @@ def _build_newton_builder_from_mapping( schema_resolvers=schema_resolvers, ) if simplify_meshes: - p.approximate_meshes("convex_hull", keep_visual_shapes=True) + if has_sdf_patterns: + sdf_bodies: set[int] = set() + if body_pats is not None: + for bi in range(len(p.body_label)): + if any(pat.search(p.body_label[bi]) for pat in body_pats): + sdf_bodies.add(bi) + + approx_indices = [] + for i in range(len(p.shape_type)): + if p.shape_type[i] != GeoType.MESH: + continue + # Skip shapes that will use SDF (matched by body or shape pattern) + if p.shape_body[i] in sdf_bodies: + continue + if shape_pats is not None: + lbl = p.shape_label[i] if i < len(p.shape_label) else "" + if any(pat.search(lbl) for pat in shape_pats): + continue + approx_indices.append(i) + if approx_indices: + p.approximate_meshes("convex_hull", shape_indices=approx_indices, keep_visual_shapes=True) + else: + p.approximate_meshes("convex_hull", keep_visual_shapes=True) + # Build SDF on prototype before add_builder copies it N times. + # Mesh objects are shared by reference, so SDF is built once and + # all environments inherit it. + NewtonManager._apply_sdf_config(p) protos[src_path] = p # create a separate world for each environment (heterogeneous spawning) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi index 4e589e6f69d1..45439af1d6ba 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi +++ b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi @@ -5,12 +5,22 @@ __all__ = [ "FeatherstoneSolverCfg", + "HydroelasticSDFCfg", "MJWarpSolverCfg", "NewtonCfg", + "NewtonCollisionPipelineCfg", "NewtonManager", "NewtonSolverCfg", + "SDFCfg", "XPBDSolverCfg", ] +from .newton_collision_cfg import HydroelasticSDFCfg, NewtonCollisionPipelineCfg, SDFCfg from .newton_manager import NewtonManager -from .newton_manager_cfg import FeatherstoneSolverCfg, MJWarpSolverCfg, NewtonCfg, NewtonSolverCfg, XPBDSolverCfg +from .newton_manager_cfg import ( + FeatherstoneSolverCfg, + MJWarpSolverCfg, + NewtonCfg, + NewtonSolverCfg, + XPBDSolverCfg, +) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py new file mode 100644 index 000000000000..35a0094911ef --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -0,0 +1,334 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for Newton collision pipeline.""" + +from __future__ import annotations + +from typing import Any, Literal + +from isaaclab.utils import configclass + + +@configclass +class HydroelasticSDFCfg: + """Configuration for SDF-based hydroelastic collision handling. + + Hydroelastic contacts generate distributed contact areas instead of point contacts, + providing more realistic force distribution for manipulation and compliant surfaces. + + For more details, see the `Newton Collisions Guide`_. + + .. _Newton Collisions Guide: https://newton-physics.github.io/newton/latest/concepts/collisions.html#hydroelastic-contacts + """ + + reduce_contacts: bool = True + """Whether to reduce contacts to a smaller representative set per shape pair. + + When False, all generated contacts are passed through without reduction. + + Defaults to ``True`` (same as Newton's default). + """ + + buffer_fraction: float = 1.0 + """Fraction of worst-case hydroelastic buffer allocations. Range: (0, 1]. + + Lower values reduce memory usage but may cause overflows in dense scenes. + Overflows are bounds-safe and emit warnings; increase this value when warnings appear. + + Defaults to ``1.0`` (same as Newton's default). + """ + + normal_matching: bool = True + """Whether to rotate reduced contact normals to align with aggregate force direction. + + Only active when ``reduce_contacts`` is True. + + Defaults to ``True`` (same as Newton's default). + """ + + anchor_contact: bool = False + """Whether to add an anchor contact at the center of pressure for each normal bin. + + The anchor contact helps preserve moment balance. Only active when ``reduce_contacts`` is True. + + Defaults to ``False`` (same as Newton's default). + """ + + margin_contact_area: float = 0.01 + """Contact area [m^2] used for non-penetrating contacts at the margin. + + Defaults to ``0.01`` (same as Newton's default). + """ + + output_contact_surface: bool = False + """Whether to output hydroelastic contact surface vertices for visualization. + + Defaults to ``False`` (same as Newton's default). + """ + + moment_matching: bool = False + """Whether to adjust reduced contact friction so net max moment matches unreduced. + + Only active when ``reduce_contacts`` is True. + + Defaults to ``False`` (same as Newton's default). + """ + + buffer_mult_broad: int = 1 + """Multiplier for preallocated broadphase buffer. + + Defaults to ``1`` (same as Newton's default). + """ + + buffer_mult_iso: int = 1 + """Multiplier for iso-surface extraction buffers. + + Defaults to ``1`` (same as Newton's default). + """ + + buffer_mult_contact: int = 1 + """Multiplier for face contact buffer. + + Defaults to ``1`` (same as Newton's default). + """ + + grid_size: int = 262144 + """Grid size for hydroelastic contact handling (256 * 8 * 128). + + Defaults to ``262144`` (same as Newton's default). + """ + + +@configclass +class NewtonCollisionPipelineCfg: + """Configuration for Newton collision pipeline. + + Full-featured collision pipeline with GJK/MPR narrow phase and pluggable broad phase. + When this config is set on :attr:`NewtonCfg.collision_cfg`: + + - **MJWarpSolverCfg**: Newton's collision pipeline replaces MuJoCo's internal contact solver. + - **Other solvers** (XPBD, Featherstone, etc.): Configures the collision pipeline parameters + (these solvers always use Newton's collision pipeline). + + Key features: + + - GJK/MPR algorithms for convex-convex collision detection + - Multiple broad phase options: NXN (all-pairs), SAP (sweep-and-prune), EXPLICIT (precomputed pairs) + - Mesh-mesh collision via SDF with contact reduction + - Optional hydroelastic contact model for compliant surfaces + + For more details, see the `Newton Collisions Guide`_ and `CollisionPipeline API`_. + + .. _Newton Collisions Guide: https://newton-physics.github.io/newton/latest/concepts/collisions.html + .. _CollisionPipeline API: https://newton-physics.github.io/newton/api/_generated/newton.CollisionPipeline.html + """ + + broad_phase: Literal["explicit", "nxn", "sap"] = "explicit" + """Broad phase algorithm for collision detection. + + Options: + + - ``"explicit"``: Use precomputed shape pairs from ``model.shape_contact_pairs``. + - ``"nxn"``: All-pairs brute force. Simple but O(n^2) complexity. + - ``"sap"``: Sweep-and-prune. Good for scenes with many dynamic objects. + + Defaults to ``"explicit"`` (same as Newton's default when ``broad_phase=None``). + """ + + reduce_contacts: bool = True + """Whether to reduce contacts for mesh-mesh collisions. + + When True, uses shared memory contact reduction to select representative contacts. + Improves performance and stability for meshes with many vertices. + + Defaults to ``True`` (same as Newton's default). + """ + + rigid_contact_max: int | None = None + """Maximum number of rigid contacts to allocate. + + Resolution order: + + 1. If provided, use this value. + 2. Else if ``model.rigid_contact_max > 0``, use the model value. + 3. Else estimate automatically from model shape and pair metadata. + + Defaults to ``None`` (auto-estimate, same as Newton's default). + """ + + max_triangle_pairs: int = 1_000_000 + """Maximum number of triangle pairs allocated by narrow phase for mesh and heightfield collisions. + + Increase this when scenes with large/complex meshes or heightfields report + triangle-pair overflow warnings. + + Defaults to ``1_000_000`` (same as Newton's default). + """ + + soft_contact_max: int | None = None + """Maximum number of soft contacts to allocate. + + If None, computed as ``shape_count * particle_count``. + + Defaults to ``None`` (auto-compute, same as Newton's default). + """ + + soft_contact_margin: float = 0.01 + """Margin [m] for soft contact generation. + + Defaults to ``0.01`` (same as Newton's default). + """ + + requires_grad: bool | None = None + """Whether to enable gradient computation for collision. + + If ``None``, uses ``model.requires_grad``. + + Defaults to ``None`` (same as Newton's default). + """ + + sdf_hydroelastic_config: HydroelasticSDFCfg | None = None + """Configuration for SDF-based hydroelastic collision handling. + + If ``None``, hydroelastic contacts are disabled. + If set, enables hydroelastic contacts with the specified parameters. + + Defaults to ``None`` (hydroelastic disabled, same as Newton's default). + """ + + def to_pipeline_args(self) -> dict[str, Any]: + """Build keyword arguments for :class:`newton.CollisionPipeline`. + + Converts this configuration into the dict expected by + ``CollisionPipeline.__init__``, handling nested config conversion + (e.g. :class:`HydroelasticSDFCfg` → ``HydroelasticSDF.Config``). + + Returns: + Keyword arguments suitable for ``CollisionPipeline(model, **args)``. + """ + from newton.geometry import HydroelasticSDF + + cfg_dict = self.to_dict() + hydro_cfg = cfg_dict.pop("sdf_hydroelastic_config", None) + if hydro_cfg is not None: + cfg_dict["sdf_hydroelastic_config"] = HydroelasticSDF.Config(**hydro_cfg) + return cfg_dict + + +@configclass +class SDFCfg: + """Configuration for SDF mesh collision shapes. + + Specifies how SDF (Signed Distance Field) voxel grids are built and assigned + to bodies or shapes in a Newton model. Bodies and shapes are selected by + regex patterns; the SDF resolution can be set globally or overridden + per-pattern. + + Optional hydroelastic stiffness can be assigned to matched SDF shapes. + Pipeline-level hydroelastic parameters (contact reduction, buffer sizes, + etc.) are configured separately via + :attr:`NewtonCollisionPipelineCfg.sdf_hydroelastic_config`. + + Note: + At least one of :attr:`body_patterns` or :attr:`shape_patterns` must be + set. At least one of :attr:`max_resolution` or + :attr:`target_voxel_size` must be set. + """ + + max_resolution: int | None = None + """Maximum voxel dimension for the SDF grid. + + Must be divisible by 8. Typical values: 128, 256, 512. When both this + and :attr:`target_voxel_size` are set, both are forwarded to Newton's + ``mesh.build_sdf()``; Newton uses :attr:`target_voxel_size` to derive + the actual resolution. + + Defaults to ``None``. + """ + + target_voxel_size: float | None = None + """Target voxel size [m] for the SDF grid. + + When both this and :attr:`max_resolution` are set, both are forwarded to + Newton and this value is used to derive the actual resolution. + + Defaults to ``None``. + """ + + narrow_band_range: tuple[float, float] = (-0.1, 0.1) + """Narrow band distance range (inner, outer) [m]. + + Defines the signed-distance extent stored in the SDF voxel grid. + Negative values are inside the mesh, positive values outside. + + Defaults to ``(-0.1, 0.1)``. + """ + + margin: float | None = None + """Collision margin [m] for SDF shapes. + + When ``None``, the Newton builder default is used. + + Defaults to ``None``. + """ + + body_patterns: list[str] | None = None + """Regex patterns for body labels. + + Matched bodies receive SDF collision shapes on all their mesh geometries. + At least one of :attr:`body_patterns` or :attr:`shape_patterns` must be set. + + Defaults to ``None``. + """ + + shape_patterns: list[str] | None = None + """Regex patterns for shape labels. + + Matched shapes receive SDF collision geometry directly. + At least one of :attr:`body_patterns` or :attr:`shape_patterns` must be set. + + Defaults to ``None``. + """ + + pattern_resolutions: dict[str, int] | None = None + """Per-pattern SDF resolution overrides. + + Maps a regex string to a ``max_resolution`` value. Patterns are evaluated + in insertion order; the first match wins. Unmatched bodies or shapes fall + back to the global :attr:`max_resolution` or :attr:`target_voxel_size`. + + Defaults to ``None``. + """ + + use_visual_meshes: bool = False + """Whether to create collision shapes from visual meshes. + + When ``True``, matched bodies that lack explicit collision geometry have SDF + collision shapes built from their visual meshes instead. + + Defaults to ``False``. + """ + + k_hydro: float | None = None + """Hydroelastic stiffness [Pa] assigned to matched SDF shapes. + + When ``None``, no ``HYDROELASTIC`` flag is set and hydroelastic contacts are + disabled for these shapes. When set, matched shapes receive the flag with + this stiffness value. + + Defaults to ``None``. + """ + + hydroelastic_shape_patterns: list[str] | None = None + """Regex patterns restricting which SDF shapes receive hydroelastic stiffness. + + Only relevant when :attr:`k_hydro` is set. When ``None``, all SDF shapes + matched by :attr:`body_patterns` or :attr:`shape_patterns` get the + hydroelastic flag. When set, only shapes whose labels match at least one + pattern here receive it. + + Defaults to ``None``. + """ diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 3e967751d8df..de4d8d52eedf 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -11,6 +11,7 @@ import ctypes import inspect import logging +import re from typing import TYPE_CHECKING import numpy as np @@ -38,6 +39,8 @@ if TYPE_CHECKING: from isaaclab.sim.simulation_context import SimulationContext + from .newton_collision_cfg import NewtonCollisionPipelineCfg + logger = logging.getLogger(__name__) @@ -91,6 +94,7 @@ class NewtonManager(PhysicsManager): _contacts: Contacts | None = None _needs_collision_pipeline: bool = False _collision_pipeline = None + _collision_cfg: NewtonCollisionPipelineCfg | None = None _newton_contact_sensors: dict = {} # Maps sensor_key to NewtonContactSensor _report_contacts: bool = False _fk_dirty: bool = False @@ -365,6 +369,7 @@ def clear(cls): cls._contacts = None cls._needs_collision_pipeline = False cls._collision_pipeline = None + cls._collision_cfg = None cls._newton_contact_sensors = {} cls._report_contacts = False cls._fk_dirty = False @@ -387,6 +392,217 @@ def add_model_change(cls, change: SolverNotifyFlags) -> None: """Register a model change to notify the solver.""" cls._model_changes.add(change) + @staticmethod + def _build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, label: str) -> bool: + """Build SDF on a mesh, resolving per-pattern resolution overrides. + + Args: + mesh: Newton mesh object to build SDF on. + sdf_cfg: The active :class:`SDFCfg` instance. + res_overrides: Compiled ``(pattern, resolution)`` pairs, or ``None``. + label: Shape label used for pattern resolution matching. + + Returns: + ``True`` if SDF was built, ``False`` if skipped (no mesh source). + """ + if mesh is None: + logger.warning(f"SDF: shape '{label}' matched but has no mesh source. Skipping SDF build.") + return False + if mesh.sdf is not None: + mesh.clear_sdf() + resolution = sdf_cfg.max_resolution + if res_overrides is not None: + for pat, res in res_overrides: + if pat.search(label): + resolution = res + break + sdf_kwargs: dict = dict(narrow_band_range=sdf_cfg.narrow_band_range) + if resolution is not None: + sdf_kwargs["max_resolution"] = resolution + if sdf_cfg.target_voxel_size is not None: + sdf_kwargs["target_voxel_size"] = sdf_cfg.target_voxel_size + mesh.build_sdf(**sdf_kwargs) + return True + + @classmethod + def _create_sdf_collision_from_visual( + cls, builder: ModelBuilder, sdf_shape_indices: set[int], sdf_cfg, res_overrides, hydro_patterns=None + ): + """Create collision shapes from visual meshes for matched bodies lacking collision geometry. + + Args: + builder: Newton model builder to modify. + sdf_shape_indices: Shape indices that matched SDF patterns. + sdf_cfg: The active :class:`SDFCfg` instance. + res_overrides: Compiled ``(pattern, resolution)`` pairs, or ``None``. + hydro_patterns: Compiled hydroelastic shape patterns, or ``None`` + (meaning all shapes get hydroelastic if ``k_hydro`` is set). + + Returns: + Tuple of ``(num_added, num_hydro)`` counts. + """ + from newton import ShapeFlags + + matched_bodies: set[int] = {builder.shape_body[si] for si in sdf_shape_indices} + bodies_with_collision: set[int] = set() + for si in range(builder.shape_count): + if builder.shape_flags[si] & ShapeFlags.COLLIDE_SHAPES and builder.shape_body[si] in matched_bodies: + bodies_with_collision.add(builder.shape_body[si]) + + num_added = 0 + num_hydro = 0 + for body_idx in matched_bodies - bodies_with_collision: + visual_si = None + for si in sdf_shape_indices: + if builder.shape_body[si] == body_idx and builder.shape_source[si] is not None: + visual_si = si + break + if visual_si is None: + body_lbl = builder.body_label[body_idx] + logger.warning(f"SDF: body '{body_lbl}' matched but has no visual mesh to create collision from.") + continue + + mesh = builder.shape_source[visual_si] + cls._build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, builder.shape_label[visual_si]) + + # Determine hydroelastic for this shape (respecting pattern filter) + shape_lbl = builder.shape_label[visual_si] + enable_hydro = False + if sdf_cfg.k_hydro is not None: + enable_hydro = hydro_patterns is None or any(p.search(shape_lbl) for p in hydro_patterns) + + shape_cfg_kwargs: dict = dict( + density=0.0, + has_shape_collision=True, + has_particle_collision=True, + is_visible=False, + ) + if sdf_cfg.margin is not None: + shape_cfg_kwargs["margin"] = sdf_cfg.margin + if enable_hydro: + shape_cfg_kwargs["is_hydroelastic"] = True + shape_cfg_kwargs["kh"] = sdf_cfg.k_hydro + + body_lbl = builder.body_label[body_idx] + builder.add_shape_mesh( + body=body_idx, + xform=builder.shape_transform[visual_si], + mesh=mesh, + scale=builder.shape_scale[visual_si], + cfg=ModelBuilder.ShapeConfig(**shape_cfg_kwargs), + label=f"{body_lbl}/sdf_collision", + ) + num_added += 1 + if enable_hydro: + num_hydro += 1 + + return num_added, num_hydro + + @classmethod + def _apply_sdf_config(cls, builder: ModelBuilder): + """Apply SDF collision and optional hydroelastic flags to matching mesh shapes. + + Reads :class:`SDFCfg` from the active physics config. Collects shapes + matching body/shape regex patterns, builds SDF on their meshes, and + optionally sets the ``HYDROELASTIC`` flag with :attr:`SDFCfg.k_hydro`. + + Args: + builder: Newton model builder to modify (before finalization). + """ + from newton import GeoType, ShapeFlags + + cfg = PhysicsManager._cfg + if cfg is None: + return + sdf_cfg = cfg.sdf_cfg # type: ignore[union-attr] + if sdf_cfg is None: + return + + if sdf_cfg.max_resolution is None and sdf_cfg.target_voxel_size is None: + logger.warning("SDFCfg provided but neither max_resolution nor target_voxel_size is set. SDF disabled.") + return + + # Compile patterns (with validation) + def _compile(patterns: list[str] | None, field: str) -> list[re.Pattern] | None: + if not patterns: + return None + compiled = [] + for i, p in enumerate(patterns): + try: + compiled.append(re.compile(p)) + except re.error as e: + raise ValueError(f"Invalid regex in SDFCfg.{field}[{i}]: {p!r} — {e}") from e + return compiled + + body_patterns = _compile(sdf_cfg.body_patterns, "body_patterns") + shape_patterns = _compile(sdf_cfg.shape_patterns, "shape_patterns") + res_overrides = None + if sdf_cfg.pattern_resolutions: + res_overrides = [] + for p, r in sdf_cfg.pattern_resolutions.items(): + try: + res_overrides.append((re.compile(p), r)) + except re.error as e: + raise ValueError(f"Invalid regex in SDFCfg.pattern_resolutions key {p!r} — {e}") from e + hydro_patterns = None + if sdf_cfg.k_hydro is not None: + hydro_patterns = _compile(sdf_cfg.hydroelastic_shape_patterns, "hydroelastic_shape_patterns") + + if body_patterns is None and shape_patterns is None: + logger.warning("SDFCfg has no body_patterns or shape_patterns set. No shapes will receive SDF.") + return + + # Build reverse map: body_idx -> [mesh shape indices] + body_to_shapes: dict[int, list[int]] = {} + for si in range(builder.shape_count): + if builder.shape_type[si] == GeoType.MESH: + body_to_shapes.setdefault(builder.shape_body[si], []).append(si) + + sdf_shape_indices: set[int] = set() + + if body_patterns is not None: + for body_idx in range(len(builder.body_label)): + if any(p.search(builder.body_label[body_idx]) for p in body_patterns): + sdf_shape_indices.update(body_to_shapes.get(body_idx, [])) + + if shape_patterns is not None: + for shape_indices in body_to_shapes.values(): + for si in shape_indices: + if any(p.search(builder.shape_label[si]) for p in shape_patterns): + sdf_shape_indices.add(si) + + # Patch existing collision meshes + num_patched = 0 + num_hydro = 0 + for si in sdf_shape_indices: + if not (builder.shape_flags[si] & ShapeFlags.COLLIDE_SHAPES): + continue + if not cls._build_sdf_on_mesh(builder.shape_source[si], sdf_cfg, res_overrides, builder.shape_label[si]): + continue + if sdf_cfg.margin is not None: + builder.shape_margin[si] = sdf_cfg.margin + if sdf_cfg.k_hydro is not None: + apply_hydro = hydro_patterns is None or any(p.search(builder.shape_label[si]) for p in hydro_patterns) + if apply_hydro: + builder.shape_flags[si] |= ShapeFlags.HYDROELASTIC + builder.shape_material_kh[si] = sdf_cfg.k_hydro + num_hydro += 1 + num_patched += 1 + + # Optionally create collision shapes from visual meshes + num_added = 0 + if sdf_cfg.use_visual_meshes: + num_added, hydro_from_visual = cls._create_sdf_collision_from_visual( + builder, sdf_shape_indices, sdf_cfg, res_overrides, hydro_patterns + ) + num_hydro += hydro_from_visual + + hydro_msg = f", {num_hydro} hydroelastic shape(s)" if sdf_cfg.k_hydro is not None else "" + logger.info( + f"SDF config: {num_added} collision shape(s) added, {num_patched} existing shape(s) patched{hydro_msg}. " + f"(max_resolution={sdf_cfg.max_resolution}, narrow_band={sdf_cfg.narrow_band_range})" + ) + @classmethod def invalidate_fk(cls) -> None: """Mark forward kinematics as needing recomputation. @@ -520,6 +736,7 @@ def instantiate_builder_from_stage(cls): cls._num_envs = len(env_paths) + cls._apply_sdf_config(builder) cls.set_builder(builder) @classmethod @@ -532,7 +749,22 @@ def _initialize_contacts(cls) -> None: if cls._needs_collision_pipeline: # Newton collision pipeline: create pipeline and generate contacts if cls._collision_pipeline is None: - cls._collision_pipeline = CollisionPipeline(cls._model, broad_phase="explicit") + if cls._collision_cfg is not None: + cls._collision_pipeline = CollisionPipeline(cls._model, **cls._collision_cfg.to_pipeline_args()) + else: + cls._collision_pipeline = CollisionPipeline(cls._model, broad_phase="explicit") + + # Warn if hydroelastic was requested but no shapes qualify + hydro_requested = ( + cls._collision_cfg is not None and cls._collision_cfg.sdf_hydroelastic_config is not None + ) + if hydro_requested and cls._collision_pipeline.hydroelastic_sdf is None: + logger.warning( + "HydroelasticSDFCfg was set but no hydroelastic shape pairs found. " + "Ensure shapes have SDF built (via SDFCfg with k_hydro set) and that " + "both shapes in each contact pair have the HYDROELASTIC flag." + ) + if cls._contacts is None: cls._contacts = cls._collision_pipeline.contacts() @@ -592,22 +824,34 @@ def initialize_solver(cls) -> None: else: raise ValueError(f"Invalid solver type: {cls._solver_type}") + # Store collision pipeline config + cls._collision_cfg = cfg.collision_cfg # type: ignore[union-attr] + # Determine if we need external collision detection # - SolverMuJoCo with use_mujoco_contacts=True: uses internal MuJoCo collision detection # - SolverMuJoCo with use_mujoco_contacts=False: needs Newton's unified collision pipeline # - Other solvers (XPBD, Featherstone): always need Newton's unified collision pipeline if isinstance(cls._solver, SolverMuJoCo): - # Handle both dict and object configs - if hasattr(solver_cfg, "use_mujoco_contacts"): - use_mujoco_contacts = solver_cfg.use_mujoco_contacts - elif isinstance(solver_cfg, dict): - use_mujoco_contacts = solver_cfg.get("use_mujoco_contacts", False) - else: - use_mujoco_contacts = getattr(solver_cfg, "use_mujoco_contacts", False) - cls._needs_collision_pipeline = not use_mujoco_contacts + cls._needs_collision_pipeline = not solver_cfg.use_mujoco_contacts + if solver_cfg.use_mujoco_contacts and cls._collision_cfg is not None: + raise ValueError( + "NewtonManager: collision_cfg cannot be set when use_mujoco_contacts=True." + " Either set use_mujoco_contacts=False or remove collision_cfg." + ) else: cls._needs_collision_pipeline = True + # Force Newton pipeline when SDF is configured + sdf_cfg = cfg.sdf_cfg # type: ignore[union-attr] + has_sdf = ( + sdf_cfg is not None + and (sdf_cfg.body_patterns is not None or sdf_cfg.shape_patterns is not None) + and (sdf_cfg.max_resolution is not None or sdf_cfg.target_voxel_size is not None) + ) + if has_sdf and not cls._needs_collision_pipeline: + logger.warning("SDF collision requires Newton collision pipeline. Overriding use_mujoco_contacts.") + cls._needs_collision_pipeline = True + # Initialize contacts and collision pipeline cls._initialize_contacts() diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py index afbf7f54ba0c..1462fb1a46a6 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py @@ -12,8 +12,10 @@ from isaaclab.physics import PhysicsCfg from isaaclab.utils import configclass +from .newton_collision_cfg import NewtonCollisionPipelineCfg, SDFCfg + if TYPE_CHECKING: - from .newton_manager import NewtonManager + from isaaclab_newton.physics import NewtonManager @configclass @@ -104,7 +106,27 @@ class MJWarpSolverCfg(NewtonSolverCfg): """Whether to use parallel line search.""" use_mujoco_contacts: bool = True - """Whether to use MuJoCo's contact solver.""" + """Whether to use MuJoCo's internal contact solver. + + If ``True`` (default), MuJoCo handles collision detection and contact resolution internally. + If ``False``, Newton's :class:`CollisionPipeline` is used instead. A default pipeline + (``broad_phase="explicit"``) is created automatically when :attr:`NewtonCfg.collision_cfg` + is ``None``. Set :attr:`NewtonCfg.collision_cfg` to a :class:`NewtonCollisionPipelineCfg` + to customize pipeline parameters (broad phase, contact limits, hydroelastic, etc.). + + .. note:: + Setting ``collision_cfg`` while ``use_mujoco_contacts=True`` raises + :class:`ValueError` because the two collision modes are mutually exclusive. + """ + + tolerance: float = 1e-6 + """Solver convergence tolerance for the constraint residual. + + The solver iterates until the residual drops below this threshold or + ``iterations`` is reached. Lower values give more precise constraint + satisfaction at the cost of more iterations. MuJoCo default is ``1e-8``; + Newton default is ``1e-6``. + """ @configclass @@ -218,3 +240,32 @@ class NewtonCfg(PhysicsCfg): solver_cfg: NewtonSolverCfg = MJWarpSolverCfg() """Solver configuration. Default is MJWarpSolverCfg().""" + + collision_cfg: NewtonCollisionPipelineCfg | None = None + """Newton collision pipeline configuration. + + Controls how Newton's :class:`CollisionPipeline` is configured when it is active. + The pipeline is active when: + + - :class:`MJWarpSolverCfg` with ``use_mujoco_contacts=False``, or + - any non-MuJoCo solver (:class:`XPBDSolverCfg`, :class:`FeatherstoneSolverCfg`). + + If ``None`` (default), a pipeline with ``broad_phase="explicit"`` is created + automatically. Set this to a :class:`NewtonCollisionPipelineCfg` to customize + parameters such as broad phase algorithm, contact limits, or hydroelastic mode. + + .. note:: + Must not be set when ``use_mujoco_contacts=True`` (raises :class:`ValueError`). + """ + + sdf_cfg: SDFCfg | None = None + """SDF collision configuration. + + When set, mesh collision shapes matching the configured patterns will have + SDF built via Newton's ``mesh.build_sdf()`` at simulation start. This + also forces Newton's collision pipeline to be active (overriding + ``use_mujoco_contacts=True`` if necessary). + + See :class:`~isaaclab_newton.physics.newton_collision_cfg.SDFCfg` for + available parameters. + """ diff --git a/source/isaaclab_newton/test/physics/test_sdf_config.py b/source/isaaclab_newton/test/physics/test_sdf_config.py new file mode 100644 index 000000000000..b2ed547753e8 --- /dev/null +++ b/source/isaaclab_newton/test/physics/test_sdf_config.py @@ -0,0 +1,487 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Tests for SDF collision configuration and application logic.""" + +import re +from unittest.mock import MagicMock, patch + +from newton import GeoType, ModelBuilder, ShapeFlags + + +class TestBuildSdfOnMesh: + """Tests for NewtonManager._build_sdf_on_mesh.""" + + @staticmethod + def _make_sdf_cfg(max_resolution=256, narrow_band_range=(-0.1, 0.1), target_voxel_size=None): + cfg = MagicMock() + cfg.max_resolution = max_resolution + cfg.narrow_band_range = narrow_band_range + cfg.target_voxel_size = target_voxel_size + return cfg + + def test_none_mesh_is_noop(self): + """Passing None as mesh should not raise.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + sdf_cfg = self._make_sdf_cfg() + NewtonManager._build_sdf_on_mesh(None, sdf_cfg, None, "test_label") + + def test_builds_sdf_with_max_resolution(self): + """SDF is built on mesh with max_resolution and narrow_band_range.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + mesh = MagicMock() + mesh.sdf = None + sdf_cfg = self._make_sdf_cfg(max_resolution=128) + + NewtonManager._build_sdf_on_mesh(mesh, sdf_cfg, None, "test_label") + + mesh.build_sdf.assert_called_once_with(narrow_band_range=(-0.1, 0.1), max_resolution=128) + + def test_clears_existing_sdf_before_rebuild(self): + """Existing SDF on mesh is cleared before building a new one.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + mesh = MagicMock() + mesh.sdf = "existing_sdf" + sdf_cfg = self._make_sdf_cfg() + + NewtonManager._build_sdf_on_mesh(mesh, sdf_cfg, None, "test_label") + + mesh.clear_sdf.assert_called_once() + mesh.build_sdf.assert_called_once() + + def test_target_voxel_size_passed_alongside_resolution(self): + """When target_voxel_size is set, it is passed alongside max_resolution.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + mesh = MagicMock() + mesh.sdf = None + sdf_cfg = self._make_sdf_cfg(max_resolution=256, target_voxel_size=0.005) + + NewtonManager._build_sdf_on_mesh(mesh, sdf_cfg, None, "test_label") + + call_kwargs = mesh.build_sdf.call_args[1] + assert call_kwargs["target_voxel_size"] == 0.005 + assert call_kwargs["max_resolution"] == 256 + + def test_resolution_override_by_pattern(self): + """Per-pattern resolution override is applied when label matches.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + mesh = MagicMock() + mesh.sdf = None + sdf_cfg = self._make_sdf_cfg(max_resolution=256) + res_overrides = [(re.compile(".*elbow.*"), 128)] + + NewtonManager._build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, "/World/Robot/elbow_link/collision") + + call_kwargs = mesh.build_sdf.call_args[1] + assert call_kwargs["max_resolution"] == 128 + + def test_resolution_override_no_match_uses_global(self): + """When label doesn't match any override, global max_resolution is used.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + mesh = MagicMock() + mesh.sdf = None + sdf_cfg = self._make_sdf_cfg(max_resolution=256) + res_overrides = [(re.compile(".*elbow.*"), 128)] + + NewtonManager._build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, "/World/Robot/wrist_link/collision") + + call_kwargs = mesh.build_sdf.call_args[1] + assert call_kwargs["max_resolution"] == 256 + + def test_resolution_override_first_match_wins(self): + """First matching pattern in res_overrides determines resolution.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + mesh = MagicMock() + mesh.sdf = None + sdf_cfg = self._make_sdf_cfg(max_resolution=256) + res_overrides = [ + (re.compile(".*link.*"), 64), + (re.compile(".*elbow.*"), 128), + ] + + NewtonManager._build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, "/World/Robot/elbow_link/collision") + + call_kwargs = mesh.build_sdf.call_args[1] + assert call_kwargs["max_resolution"] == 64 # ".*link.*" matches first + + +class TestApplySdfConfig: + """Tests for NewtonManager._apply_sdf_config shape index collection and patching.""" + + @staticmethod + def _make_builder(bodies, shapes): + """Create a minimal ModelBuilder-like mock. + + Args: + bodies: List of body label strings. + shapes: List of dicts with keys: body_idx, label, geo_type, flags, source. + """ + builder = MagicMock(spec=ModelBuilder) + builder.body_label = bodies + builder.shape_count = len(shapes) + builder.shape_type = [s["geo_type"] for s in shapes] + builder.shape_body = [s["body_idx"] for s in shapes] + builder.shape_label = [s["label"] for s in shapes] + builder.shape_flags = [s["flags"] for s in shapes] + builder.shape_source = [s.get("source") for s in shapes] + builder.shape_margin = [0.0] * len(shapes) + builder.shape_material_kh = [0.0] * len(shapes) + return builder + + @staticmethod + def _make_cfg( + body_patterns=None, + shape_patterns=None, + max_resolution=256, + k_hydro=None, + hydroelastic_shape_patterns=None, + ): + cfg = MagicMock() + cfg.sdf_cfg = MagicMock() + cfg.sdf_cfg.max_resolution = max_resolution + cfg.sdf_cfg.target_voxel_size = None + cfg.sdf_cfg.narrow_band_range = (-0.1, 0.1) + cfg.sdf_cfg.margin = None + cfg.sdf_cfg.body_patterns = body_patterns + cfg.sdf_cfg.shape_patterns = shape_patterns + cfg.sdf_cfg.pattern_resolutions = None + cfg.sdf_cfg.use_visual_meshes = False + cfg.sdf_cfg.k_hydro = k_hydro + cfg.sdf_cfg.hydroelastic_shape_patterns = hydroelastic_shape_patterns + return cfg + + def test_no_sdf_cfg_is_noop(self): + """_apply_sdf_config returns early when sdf_cfg is None.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + builder = MagicMock(spec=ModelBuilder) + with patch("isaaclab_newton.physics.newton_manager.PhysicsManager") as pm: + pm._cfg = MagicMock() + pm._cfg.sdf_cfg = None + NewtonManager._apply_sdf_config(builder) + assert not builder.method_calls + + def test_no_patterns_warns(self): + """_apply_sdf_config warns when no patterns are set.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + builder = MagicMock(spec=ModelBuilder) + cfg = self._make_cfg(body_patterns=None, shape_patterns=None) + with patch("isaaclab_newton.physics.newton_manager.PhysicsManager") as pm: + pm._cfg = cfg + with patch("isaaclab_newton.physics.newton_manager.logger") as mock_logger: + NewtonManager._apply_sdf_config(builder) + mock_logger.warning.assert_called() + + def test_body_pattern_collects_shapes(self): + """Shapes under matching bodies are collected for SDF.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + bodies = ["/World/Robot/elbow", "/World/Robot/wrist"] + shapes = [ + { + "body_idx": 0, + "label": "/World/Robot/elbow/col", + "geo_type": GeoType.MESH, + "flags": ShapeFlags.COLLIDE_SHAPES, + "source": MagicMock(sdf=None), + }, + { + "body_idx": 1, + "label": "/World/Robot/wrist/col", + "geo_type": GeoType.MESH, + "flags": ShapeFlags.COLLIDE_SHAPES, + "source": MagicMock(sdf=None), + }, + ] + builder = self._make_builder(bodies, shapes) + cfg = self._make_cfg(body_patterns=[".*elbow.*"]) + + with patch("isaaclab_newton.physics.newton_manager.PhysicsManager") as pm: + pm._cfg = cfg + NewtonManager._apply_sdf_config(builder) + + shapes[0]["source"].build_sdf.assert_called_once() + shapes[1]["source"].build_sdf.assert_not_called() + + def test_hydroelastic_flag_set_when_k_hydro(self): + """HYDROELASTIC flag is set on matched shapes when k_hydro is provided.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + bodies = ["/World/Robot/elbow"] + shapes = [ + { + "body_idx": 0, + "label": "/World/Robot/elbow/col", + "geo_type": GeoType.MESH, + "flags": ShapeFlags.COLLIDE_SHAPES, + "source": MagicMock(sdf=None), + }, + ] + builder = self._make_builder(bodies, shapes) + cfg = self._make_cfg(body_patterns=[".*elbow.*"], k_hydro=1e10) + + with patch("isaaclab_newton.physics.newton_manager.PhysicsManager") as pm: + pm._cfg = cfg + NewtonManager._apply_sdf_config(builder) + + assert builder.shape_flags[0] & ShapeFlags.HYDROELASTIC + assert builder.shape_material_kh[0] == 1e10 + + def test_hydroelastic_shape_patterns_filter(self): + """hydroelastic_shape_patterns limits which shapes get HYDROELASTIC flag.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + bodies = ["/World/Robot/elbow", "/World/Robot/wrist"] + shapes = [ + { + "body_idx": 0, + "label": "/World/Robot/elbow/col", + "geo_type": GeoType.MESH, + "flags": ShapeFlags.COLLIDE_SHAPES, + "source": MagicMock(sdf=None), + }, + { + "body_idx": 1, + "label": "/World/Robot/wrist/col", + "geo_type": GeoType.MESH, + "flags": ShapeFlags.COLLIDE_SHAPES, + "source": MagicMock(sdf=None), + }, + ] + builder = self._make_builder(bodies, shapes) + cfg = self._make_cfg( + body_patterns=[".*"], + k_hydro=1e10, + hydroelastic_shape_patterns=[".*elbow.*"], + ) + + with patch("isaaclab_newton.physics.newton_manager.PhysicsManager") as pm: + pm._cfg = cfg + NewtonManager._apply_sdf_config(builder) + + shapes[0]["source"].build_sdf.assert_called_once() + shapes[1]["source"].build_sdf.assert_called_once() + assert builder.shape_flags[0] & ShapeFlags.HYDROELASTIC + assert not (builder.shape_flags[1] & ShapeFlags.HYDROELASTIC) + + def test_shape_pattern_matching(self): + """shape_patterns directly matches shape labels.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + bodies = ["/World/Robot/body"] + shapes = [ + { + "body_idx": 0, + "label": "/World/Robot/body/Gear_col", + "geo_type": GeoType.MESH, + "flags": ShapeFlags.COLLIDE_SHAPES, + "source": MagicMock(sdf=None), + }, + { + "body_idx": 0, + "label": "/World/Robot/body/frame_col", + "geo_type": GeoType.MESH, + "flags": ShapeFlags.COLLIDE_SHAPES, + "source": MagicMock(sdf=None), + }, + ] + builder = self._make_builder(bodies, shapes) + cfg = self._make_cfg(shape_patterns=[".*Gear.*"]) + + with patch("isaaclab_newton.physics.newton_manager.PhysicsManager") as pm: + pm._cfg = cfg + NewtonManager._apply_sdf_config(builder) + + shapes[0]["source"].build_sdf.assert_called_once() + shapes[1]["source"].build_sdf.assert_not_called() + + def test_non_mesh_shapes_skipped(self): + """Non-mesh shapes are never collected for SDF.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + bodies = ["/World/Robot/elbow"] + shapes = [ + { + "body_idx": 0, + "label": "/World/Robot/elbow/box", + "geo_type": GeoType.BOX, + "flags": ShapeFlags.COLLIDE_SHAPES, + "source": None, + }, + ] + builder = self._make_builder(bodies, shapes) + cfg = self._make_cfg(body_patterns=[".*elbow.*"]) + + with patch("isaaclab_newton.physics.newton_manager.PhysicsManager") as pm: + pm._cfg = cfg + NewtonManager._apply_sdf_config(builder) + + +class TestCreateSdfCollisionFromVisual: + """Tests for NewtonManager._create_sdf_collision_from_visual.""" + + @staticmethod + def _make_builder(bodies, shapes): + """Create a minimal ModelBuilder-like mock.""" + builder = MagicMock(spec=ModelBuilder) + builder.body_label = bodies + builder.shape_count = len(shapes) + builder.shape_type = [s["geo_type"] for s in shapes] + builder.shape_body = [s["body_idx"] for s in shapes] + builder.shape_label = [s["label"] for s in shapes] + builder.shape_flags = [s["flags"] for s in shapes] + builder.shape_source = [s.get("source") for s in shapes] + builder.shape_transform = [s.get("xform", (0, 0, 0, 0, 0, 0, 1)) for s in shapes] + builder.shape_scale = [s.get("scale", (1, 1, 1)) for s in shapes] + builder.shape_margin = [0.0] * len(shapes) + builder.shape_material_kh = [0.0] * len(shapes) + return builder + + @staticmethod + def _make_sdf_cfg(k_hydro=None, margin=None): + cfg = MagicMock() + cfg.max_resolution = 256 + cfg.target_voxel_size = None + cfg.narrow_band_range = (-0.1, 0.1) + cfg.margin = margin + cfg.k_hydro = k_hydro + return cfg + + def test_creates_collision_from_visual_mesh(self): + """A body with only a visual mesh (no COLLIDE_SHAPES) gets a new collision shape.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + bodies = ["/World/Robot/arm"] + mesh = MagicMock(sdf=None) + shapes = [ + { + "body_idx": 0, + "label": "/World/Robot/arm/visual", + "geo_type": GeoType.MESH, + "flags": 0, + "source": mesh, + }, + ] + builder = self._make_builder(bodies, shapes) + sdf_cfg = self._make_sdf_cfg() + + num_added, num_hydro = NewtonManager._create_sdf_collision_from_visual(builder, {0}, sdf_cfg, None) + + assert num_added == 1 + assert num_hydro == 0 + builder.add_shape_mesh.assert_called_once() + mesh.build_sdf.assert_called_once() + + def test_skips_body_with_existing_collision(self): + """A body that already has a COLLIDE_SHAPES shape is not given a visual-mesh collision.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + bodies = ["/World/Robot/arm"] + shapes = [ + { + "body_idx": 0, + "label": "/World/Robot/arm/collision", + "geo_type": GeoType.MESH, + "flags": ShapeFlags.COLLIDE_SHAPES, + "source": MagicMock(sdf=None), + }, + ] + builder = self._make_builder(bodies, shapes) + sdf_cfg = self._make_sdf_cfg() + + num_added, _ = NewtonManager._create_sdf_collision_from_visual(builder, {0}, sdf_cfg, None) + + assert num_added == 0 + builder.add_shape_mesh.assert_not_called() + + def test_warns_when_no_visual_mesh_source(self): + """A body with no mesh source logs a warning and is skipped.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + bodies = ["/World/Robot/arm"] + shapes = [ + { + "body_idx": 0, + "label": "/World/Robot/arm/visual", + "geo_type": GeoType.MESH, + "flags": 0, + "source": None, + }, + ] + builder = self._make_builder(bodies, shapes) + sdf_cfg = self._make_sdf_cfg() + + with patch("isaaclab_newton.physics.newton_manager.logger") as mock_logger: + num_added, _ = NewtonManager._create_sdf_collision_from_visual(builder, {0}, sdf_cfg, None) + mock_logger.warning.assert_called() + + assert num_added == 0 + + def test_k_hydro_sets_hydroelastic_on_new_shape(self): + """When k_hydro is set, the new collision shape gets is_hydroelastic=True.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + bodies = ["/World/Robot/arm"] + mesh = MagicMock(sdf=None) + shapes = [ + { + "body_idx": 0, + "label": "/World/Robot/arm/visual", + "geo_type": GeoType.MESH, + "flags": 0, + "source": mesh, + }, + ] + builder = self._make_builder(bodies, shapes) + sdf_cfg = self._make_sdf_cfg(k_hydro=1e10) + + num_added, num_hydro = NewtonManager._create_sdf_collision_from_visual(builder, {0}, sdf_cfg, None) + + assert num_added == 1 + assert num_hydro == 1 + call_kwargs = builder.add_shape_mesh.call_args[1] + assert call_kwargs["cfg"].is_hydroelastic is True + + def test_hydro_patterns_filter_respected(self): + """hydroelastic patterns filter which visual-mesh shapes get HYDROELASTIC.""" + from isaaclab_newton.physics.newton_manager import NewtonManager + + bodies = ["/World/Robot/arm", "/World/Robot/leg"] + mesh_arm = MagicMock(sdf=None) + mesh_leg = MagicMock(sdf=None) + shapes = [ + { + "body_idx": 0, + "label": "/World/Robot/arm/visual", + "geo_type": GeoType.MESH, + "flags": 0, + "source": mesh_arm, + }, + { + "body_idx": 1, + "label": "/World/Robot/leg/visual", + "geo_type": GeoType.MESH, + "flags": 0, + "source": mesh_leg, + }, + ] + builder = self._make_builder(bodies, shapes) + sdf_cfg = self._make_sdf_cfg(k_hydro=1e10) + hydro_patterns = [re.compile(".*arm.*")] + + num_added, num_hydro = NewtonManager._create_sdf_collision_from_visual( + builder, {0, 1}, sdf_cfg, None, hydro_patterns + ) + + assert num_added == 2 + assert num_hydro == 1 # only arm matches hydro pattern