-
Notifications
You must be signed in to change notification settings - Fork 3.4k
Add SDF collision and hydroelastic shape configuration #5228
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
base: develop
Are you sure you want to change the base?
Changes from all commits
43ae9f3
c9c1a29
7bab8a0
7bde3cb
99b6848
a106714
a01f9fd
398f666
07f71ac
d7e93d1
13508bb
0d602a6
8d24953
9d630a5
87af9c5
1d09ba1
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,6 +1,46 @@ | ||
| Changelog | ||
| --------- | ||
|
|
||
| 0.5.12 (2026-04-10) | ||
|
Contributor
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. nit: RST heading underline should match the title length. |
||
| ~~~~~~~~~~~~~~~~~~~ | ||
|
|
||
| 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) | ||
| ~~~~~~~~~~~~~~~~~~~ | ||
|
|
||
|
|
||
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -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) | ||
|
Contributor
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
This works at runtime because This is fine for now, but consider:
Non-blocking — the current approach works correctly. |
||
| 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) | ||
|
|
||
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Version jumps from 0.5.10 to 0.5.12, skipping 0.5.11
The changelog adds both 0.5.11 and 0.5.12 entries, but this PR's base presumably already has 0.5.10. Since this PR is built on top of #5219, 0.5.11 likely comes from that PR. Make sure the version bump chain is correct when both PRs merge — if #5219 sets 0.5.11, then this PR setting 0.5.12 is correct, but if #5219 doesn't bump the version, you'll have a gap in the version history.