Skip to content

Add SDF collision and hydroelastic shape configuration#5228

Open
AntoineRichard wants to merge 16 commits intodevelopfrom
antoiner/newton-sdf-config
Open

Add SDF collision and hydroelastic shape configuration#5228
AntoineRichard wants to merge 16 commits intodevelopfrom
antoiner/newton-sdf-config

Conversation

@AntoineRichard
Copy link
Copy Markdown
Collaborator

Summary

  • Add SDFCfg configclass for SDF mesh collision configuration (body/shape regex patterns, resolution overrides, hydroelastic flag enablement)
  • Add missing HydroelasticSDFCfg pipeline params (moment_matching, buffer_mult_*, grid_size)
  • Add manager methods to build SDF on matching shapes before model finalization
  • Integrate with Newton cloner to apply SDF on prototypes before replication and skip convex hull for SDF shapes
  • Force Newton collision pipeline when SDF is configured
  • Add warning when hydroelastic config has no effect (no qualifying shape pairs)

Built on top of #5219. Ports SDF/hydroelastic shape preparation from #5160 into #5219's config design.

Test plan

  • 14 unit tests for _build_sdf_on_mesh and _apply_sdf_config (pattern matching, resolution overrides, hydroelastic flags)
  • Integration test with a Newton scene using SDFCfg + NewtonCollisionPipelineCfg

🤖 Generated with Claude Code

ooctipus and others added 12 commits April 9, 2026 18:06
Add NewtonCollisionPipelineCfg and HydroelasticSDFCfg to expose
Newton's collision pipeline parameters (broad phase, contact limits,
hydroelastic mode) through NewtonCfg.collision_cfg.

Add MJWarpSolverCfg.tolerance for solver convergence control.

Fix validation order so collision_cfg is stored before the
use_mujoco_contacts consistency check runs.  Reset _collision_cfg
in clear() to avoid stale state across reset cycles.  Fall back to
a default CollisionPipeline when collision_cfg is None.
Move config resolution out of NewtonManager into
NewtonCollisionPipelineCfg.to_pipeline_args(), following the Kamino
to_solver_config() pattern. Fix truthiness check on hydroelastic config
dict to use explicit `is not None`. Add missing changelog entry for
MJWarpSolverCfg.tolerance. Use specific return type hint dict[str, Any].
Add SDFCfg @configclass for SDF mesh collision configuration with fields
for resolution, narrow band, margin, body/shape pattern matching,
per-pattern resolution overrides, visual mesh fallback, and hydroelastic
stiffness assignment.

Add five missing fields to HydroelasticSDFCfg: moment_matching,
buffer_mult_broad, buffer_mult_iso, buffer_mult_contact, and grid_size.
Add sdf_cfg field (SDFCfg | None) to NewtonCfg and re-export SDFCfg
from the physics package __init__.pyi so callers can configure SDF
mesh collision at the top-level manager config.
Add _build_sdf_on_mesh, _create_sdf_collision_from_visual, and
_apply_sdf_config classmethods to NewtonManager. These methods apply
SDFCfg settings to matching mesh shapes in the model builder, including
per-pattern resolution overrides and optional hydroelastic flags.
When SDF patterns are configured, exclude matching mesh shapes from
convex-hull approximation so their triangle geometry is preserved for
mesh.build_sdf(). Also call NewtonManager._apply_sdf_config() on each
prototype before add_builder copies it N times, so SDF is built once
and all environments inherit it.
Unit tests for NewtonManager._build_sdf_on_mesh and
_apply_sdf_config using unittest.mock to avoid needing a running
Newton simulation. Covers resolution overrides, hydroelastic flags,
body/shape pattern matching, and non-mesh shape exclusion.
@github-actions github-actions bot added documentation Improvements or additions to documentation isaac-lab Related to Isaac Lab team labels Apr 10, 2026
@greptile-apps
Copy link
Copy Markdown
Contributor

greptile-apps bot commented Apr 10, 2026

Greptile Summary

This PR adds SDFCfg for configuring SDF mesh collision on Newton shapes (body/shape regex patterns, per-pattern resolution overrides, hydroelastic stiffness), fills in missing HydroelasticSDFCfg pipeline params, integrates SDF building into the Newton cloner (preserving triangle meshes for SDF shapes before convex-hull approximation), and forces Newton's collision pipeline when SDF is configured. All three P2 findings are in newton_manager.py and are non-blocking for merge.

