diff --git a/source/isaaclab_newton/changelog.d/mzamoramora-collision-decimation.minor.rst b/source/isaaclab_newton/changelog.d/mzamoramora-collision-decimation.minor.rst new file mode 100644 index 000000000000..f2cc59363b0d --- /dev/null +++ b/source/isaaclab_newton/changelog.d/mzamoramora-collision-decimation.minor.rst @@ -0,0 +1,13 @@ +Added +^^^^^ + +* Added :attr:`~isaaclab_newton.physics.NewtonCfg.collision_decimation` to + re-invoke the Newton collision pipeline every ``N`` solver substeps within + a physics tick. Defaults to ``0`` (legacy: one collide per tick). When set + to ``0 < N < num_substeps``, the substep loop in + :meth:`~isaaclab_newton.physics.NewtonManager._run_solver_substeps` calls + the collision pipeline again at the matching substep boundaries so contact + normals reflect the bodies' just-integrated poses. The last substep is + intentionally skipped — its contact set would only feed the next tick. + :meth:`~isaaclab_newton.physics.NewtonCfg.__post_init__` warns when + ``collision_decimation >= num_substeps`` (the gate is silently bypassed). diff --git a/source/isaaclab_newton/changelog.d/mzamoramora-rigid-object-kinematic-fix.rst b/source/isaaclab_newton/changelog.d/mzamoramora-rigid-object-kinematic-fix.rst new file mode 100644 index 000000000000..b858c3c9fafc --- /dev/null +++ b/source/isaaclab_newton/changelog.d/mzamoramora-rigid-object-kinematic-fix.rst @@ -0,0 +1,16 @@ +Fixed +^^^^^ + +* Fixed :class:`~isaaclab_newton.assets.RigidObjectData` crashing with + ``IndexError: tuple index out of range`` for kinematic-enabled + single-body fixed-base rigid objects. The ``is_fixed_base`` branch + in ``_create_simulation_bindings`` indexed ``[:, 0, 0]`` assuming a + 3D ``(count, links, 1)`` layout, but Newton returns a 2D + ``(count, links)`` array when the view contains a single body. + Dispatch on actual ``ndim`` instead so both multi-link fixed-base + articulations and single-body kinematic rigid objects are handled + correctly. Also fixes the matching no-velocity fallback in + ``_create_buffers``: ``_sim_bind_body_com_vel_w`` is now allocated + as ``(num_instances, 1)`` to match the + ``derive_body_acceleration_from_body_com_velocities`` kernel's + 2D signature. diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py index 43e719d3a580..84d180bb34a8 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py @@ -804,18 +804,22 @@ def _create_simulation_bindings(self) -> None: self._num_bodies = self._root_view.link_count # -- root properties - if self._root_view.is_fixed_base: - self._sim_bind_root_link_pose_w = self._root_view.get_root_transforms(SimulationManager.get_state_0())[ - :, 0, 0 - ] - else: - self._sim_bind_root_link_pose_w = self._root_view.get_root_transforms(SimulationManager.get_state_0())[:, 0] + # Newton's ``get_root_transforms`` / ``get_root_velocities`` return + # either a 2D ``(count, links_per_env)`` array (single-body + # kinematic-enabled rigid objects, e.g. Factory's bolt) or a 3D + # ``(count, links_per_env, 1)`` array (multi-link fixed-base + # articulations). Dispatch on actual ``ndim`` so we don't crash + # with ``IndexError: tuple index out of range`` when a fixed-base + # rigid object yields the 2D layout. + root_xforms = self._root_view.get_root_transforms(SimulationManager.get_state_0()) + self._sim_bind_root_link_pose_w = root_xforms[:, 0, 0] if root_xforms.ndim >= 3 else root_xforms[:, 0] self._sim_bind_root_com_vel_w = self._root_view.get_root_velocities(SimulationManager.get_state_0()) if self._sim_bind_root_com_vel_w is not None: - if self._root_view.is_fixed_base: - self._sim_bind_root_com_vel_w = self._sim_bind_root_com_vel_w[:, 0, 0] - else: - self._sim_bind_root_com_vel_w = self._sim_bind_root_com_vel_w[:, 0] + self._sim_bind_root_com_vel_w = ( + self._sim_bind_root_com_vel_w[:, 0, 0] + if self._sim_bind_root_com_vel_w.ndim >= 3 + else self._sim_bind_root_com_vel_w[:, 0] + ) # -- body properties self._sim_bind_body_com_pos_b = self._root_view.get_attribute("body_com", SimulationManager.get_model())[:, 0] self._sim_bind_body_link_pose_w = self._root_view.get_link_transforms(SimulationManager.get_state_0())[:, 0] @@ -854,11 +858,17 @@ def _create_buffers(self) -> None: "Failed to get root com velocity. If the rigid object is fixed, this is expected. " "Setting root com velocity to zeros." ) + # ``_sim_bind_root_com_vel_w`` is consumed as a 1D array by + # ``get_root_link_vel_from_root_com_vel`` (kernel signature has + # ``com_vel: wp.array(dtype=wp.spatial_vectorf)``), so the fallback + # stays 1D. ``_sim_bind_body_com_vel_w`` feeds the 2D-typed + # ``derive_body_acceleration_from_body_com_velocities`` kernel, so + # the fallback is 2D ``(num_instances, 1)`` to match. self._sim_bind_root_com_vel_w = wp.zeros( (self._num_instances,), dtype=wp.spatial_vectorf, device=self.device ) self._sim_bind_body_com_vel_w = wp.zeros( - (self._num_instances,), dtype=wp.spatial_vectorf, device=self.device + (self._num_instances, 1), dtype=wp.spatial_vectorf, device=self.device ) # -- default root pose and velocity self._default_root_pose = wp.zeros((self._num_instances,), dtype=wp.transformf, device=self.device) diff --git a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py index e99b1eb7abdd..429e59cd390c 100644 --- a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py +++ b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py @@ -5,18 +5,32 @@ from __future__ import annotations +import re from collections.abc import Sequence 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 +from isaaclab.physics import PhysicsManager + 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: Sequence[str], @@ -66,8 +80,6 @@ def _build_newton_builder_from_mapping( # Deformable prim paths are handled by per_world_builder_hooks, not add_usd. # Resolve the regex prim_path patterns to concrete env_0 paths so add_usd # can skip them via ignore_paths. - import re - _deformable_ignore_paths: list[str] = [] if hasattr(NewtonManager, "_deformable_registry"): for entry in NewtonManager._deformable_registry: @@ -81,6 +93,16 @@ def _build_newton_builder_from_mapping( if pat.match(child_path): _deformable_ignore_paths.append(child_path) + # 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 = NewtonManager.create_builder(up_axis=up_axis) @@ -94,7 +116,33 @@ def _build_newton_builder_from_mapping( ignore_paths=_deformable_ignore_paths if _deformable_ignore_paths else None, ) 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 # Inject registered sites into prototypes (and global sites into main builder) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi index 176973cd5400..9d832a73d9f1 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi +++ b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi @@ -17,6 +17,7 @@ __all__ = [ "NewtonShapeCfg", "NewtonSolverCfg", "NewtonXPBDManager", + "SDFCfg", "XPBDSolverCfg", ] @@ -26,7 +27,7 @@ from .kamino_manager import NewtonKaminoManager from .kamino_manager_cfg import KaminoSolverCfg from .mjwarp_manager import NewtonMJWarpManager from .mjwarp_manager_cfg import MJWarpSolverCfg -from .newton_collision_cfg import HydroelasticSDFCfg, NewtonCollisionPipelineCfg +from .newton_collision_cfg import HydroelasticSDFCfg, NewtonCollisionPipelineCfg, SDFCfg from .newton_manager import NewtonManager from .newton_manager_cfg import ( NewtonCfg, diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py index 9e75d3153308..72c33403127d 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -69,6 +69,38 @@ class HydroelasticSDFCfg: 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: @@ -184,3 +216,119 @@ def to_pipeline_args(self) -> dict[str, Any]: 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 500d07b726e7..77d3fc6c7835 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -10,10 +10,12 @@ import contextlib import ctypes import logging +import re from abc import abstractmethod from collections.abc import Callable from typing import TYPE_CHECKING +import torch import warp as wp # Load CUDA runtime for relaxed-mode graph capture (RTX-compatible). @@ -199,6 +201,7 @@ class NewtonManager(PhysicsManager): _solver_dt: float = 1.0 / 200.0 _num_substeps: int = 1 _decimation: int = 1 + _collision_decimation: int = 0 _num_envs: int | None = None # Newton model and state @@ -230,6 +233,10 @@ class NewtonManager(PhysicsManager): _world_reset_mask: wp.array | None = None # (num_envs,) wp.int32 — for SolverKamino.reset(world_mask=...) _fk_reset_mask: wp.array | None = None # (articulation_count,) wp.bool — for eval_fk(mask=...) + # Per-world mask of NaN-divergent envs queued for sanitize at next reset. + # Populated by :meth:`sanitize_nan_envs`, drained by :meth:`sanitize_pending_nan_envs`. + _nan_env_mask_pending_reset: torch.Tensor | None = None + # Newton actuator adapter (owns actuators and double-buffered states) _adapter: NewtonActuatorAdapter | None = None # In-graph hooks invoked after the actuator step and before the solver @@ -677,6 +684,7 @@ def clear(cls): # Per-world reset masks NewtonManager._world_reset_mask = None NewtonManager._fk_reset_mask = None + NewtonManager._nan_env_mask_pending_reset = None NewtonManager._graph = None NewtonManager._graph_capture_pending = False NewtonManager._newton_stage_path = None @@ -891,6 +899,212 @@ 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]) + + 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 = getattr(cfg, "sdf_cfg", None) + 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 + + 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 + + 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) + + 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 + + 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, @@ -1218,6 +1432,7 @@ def initialize_solver(cls) -> None: with Timer(name="newton_initialize_solver", msg="Initialize solver took:"): NewtonManager._num_substeps = cfg.num_substeps # type: ignore[union-attr] + NewtonManager._collision_decimation = cfg.collision_decimation # type: ignore[union-attr] NewtonManager._solver_dt = cls.get_physics_dt() / cls._num_substeps NewtonManager._collision_cfg = cfg.collision_cfg # type: ignore[union-attr] @@ -1439,10 +1654,17 @@ def _capture_relaxed_graph(cls, device: str): @classmethod def _run_solver_substeps(cls, contacts) -> None: """Run ``num_substeps`` solver iterations, handling double-buffered state swap.""" + collide_every = cls._collision_decimation + # Last substep is skipped: its contact set would only feed the next tick's + # top-of-loop collide(), not this one. + collide_mid_loop = collide_every > 0 and cls._needs_collision_pipeline and contacts is not None + if cls._use_single_state: - for _ in range(cls._num_substeps): + for i in range(cls._num_substeps): cls._step_solver(cls._state_0, cls._state_0, cls._control, contacts, cls._solver_dt) cls._state_0.clear_forces() + if collide_mid_loop and (i + 1) % collide_every == 0 and i + 1 < cls._num_substeps: + cls._collision_pipeline.collide(cls._state_0, contacts) else: cfg = PhysicsManager._cfg need_copy_on_last = (cfg is not None and cfg.use_cuda_graph) and cls._num_substeps % 2 == 1 # type: ignore[union-attr] @@ -1453,6 +1675,8 @@ def _run_solver_substeps(cls, contacts) -> None: else: NewtonManager._state_0, NewtonManager._state_1 = cls._state_1, cls._state_0 cls._state_0.clear_forces() + if collide_mid_loop and (i + 1) % collide_every == 0 and i + 1 < cls._num_substeps: + cls._collision_pipeline.collide(cls._state_0, contacts) @classmethod def _update_sensors(cls, contacts) -> None: @@ -1469,6 +1693,141 @@ def _update_sensors(cls, contacts) -> None: for sensor in cls._newton_contact_sensors.values(): sensor.update(cls._state_0, eval_contacts) + # ------------------------------------------------------------------ + # NaN recovery + # ------------------------------------------------------------------ + + @classmethod + def sanitize_world_state(cls, env_ids) -> None: + """Reset solver internals + Newton State buffers for the given worlds. + + Used by tasks that detect NaN-divergent worlds and want to recover + them in place (vs throwing the episode away). Zeroes per-world + MJWarp solver scratch buffers (``qacc_warmstart``, ``qfrc_*``, + ``cacc``, ``cfrc_*``) and Newton State velocity/force buffers + (``joint_qd``, ``body_qd``, ``body_f``, ``body_qdd``, + ``body_parent_f``) at the indexed worlds, then runs + :func:`newton.eval_fk` to re-derive ``state.body_q`` from + ``joint_q``. + + ``mjwarp.reset_data`` covers ``qacc_warmstart`` and the contact + arrays but leaves the derived ``qfrc_*`` family and the COM-frame + ``cfrc_int``/``cacc``/``cfrc_ext`` untouched. Empirically those + latter buffers re-divergence the same world on the next solver + step (``cfrc_int`` is read-modify-written via ``wp.atomic_add`` in + ``mujoco_warp/_src/smooth.py:_cfrc_backward``, so a stale NaN + survives the next ``rne()`` call). This implementation walks all + 17 named MJWarp fields explicitly to close that gap. See + ``newton-physics/newton#1266`` for the upstream discussion. + + No-op for solvers without a ``mjw_data`` attribute (XPBD, + Featherstone, Kamino) — those solvers do not exhibit the MuJoCo + warm-start contamination pattern this addresses. + + Args: + env_ids: 1-D ``int`` torch tensor of world IDs to sanitize, on + the simulation device. + """ + import newton as _newton # noqa: PLC0415 + + if cls._model is None or cls._solver is None or cls._num_envs is None: + return + if env_ids.numel() == 0: + return + + mjw_data = getattr(cls._solver, "mjw_data", None) + if mjw_data is None: + return # not the MJWarp backend; nothing to scrub + + env_ids_list = env_ids.tolist() + for field_name in ( + "qacc_warmstart", + "qacc", + "qacc_smooth", + "qfrc_applied", + "qfrc_bias", + "qfrc_spring", + "qfrc_damper", + "qfrc_gravcomp", + "qfrc_fluid", + "qfrc_passive", + "qfrc_actuator", + "qfrc_smooth", + "qfrc_constraint", + "qfrc_inverse", + "cacc", + "cfrc_int", + "cfrc_ext", + ): + arr = getattr(mjw_data, field_name, None) + if arr is None: + continue + t = wp.to_torch(arr) + for env_id in env_ids_list: + t[env_id] = 0.0 + + state = cls._state_0 + model = cls._model + num_envs = cls._num_envs + nd_per_env = model.joint_dof_count // num_envs + nb_per_env = model.body_count // num_envs + for buf_name, per_env in ( + ("joint_qd", nd_per_env), + ("body_qd", nb_per_env), + ("body_f", nb_per_env), + ("body_qdd", nb_per_env), + ("body_parent_f", nb_per_env), + ): + buf = getattr(state, buf_name, None) + if buf is None: + continue + wp.to_torch(buf).view(num_envs, per_env, -1)[env_ids] = 0.0 + + _newton.eval_fk(model, state.joint_q, state.joint_qd, state) + body_q_prev = getattr(state, "body_q_prev", None) + if body_q_prev is not None: + bq = wp.to_torch(state.body_q).view(num_envs, nb_per_env, -1) + wp.to_torch(body_q_prev).view(num_envs, nb_per_env, -1)[env_ids] = bq[env_ids] + + @classmethod + def flag_nan_envs(cls, mask: torch.Tensor) -> None: + """Flag NaN-divergent envs for sanitization at the next episode reset. + + Call from a task's per-step NaN detection (e.g. ``torch.isnan`` over + obs/state tensors). Pure bookkeeping: the mask is ORed into + :attr:`_nan_env_mask_pending_reset`; no solver state is touched yet. + :meth:`sanitize_pending_nan_envs` drains the queue at the next reset. + + Tasks that need to keep training between detection and reset should + ``torch.nan_to_num`` their obs/state/reward themselves — flagging is + the only side effect here. + + No-op when ``mask.any()`` is False. + + Args: + mask: 1-D ``bool`` torch tensor of shape ``(num_worlds,)``; True for + envs just detected as NaN-divergent. + """ + if not mask.any(): + return + if cls._nan_env_mask_pending_reset is None: + cls._nan_env_mask_pending_reset = torch.zeros_like(mask) + cls._nan_env_mask_pending_reset |= mask + + @classmethod + def sanitize_pending_nan_envs(cls) -> None: + """Drain the pending-reset NaN queue and sanitize those envs. + + Call from a task's reset hook (e.g. ``_reset_idx``) before re-init. + No-op when no envs were queued via :meth:`flag_nan_envs` since the + last call. + """ + if cls._nan_env_mask_pending_reset is None or not cls._nan_env_mask_pending_reset.any(): + return + nan_ids = cls._nan_env_mask_pending_reset.nonzero(as_tuple=False).squeeze(-1) + cls.sanitize_world_state(nan_ids) + cls._nan_env_mask_pending_reset.zero_() + # ------------------------------------------------------------------ # Composite stepping routines # ------------------------------------------------------------------ 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 6ff646aff57b..c2d6aabda565 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py @@ -7,16 +7,19 @@ from __future__ import annotations +import logging from typing import TYPE_CHECKING from isaaclab.physics import PhysicsCfg from isaaclab.utils.configclass import configclass -from .newton_collision_cfg import NewtonCollisionPipelineCfg +from .newton_collision_cfg import NewtonCollisionPipelineCfg, SDFCfg if TYPE_CHECKING: from isaaclab_newton.physics import NewtonManager +logger = logging.getLogger(__name__) + @configclass class NewtonSolverCfg: @@ -97,6 +100,9 @@ class NewtonCfg(PhysicsCfg): num_substeps: int = 1 """Number of substeps to use for the solver.""" + collision_decimation: int = 0 + """Re-collide every N solver substeps within a physics tick (``0`` = once per tick).""" + debug_mode: bool = False """Whether to enable debug mode for the solver.""" @@ -138,6 +144,15 @@ class NewtonCfg(PhysicsCfg): :class:`NewtonShapeCfg` for the declared fields. """ + sdf_cfg: SDFCfg | None = None + """SDF mesh collision configuration. + + When set, matched bodies/shapes receive SDF collision and optional + hydroelastic stiffness. See :class:`~isaaclab_newton.physics.SDFCfg` + for the declared fields. When ``None`` (default), no SDF collision is + configured. + """ + def __post_init__(self): # NewtonCfg.class_type is auto-derived from solver_cfg.class_type. # Refuse a user-set value: setting both is ambiguous and was @@ -149,3 +164,12 @@ def __post_init__(self): self.solver_cfg = MJWarpSolverCfg() self.class_type = self.solver_cfg.class_type + + # Mid-tick re-collide is silently disabled when collision_decimation >= num_substeps. + if self.collision_decimation > 0 and self.collision_decimation >= self.num_substeps: + logger.warning( + "NewtonCfg.collision_decimation=%d is >= num_substeps=%d; mid-tick re-collide is disabled. " + "Set 0 < collision_decimation < num_substeps to enable.", + self.collision_decimation, + self.num_substeps, + ) diff --git a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py index f8ac77586cc0..8458f466632c 100644 --- a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py +++ b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py @@ -134,6 +134,29 @@ def test_newton_cfg_post_init_propagates_class_type( assert cfg.class_type.__name__ == expected_manager.__name__ +@pytest.mark.parametrize( + "num_substeps, collision_decimation, should_warn", + [ + (8, 0, False), # Default: feature disabled, no warning. + (8, 1, False), # Valid: re-collide every substep. + (8, 2, False), # Valid: re-collide every 2 substeps. + (8, 7, False), # Valid edge: one mid-loop re-collide at i=6. + (8, 8, True), # Equal to num_substeps: gate never fires. + (8, 16, True), # Larger than num_substeps: gate never fires. + ], +) +def test_newton_cfg_collision_decimation_warning(num_substeps, collision_decimation, should_warn, caplog): + """``NewtonCfg.__post_init__`` warns when ``collision_decimation >= num_substeps``.""" + import logging + + with caplog.at_level(logging.WARNING, logger="isaaclab_newton.physics.newton_manager_cfg"): + cfg = NewtonCfg(num_substeps=num_substeps, collision_decimation=collision_decimation) + warned = any("collision_decimation" in rec.getMessage() for rec in caplog.records) + assert warned is should_warn + # Cfg field round-trips regardless of warning. + assert cfg.collision_decimation == collision_decimation + + # --------------------------------------------------------------------------- # Manager class hierarchy and factory contracts # --------------------------------------------------------------------------- @@ -272,3 +295,68 @@ def test_mjwarp_internal_contacts_with_collision_cfg_raises(): with pytest.raises(ValueError, match="collision_cfg cannot be set"): sim.reset() + + +@pytest.mark.parametrize( + "num_substeps, collision_decimation, expected_mid_loop_collides", + [ + (8, 0, 0), # Feature disabled. + (8, 2, 3), # Re-collide after substeps 2, 4, 6 (skip last). + (8, 4, 1), # Re-collide after substep 4 only. + (8, 7, 1), # Re-collide after substep 7 only. + (8, 8, 0), # Gated off (>= num_substeps). + ], +) +def test_collision_decimation_invokes_mid_loop_collide(num_substeps, collision_decimation, expected_mid_loop_collides): + """``_run_solver_substeps`` re-invokes ``collide`` at the expected substeps. + + Wraps :attr:`NewtonManager._collision_pipeline.collide` with a counter and + runs one physics tick. The collide-call count is ``1`` (top-of-tick) plus + one per matching mid-loop substep, excluding the last substep. + + The scene has a free-joint sphere falling onto a ground plane so the + broadphase actually generates pairs — guards against a future change + that skips ``collide()`` when there are no collidable shapes. + """ + sim_cfg = SimulationCfg( + dt=1.0 / 120.0, + device="cuda:0", + gravity=(0.0, 0.0, -9.81), + physics=NewtonCfg( + solver_cfg=MJWarpSolverCfg(use_mujoco_contacts=False), + num_substeps=num_substeps, + collision_decimation=collision_decimation, + use_cuda_graph=False, + ), + ) + + with build_simulation_context(sim_cfg=sim_cfg) as sim: + builder = NewtonManager.create_builder() + body = builder.add_body(mass=1.0) + builder.add_joint_free(child=body) + builder.add_shape_sphere(body=body, radius=0.05) + builder.add_ground_plane() + # Lift the sphere to 0.5 m above the plane so the scene is non-degenerate. + # joint_q for a free joint is [tx, ty, tz, qx, qy, qz, qw]. + builder.joint_q[-7:] = [0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0] + NewtonManager.set_builder(builder) + sim.reset() + + # Wrap collide() with a counter — must run after sim.reset() so the + # pipeline is allocated, and use_cuda_graph=False so the wrapped + # Python callable isn't bypassed by a captured graph. + calls = {"n": 0} + original_collide = NewtonManager._collision_pipeline.collide + + def counting_collide(state, contacts): + calls["n"] += 1 + return original_collide(state, contacts) + + NewtonManager._collision_pipeline.collide = counting_collide + try: + sim.step(render=False) + finally: + NewtonManager._collision_pipeline.collide = original_collide + + # Expect: 1 (top-of-tick) + expected_mid_loop_collides. + assert calls["n"] == 1 + expected_mid_loop_collides 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