Confidence Score: 5/5

Safe to merge; all findings are P2 style/quality issues that do not block correct operation for the primary use case.

All three flagged issues are P2: a doc/code mismatch on max_resolution vs target_voxel_size (Newton likely handles this at the API level), a dead-code warning block, and silent truncation to the first visual mesh per body in an opt-in path. Core SDF building, pattern matching, cloner integration, pipeline forcing, and the 14 unit tests are all correct. No P0/P1 defects found.

source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py — contains all three P2 findings

Important Files Changed

Filename Overview
source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py Adds SDFCfg configclass and missing HydroelasticSDFCfg fields (moment_matching, buffer_mult_*, grid_size); to_pipeline_args correctly converts nested config to Newton's HydroelasticSDF.Config.
source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py Adds _build_sdf_on_mesh, _create_sdf_collision_from_visual, and _apply_sdf_config; has a doc/code mismatch where max_resolution is passed alongside target_voxel_size, a dead-code warning block, and silent truncation to the first visual mesh per body in the visual-mesh fallback path.
source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py Integrates SDF pattern detection to exclude SDF-destined shapes from convex-hull approximation before replication; reads PhysicsManager._cfg at build time which is safe because cfg is None guard is present.
source/isaaclab_newton/test/physics/test_sdf_config.py 14 unit tests for _build_sdf_on_mesh and _apply_sdf_config; test_target_voxel_size_passed_alongside_resolution validates that both target_voxel_size and max_resolution are forwarded, which conflicts with the documented "Ignored when target_voxel_size is set" semantics.
source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py Adds `sdf_cfg: SDFCfg
source/isaaclab_newton/isaaclab_newton/physics/init.pyi Exports SDFCfg in __all__ and the re-export block; consistent with new public API.
source/isaaclab_newton/docs/CHANGELOG.rst New version heading 0.5.12 (2026-04-10) with correct RST formatting; extension.toml version matches; entries are accurate and include migration-relevant context.

Sequence Diagram

sequenceDiagram
    participant Cloner as newton_replicate
    participant Builder as ModelBuilder (proto)
    participant NM as NewtonManager
    participant Mesh as newton Mesh

    Cloner->>Builder: add_usd(src_path, skip_mesh_approximation=True)
    Cloner->>Cloner: detect SDF body/shape patterns from PhysicsManager._cfg
    Cloner->>Builder: approximate_meshes(convex_hull, shape_indices=non_sdf_indices)
    Note over Builder: SDF-matched shapes keep original triangle mesh
    Cloner->>NM: _apply_sdf_config(proto_builder)
    NM->>Builder: iterate shape_type==MESH matching patterns
    NM->>Mesh: build_sdf(max_resolution, narrow_band_range)
    NM->>Builder: shape_flags[si] |= HYDROELASTIC (if k_hydro set)
    Cloner->>Builder: add_builder(proto, xform=...) x N envs

    Note over NM: initialize_solver()
    NM->>NM: detect has_sdf → force _needs_collision_pipeline=True
    NM->>NM: _initialize_contacts()
    NM->>NM: CollisionPipeline(model, **collision_cfg.to_pipeline_args())
    NM->>NM: warn if hydroelastic_sdf is None but HydroelasticSDFCfg set
Loading

Reviews (1): Last reviewed commit: "Add SDF changelog entries and bump to 0...." | Re-trigger Greptile

Comment on lines +415 to +420
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)
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P2 max_resolution passed alongside target_voxel_size, contradicting docs

SDFCfg.max_resolution is documented as "Ignored when target_voxel_size is set", but _build_sdf_on_mesh unconditionally passes max_resolution whenever it is non-None, even when target_voxel_size is also set. If Newton's mesh.build_sdf() lets target_voxel_size take precedence, this is merely a misleading doc. If it applies max_resolution in combination with target_voxel_size (or raises on both being set), callers who rely on the documented "target_voxel_size overrides max_resolution" behaviour will get wrong or unexpected results silently.

The fix is to skip the max_resolution kwarg when target_voxel_size is supplied:

Suggested change
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)
sdf_kwargs: dict = dict(narrow_band_range=sdf_cfg.narrow_band_range)
if sdf_cfg.target_voxel_size is not None:
sdf_kwargs["target_voxel_size"] = sdf_cfg.target_voxel_size
elif resolution is not None:
sdf_kwargs["max_resolution"] = resolution
mesh.build_sdf(**sdf_kwargs)

test_target_voxel_size_passed_alongside_resolution currently asserts that both are forwarded; it should be updated to assert that only target_voxel_size appears in call_kwargs when it is set.

Comment on lines +817 to +820
# Force Newton pipeline when collision_cfg or SDF is configured
if cfg.collision_cfg is not None and not cls._needs_collision_pipeline:
logger.warning("collision_cfg set — enabling Newton collision pipeline.")
cls._needs_collision_pipeline = True
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P2 Unreachable warning — dead code

The condition cfg.collision_cfg is not None and not cls._needs_collision_pipeline can never be True at this point:

  • MuJoCo solver, use_mujoco_contacts=True + collision_cfg set: a ValueError is raised on line 810 before reaching here.
  • MuJoCo solver, use_mujoco_contacts=False: cls._needs_collision_pipeline is already True.
  • XPBD / Featherstone: cls._needs_collision_pipeline is set to True unconditionally on line 815.

The warning and the cls._needs_collision_pipeline = True assignment at line 820 are therefore dead code. Consider removing this block to avoid confusion, or document exactly which future code path it is meant to guard.

Comment on lines +460 to +485
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])

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=sdf_shape_cfg,
label=f"{body_lbl}/sdf_collision",
)
num_added += 1
if sdf_cfg.k_hydro is not None:
num_hydro += 1
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P2 Only the first visual mesh shape is used per body; silent data loss for multi-mesh bodies

When use_visual_meshes=True, _create_sdf_collision_from_visual breaks after finding the first visual-mesh shape for a body:

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   # ← only first visual mesh picked

A body with two or more visual meshes (e.g. a base + an attachment) will only get SDF collision geometry for whichever mesh happens to come first. No warning is logged when more visual shapes are skipped. Consider iterating over all visual meshes for the body, or at least emitting a warning when multiple candidates are found and only the first is used.

Copy link
Copy Markdown
Contributor

@kellyguo11 kellyguo11 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Review: Add SDF collision and hydroelastic shape configuration

Solid PR that cleanly ports SDF/hydroelastic shape preparation from #5160 into #5219's config design. The config hierarchy is well-structured, the pattern-matching system is flexible, and the cloner integration correctly preserves triangle meshes for SDF shapes. The 14 unit tests cover the key logic paths well.

I agree with Greptile's P2 findings — the max_resolution/target_voxel_size doc-vs-code mismatch and the dead-code warning block should be cleaned up. A few additional items below.

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)
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

max_resolution and target_voxel_size forwarded simultaneously despite docs saying one overrides the other

+1 to Greptile's finding. The SDFCfg.max_resolution docstring says "Ignored when target_voxel_size is set", but both are unconditionally passed to mesh.build_sdf(). Either:

  1. Make the code match the docs (use elif so target_voxel_size takes precedence), or
  2. Update the docs to say they're both forwarded and let Newton decide.

Option 1 is safer — it's what users will expect from reading the config:

        sdf_kwargs: dict = dict(narrow_band_range=sdf_cfg.narrow_band_range)
        if sdf_cfg.target_voxel_size is not None:
            sdf_kwargs["target_voxel_size"] = sdf_cfg.target_voxel_size
        elif resolution is not None:
            sdf_kwargs["max_resolution"] = resolution
        mesh.build_sdf(**sdf_kwargs)

And update test_target_voxel_size_passed_alongside_resolution accordingly.

# Force Newton pipeline when collision_cfg or SDF is configured
if cfg.collision_cfg is not None and not cls._needs_collision_pipeline:
logger.warning("collision_cfg set — enabling Newton collision pipeline.")
cls._needs_collision_pipeline = True
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Dead code — this warning block is unreachable

+1 to Greptile. By this point:

  • MuJoCo + use_mujoco_contacts=True + collision_cfg → already raised ValueError above
  • MuJoCo + use_mujoco_contacts=False_needs_collision_pipeline is already True
  • Non-MuJoCo solvers → _needs_collision_pipeline is already True

Suggest removing this block. If it's intended as a safety net for future solver types, add a comment explaining that.

continue
approx_indices.append(i)
if approx_indices:
p.approximate_meshes("convex_hull", shape_indices=approx_indices, keep_visual_shapes=True)
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

_apply_sdf_config called on prototype builders, but it reads PhysicsManager._cfg

This works at runtime because PhysicsManager._cfg is set before cloning begins. But it creates an implicit coupling: _apply_sdf_config is a classmethod on NewtonManager that reads global state from PhysicsManager._cfg, and now it's being called with prototype ModelBuilder objects (not the final builder).

This is fine for now, but consider:

  1. Adding a brief comment on the prototype builder noting that _apply_sdf_config reads from PhysicsManager._cfg, not from the builder itself.
  2. Long-term, passing sdf_cfg explicitly as a parameter rather than reading global state would make this more testable and less fragile.

Non-blocking — the current approach works correctly.

Defaults to ``None``.
"""

target_voxel_size: float | None = None
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

SDFCfg.max_resolution docstring says "Ignored when target_voxel_size is set" but that's not what the code does

As noted on the manager side, the code currently passes both. Either update this docstring to match reality, or fix the code to match this docstring. Keeping them in sync avoids user confusion.

)
num_added += 1
if sdf_cfg.k_hydro is not None:
num_hydro += 1
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Silent truncation to first visual mesh per body

+1 to Greptile. When a body has multiple visual meshes, only the first is used with no warning. At minimum, log a warning when len([si for si in sdf_shape_indices if builder.shape_body[si] == body_idx and builder.shape_source[si] is not None]) > 1.

Better yet, iterate over all visual meshes for the body, since SDF collision should cover the full geometry.


body_patterns: list[str] | None = None
"""Regex patterns for body labels.

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nit: The docstring says (-0.1, 0.1) for the default, which matches the field default. But consider documenting the physical meaning more explicitly — e.g. "10cm inside / 10cm outside the mesh surface" — since users unfamiliar with SDF narrow bands may not understand the units immediately.

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).

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

sdf_cfg docstring says it forces Newton collision pipeline, but the actual override only happens for use_mujoco_contacts=True

The docstring says: "This also forces Newton's collision pipeline to be active (overriding use_mujoco_contacts=True if necessary)."

But in initialize_solver, the SDF override only fires when not cls._needs_collision_pipeline. For non-MuJoCo solvers, the pipeline is already forced on, so there's no override. For MuJoCo with use_mujoco_contacts=True, the collision_cfg check above already raises a ValueError — so SDF with MuJoCo contacts would need to bypass that check first.

Is the intent that SDF should work with use_mujoco_contacts=True by silently overriding it? If so, the ValueError raised for collision_cfg + use_mujoco_contacts=True would block that path. Consider clarifying the intended behavior.

@@ -0,0 +1,1106 @@
# Newton SDF & Hydroelastic Config Implementation Plan
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nit: Consider whether this 1100-line plan doc should be included in the final PR. It's great for development context but adds significant weight to the repo's docs/ tree. If this is an internal-only workflow artifact, it might be better in a separate location or excluded from the PR.

Changelog
---------

0.5.12 (2026-04-10)
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nit: RST heading underline should match the title length. 0.5.12 (2026-04-10) is 19 chars but the underline ~~~~~~~~~~~~~~~~~~~ is 19 tildes — looks correct actually. 👍


# Note: Semantic Versioning is used: https://semver.org/
version = "0.5.10"
version = "0.5.12"
Copy link
Copy Markdown
Contributor

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.

Fix max_resolution/target_voxel_size docstrings to accurately describe
that both are forwarded to Newton. Remove unreachable collision_cfg
force-override block in initialize_solver.
- Pass hydro_patterns to _create_sdf_collision_from_visual so it
  respects hydroelastic_shape_patterns filter (was unconditionally
  setting HYDROELASTIC)
- Replace getattr(cfg, "sdf_cfg", None) with direct attribute access
  in all 3 locations
- Add regex validation with actionable ValueError on invalid patterns
- Log warning and return False from _build_sdf_on_mesh when mesh is
  None; only count successful SDF builds in num_patched
- Fix :attr: -> :class: for SDFCfg reference in docstring
return True

@classmethod
def _create_sdf_collision_from_visual(
Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmmmm physx sdf is defined in colliders, and modified through collision_prop_cfg, e.g. you can have convex hull nut collider, and sdf nut collider while convex hull nut collide convex hull robot, while sdf nut collides sdf bolt, but this implementation seems like that newton is global overriding all visual meshes to sdf?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

documentation Improvements or additions to documentation isaac-lab Related to Isaac Lab team

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants