From 43ae9f35d06fbe5b28519a78f3b7c3a0ee722f7c Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 9 Apr 2026 18:06:21 -0700 Subject: [PATCH 01/35] expose newton collision cfg --- .../isaaclab_newton/physics/__init__.pyi | 11 +- .../physics/newton_collision_cfg.py | 168 ++++++++++++++++++ .../isaaclab_newton/physics/newton_manager.py | 31 +++- .../physics/newton_manager_cfg.py | 43 ++++- 4 files changed, 241 insertions(+), 12 deletions(-) create mode 100644 source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py diff --git a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi index 4e589e6f69d1..1b18da3838e1 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi +++ b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi @@ -5,12 +5,21 @@ __all__ = [ "FeatherstoneSolverCfg", + "HydroelasticSDFCfg", "MJWarpSolverCfg", "NewtonCfg", + "NewtonCollisionPipelineCfg", "NewtonManager", "NewtonSolverCfg", "XPBDSolverCfg", ] +from .newton_collision_cfg import HydroelasticSDFCfg, NewtonCollisionPipelineCfg 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..be1d6a6748c9 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -0,0 +1,168 @@ +# 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 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). + """ + + +@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). + """ diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 3e967751d8df..9971aef1c101 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -28,6 +28,7 @@ _cudart = None from newton import Axis, CollisionPipeline, Contacts, Control, Model, ModelBuilder, State, eval_fk from newton._src.usd.schemas import SchemaResolverNewton, SchemaResolverPhysx +from newton.geometry import HydroelasticSDF from newton.sensors import SensorContact as NewtonContactSensor from newton.solvers import SolverBase, SolverFeatherstone, SolverMuJoCo, SolverNotifyFlags, SolverXPBD @@ -37,6 +38,7 @@ if TYPE_CHECKING: from isaaclab.sim.simulation_context import SimulationContext + from .newton_collision_cfg import NewtonCollisionPipelineCfg logger = logging.getLogger(__name__) @@ -91,6 +93,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 +368,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 @@ -532,7 +536,15 @@ 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: + cfg_dict = cls._collision_cfg.to_dict() + hydro_cfg = cfg_dict.pop("sdf_hydroelastic_config", None) + if hydro_cfg: + cfg_dict["sdf_hydroelastic_config"] = HydroelasticSDF.Config(**hydro_cfg) + cls._collision_pipeline = CollisionPipeline(cls._model, **cfg_dict) + else: + cls._collision_pipeline = CollisionPipeline(cls._model, broad_phase="explicit") + if cls._contacts is None: cls._contacts = cls._collision_pipeline.contacts() @@ -592,19 +604,20 @@ 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 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..942a6dc2f49d 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 + 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,20 @@ 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`). + """ From c9c1a29eaad465f35a724accadf7cbe33a88433a Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 9 Apr 2026 18:09:32 -0700 Subject: [PATCH 02/35] Add collision pipeline cfg, bump to 0.5.11 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. --- source/isaaclab_newton/config/extension.toml | 2 +- source/isaaclab_newton/docs/CHANGELOG.rst | 9 +++++++++ .../isaaclab_newton/physics/newton_manager.py | 1 + 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/source/isaaclab_newton/config/extension.toml b/source/isaaclab_newton/config/extension.toml index 7f6e09f992a5..9cfd4c5fc105 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.11" # 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..fa54dc114eb8 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,15 @@ Changelog --------- +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`. + + 0.5.10 (2026-04-05) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 9971aef1c101..2ca7df7cab21 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -38,6 +38,7 @@ if TYPE_CHECKING: from isaaclab.sim.simulation_context import SimulationContext + from .newton_collision_cfg import NewtonCollisionPipelineCfg logger = logging.getLogger(__name__) From 7bab8a0dc0ea168062caa17ac04916ed910425d6 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 9 Apr 2026 18:11:47 -0700 Subject: [PATCH 03/35] line break --- source/isaaclab_newton/docs/CHANGELOG.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index fa54dc114eb8..7a720e3ea215 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -7,7 +7,8 @@ Changelog Added ^^^^^ -* Added :class:`~isaaclab_newton.physics.NewtonCollisionPipelineCfg` to expose Newton collision pipeline parameters via :attr:`~isaaclab_newton.physics.NewtonCfg.collision_cfg`. +* Added :class:`~isaaclab_newton.physics.NewtonCollisionPipelineCfg` to expose Newton collision pipeline parameters via + :attr:`~isaaclab_newton.physics.NewtonCfg.collision_cfg`. 0.5.10 (2026-04-05) From 7bde3cb9379b7a0586fb29e1542626d81f35b75a Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 18:02:45 +0200 Subject: [PATCH 04/35] Address review feedback on collision pipeline config 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]. --- source/isaaclab_newton/docs/CHANGELOG.rst | 9 +++++++++ .../physics/newton_collision_cfg.py | 20 ++++++++++++++++++- .../isaaclab_newton/physics/newton_manager.py | 7 +------ 3 files changed, 29 insertions(+), 7 deletions(-) diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index 7a720e3ea215..ac613c0e3d34 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -9,6 +9,15 @@ 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/physics/newton_collision_cfg.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py index be1d6a6748c9..c8b0db0b3f4a 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -7,7 +7,7 @@ from __future__ import annotations -from typing import Literal +from typing import Any, Literal from isaaclab.utils import configclass @@ -166,3 +166,21 @@ class NewtonCollisionPipelineCfg: 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 diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 2ca7df7cab21..e7c191aab067 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -28,7 +28,6 @@ _cudart = None from newton import Axis, CollisionPipeline, Contacts, Control, Model, ModelBuilder, State, eval_fk from newton._src.usd.schemas import SchemaResolverNewton, SchemaResolverPhysx -from newton.geometry import HydroelasticSDF from newton.sensors import SensorContact as NewtonContactSensor from newton.solvers import SolverBase, SolverFeatherstone, SolverMuJoCo, SolverNotifyFlags, SolverXPBD @@ -538,11 +537,7 @@ def _initialize_contacts(cls) -> None: # Newton collision pipeline: create pipeline and generate contacts if cls._collision_pipeline is None: if cls._collision_cfg is not None: - cfg_dict = cls._collision_cfg.to_dict() - hydro_cfg = cfg_dict.pop("sdf_hydroelastic_config", None) - if hydro_cfg: - cfg_dict["sdf_hydroelastic_config"] = HydroelasticSDF.Config(**hydro_cfg) - cls._collision_pipeline = CollisionPipeline(cls._model, **cfg_dict) + cls._collision_pipeline = CollisionPipeline(cls._model, **cls._collision_cfg.to_pipeline_args()) else: cls._collision_pipeline = CollisionPipeline(cls._model, broad_phase="explicit") From 99b6848d4c3ac1b7e2e8bf619d1f924501e08405 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 18:26:10 +0200 Subject: [PATCH 05/35] Add SDF/hydroelastic config design spec and implementation plan --- .../2026-04-10-newton-sdf-hydroelastic.md | 1106 +++++++++++++++++ ...0-newton-sdf-hydroelastic-config-design.md | 156 +++ 2 files changed, 1262 insertions(+) create mode 100644 docs/superpowers/plans/2026-04-10-newton-sdf-hydroelastic.md create mode 100644 docs/superpowers/specs/2026-04-10-newton-sdf-hydroelastic-config-design.md diff --git a/docs/superpowers/plans/2026-04-10-newton-sdf-hydroelastic.md b/docs/superpowers/plans/2026-04-10-newton-sdf-hydroelastic.md new file mode 100644 index 000000000000..73d5e5039ceb --- /dev/null +++ b/docs/superpowers/plans/2026-04-10-newton-sdf-hydroelastic.md @@ -0,0 +1,1106 @@ +# Newton SDF & Hydroelastic Config Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Port SDF collision and hydroelastic shape preparation from PR #5160 into PR #5219's config design, as a new PR built on top of #5219's branch. + +**Architecture:** Add `SDFCfg` configclass for SDF mesh preparation (patterns, resolution, hydroelastic flags). Add manager methods to build SDF on matching shapes before model finalization. Integrate with the Newton cloner to apply SDF on prototypes before replication. + +**Tech Stack:** Python, Newton physics engine, Warp, IsaacLab configclass system + +--- + +### Task 0: Create feature branch + +**Files:** None + +- [ ] **Step 1: Create branch off PR #5219** + +```bash +git checkout expose_newton_collision_pipeline +git checkout -b antoiner/newton-sdf-config +``` + +- [ ] **Step 2: Commit the spec and plan docs** + +```bash +git add docs/superpowers/specs/2026-04-10-newton-sdf-hydroelastic-config-design.md docs/superpowers/plans/2026-04-10-newton-sdf-hydroelastic.md +git commit -m "Add SDF/hydroelastic config design spec and implementation plan" +``` + +--- + +### Task 1: Add `SDFCfg` configclass + +**Files:** +- Modify: `source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py` + +- [ ] **Step 1: Add `SDFCfg` class after `NewtonCollisionPipelineCfg`** + +Add this class at the end of `newton_collision_cfg.py`: + +```python +@configclass +class SDFCfg: + """Configuration for SDF (Signed Distance Field) collision on Newton meshes. + + When provided as :attr:`~isaaclab_newton.physics.NewtonCfg.sdf_cfg`, mesh + collision shapes matching the configured patterns will have SDF built via + Newton's ``mesh.build_sdf()`` API before model finalization. + + At least one of :attr:`max_resolution` or :attr:`target_voxel_size` must be + set for SDF to be built. At least one of :attr:`body_patterns` or + :attr:`shape_patterns` must be set to select which shapes receive SDF. + + .. note:: + For hydroelastic contacts to be generated, shapes must have SDF built + and the ``HYDROELASTIC`` flag set. Set :attr:`k_hydro` to enable + hydroelastic on all matched shapes, or use + :attr:`hydroelastic_shape_patterns` to limit which shapes get the flag. + The pipeline-level hydroelastic processing parameters are configured + separately via + :attr:`NewtonCollisionPipelineCfg.sdf_hydroelastic_config`. + """ + + max_resolution: int | None = None + """Maximum dimension [voxels] for sparse SDF grid (must be divisible by 8). + + Typical values: 128, 256, 512. + """ + + target_voxel_size: float | None = None + """Target voxel size [m] for sparse SDF grid. + + If provided, takes precedence over :attr:`max_resolution`. + """ + + narrow_band_range: tuple[float, float] = (-0.1, 0.1) + """Narrow band distance range (inner, outer) [m] for SDF computation.""" + + margin: float | None = None + """Collision margin [m] for SDF shapes. If ``None``, uses the builder's default.""" + + body_patterns: list[str] | None = None + """Regex patterns to match body labels (USD prim paths) for SDF. + + Bodies whose label matches at least one pattern will have SDF applied to + all their mesh shapes. Example: ``[".*elbow.*", ".*wrist.*"]``. + """ + + shape_patterns: list[str] | None = None + """Regex patterns to match shape labels (USD prim paths) for SDF. + + Only shapes whose label matches at least one pattern get SDF. + Example: ``[".*Gear.*", ".*gear.*"]``. + + .. note:: + At least one of :attr:`body_patterns` or :attr:`shape_patterns` must + be set for SDF to be applied. + """ + + pattern_resolutions: dict[str, int] | None = None + """Per-pattern SDF resolution overrides. + + Maps regex pattern to ``max_resolution`` for matching shapes. Shapes not + matching any pattern use the global :attr:`max_resolution`. First matching + pattern wins. Example: ``{".*elbow.*": 128, ".*power_supply.*": 512}``. + """ + + use_visual_meshes: bool = False + """Whether to create collision shapes from visual meshes for matched bodies + that lack collision geometry. + + When ``False`` (default), only existing collision meshes are patched with + SDF. When ``True``, bodies matching the configured patterns but lacking + collision shapes get a new collision shape created from their first visual + mesh. + """ + + k_hydro: float | None = None + """Hydroelastic stiffness coefficient [Pa] applied to matched shapes. + + If ``None`` (default), the ``HYDROELASTIC`` flag is not set on any shapes. + If set, matched shapes (optionally filtered by + :attr:`hydroelastic_shape_patterns`) get the ``HYDROELASTIC`` flag and + this stiffness value. + + .. note:: + Pipeline-level hydroelastic processing parameters (contact reduction, + buffer sizes, etc.) are configured separately via + :attr:`NewtonCollisionPipelineCfg.sdf_hydroelastic_config`. + """ + + hydroelastic_shape_patterns: list[str] | None = None + """Regex patterns to select which SDF shapes also get hydroelastic contacts. + + If ``None`` and :attr:`k_hydro` is set, all shapes matching the SDF + patterns get hydroelastic. If provided, only shapes whose label matches at + least one pattern get the ``HYDROELASTIC`` flag. + """ +``` + +- [ ] **Step 2: Add missing pipeline params to `HydroelasticSDFCfg`** + +Add these fields to `HydroelasticSDFCfg`, after the existing `output_contact_surface` field: + +```python + moment_matching: bool = False + """Whether to adjust reduced contact friction so net maximum moment matches + the unreduced reference. + + 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. + + Increase if a broadphase overflow warning is issued. + + Defaults to ``1`` (same as Newton's default). + """ + + buffer_mult_iso: int = 1 + """Multiplier for preallocated iso-surface extraction buffers. + + Increase if an iso buffer overflow warning is issued. + + Defaults to ``1`` (same as Newton's default). + """ + + buffer_mult_contact: int = 1 + """Multiplier for the preallocated face contact buffer. + + Increase if a face contact overflow warning is issued. + + Defaults to ``1`` (same as Newton's default). + """ + + grid_size: int = 262144 + """Grid size for hydroelastic contact handling. + + Defaults to ``262144`` (``256 * 8 * 128``, same as Newton's default). + """ +``` + +- [ ] **Step 3: Run pre-commit** + +```bash +./isaaclab.sh -f +``` + +Expected: All checks pass. + +- [ ] **Step 4: Commit** + +```bash +git add source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +git commit -m "Add SDFCfg configclass and missing HydroelasticSDFCfg fields" +``` + +--- + +### Task 2: Wire `SDFCfg` into `NewtonCfg` and exports + +**Files:** +- Modify: `source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py` +- Modify: `source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi` + +- [ ] **Step 1: Add `sdf_cfg` field to `NewtonCfg`** + +In `newton_manager_cfg.py`, add this import at the top (after the existing `NewtonCollisionPipelineCfg` import): + +```python +from .newton_collision_cfg import NewtonCollisionPipelineCfg, SDFCfg +``` + +Then add this field to the `NewtonCfg` class, after the existing `collision_cfg` field: + +```python + 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. + """ +``` + +- [ ] **Step 2: Update `__init__.pyi`** + +Add `SDFCfg` to `__all__` and the import from `newton_collision_cfg`: + +```python +__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, +) +``` + +- [ ] **Step 3: Run pre-commit** + +```bash +./isaaclab.sh -f +``` + +- [ ] **Step 4: Commit** + +```bash +git add source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi +git commit -m "Wire SDFCfg into NewtonCfg and update exports" +``` + +--- + +### Task 3: Add SDF manager methods + +**Files:** +- Modify: `source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py` + +- [ ] **Step 1: Add `re` import** + +Add `import re` to the imports at the top of `newton_manager.py` (after `import logging`). + +- [ ] **Step 2: Add `_build_sdf_on_mesh` static method** + +Add this method to `NewtonManager`, after the `add_model_change` method (around line 392): + +```python + @staticmethod + def _build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, label: str): + """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. + """ + if mesh is None: + return + 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) +``` + +- [ ] **Step 3: Add `_create_sdf_collision_from_visual` classmethod** + +Add this method right after `_build_sdf_on_mesh`: + +```python + @classmethod + def _create_sdf_collision_from_visual( + cls, builder: ModelBuilder, sdf_shape_indices: set[int], sdf_cfg, res_overrides + ): + """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``. + + 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]) + + 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 sdf_cfg.k_hydro is not None: + shape_cfg_kwargs["is_hydroelastic"] = True + shape_cfg_kwargs["kh"] = sdf_cfg.k_hydro + sdf_shape_cfg = ModelBuilder.ShapeConfig(**shape_cfg_kwargs) + + 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]) + + 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 + + return num_added, num_hydro +``` + +- [ ] **Step 4: Add `_apply_sdf_config` classmethod** + +Add this method right after `_create_sdf_collision_from_visual`: + +```python + @classmethod + def _apply_sdf_config(cls, builder: ModelBuilder): + """Apply SDF collision and optional hydroelastic flags to matching mesh shapes. + + Reads :attr:`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 + + # Compile patterns + body_patterns = [re.compile(p) for p in sdf_cfg.body_patterns] if sdf_cfg.body_patterns else None + shape_patterns = [re.compile(p) for p in sdf_cfg.shape_patterns] if sdf_cfg.shape_patterns else None + res_overrides = ( + [(re.compile(p), r) for p, r in sdf_cfg.pattern_resolutions.items()] + if sdf_cfg.pattern_resolutions + else None + ) + hydro_patterns = None + if sdf_cfg.k_hydro is not None and sdf_cfg.hydroelastic_shape_patterns is not None: + hydro_patterns = [re.compile(p) for p in sdf_cfg.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 + cls._build_sdf_on_mesh(builder.shape_source[si], sdf_cfg, res_overrides, builder.shape_label[si]) + 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 + ) + 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})" + ) +``` + +- [ ] **Step 5: Run pre-commit** + +```bash +./isaaclab.sh -f +``` + +- [ ] **Step 6: Commit** + +```bash +git add source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +git commit -m "Add SDF manager methods for shape preparation" +``` + +--- + +### Task 4: Integrate SDF into manager lifecycle + +**Files:** +- Modify: `source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py` + +- [ ] **Step 1: Call `_apply_sdf_config` in `instantiate_builder_from_stage`** + +In `instantiate_builder_from_stage()`, add a call to `_apply_sdf_config` just before `cls.set_builder(builder)` (around line 527): + +```python + cls._apply_sdf_config(builder) + cls.set_builder(builder) +``` + +- [ ] **Step 2: Force collision pipeline in `initialize_solver` when SDF is configured** + +In `initialize_solver()`, after the existing `if isinstance(cls._solver, SolverMuJoCo):` / `else:` block that sets `cls._needs_collision_pipeline` (after line 618), add: + +```python + # 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 + + sdf_cfg = getattr(cfg, "sdf_cfg", None) + 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 +``` + +- [ ] **Step 3: Add hydroelastic warning in `_initialize_contacts`** + +In `_initialize_contacts()`, after the pipeline is created (after the `cls._collision_pipeline = CollisionPipeline(...)` lines, around line 542), add a warning check: + +```python + # 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." + ) +``` + +This goes after the `else: cls._collision_pipeline = CollisionPipeline(...)` branch but before the `if cls._contacts is None:` line, so it applies regardless of whether `_collision_cfg` was set or not. The check should be at the same indent level as the `if cls._collision_cfg is not None:` block: + +```python + if cls._needs_collision_pipeline: + # Newton collision pipeline: create pipeline and generate contacts + if cls._collision_pipeline is None: + 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() +``` + +- [ ] **Step 4: Run pre-commit** + +```bash +./isaaclab.sh -f +``` + +- [ ] **Step 5: Commit** + +```bash +git add source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +git commit -m "Integrate SDF config into manager lifecycle" +``` + +--- + +### Task 5: Cloner integration + +**Files:** +- Modify: `source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py` + +- [ ] **Step 1: Add imports** + +Add `re` and `GeoType` imports at the top of `newton_replicate.py`: + +```python +import re +``` + +(after `from __future__ import annotations`) + +And update the newton import: + +```python +from newton import GeoType, ModelBuilder, solvers +``` + +And add the `PhysicsManager` import: + +```python +from isaaclab.physics import PhysicsManager +``` + +- [ ] **Step 2: Add SDF pattern skip and prototype SDF application** + +In `_build_newton_builder_from_mapping`, after the `env0_pos = positions[0]` line (line 64), add SDF pattern compilation: + +```python + # 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 = getattr(cfg, "sdf_cfg", None) if cfg is not None else None + body_pats = [re.compile(x) for x in sdf_cfg.body_patterns] if sdf_cfg and sdf_cfg.body_patterns else None + shape_pats = [re.compile(x) for x in 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 +``` + +Then replace the existing `if simplify_meshes:` block (line 76-77): + +```python + if simplify_meshes: + p.approximate_meshes("convex_hull", keep_visual_shapes=True) + protos[src_path] = p +``` + +with: + +```python + if simplify_meshes: + 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 +``` + +- [ ] **Step 3: Run pre-commit** + +```bash +./isaaclab.sh -f +``` + +- [ ] **Step 4: Commit** + +```bash +git add source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py +git commit -m "Skip convex hull for SDF shapes and apply SDF on cloner prototypes" +``` + +--- + +### Task 6: Tests + +**Files:** +- Create: `source/isaaclab_newton/test/physics/test_sdf_config.py` + +- [ ] **Step 1: Create test file** + +Create `source/isaaclab_newton/test/physics/test_sdf_config.py` with the following content. Tests use `unittest.mock` to avoid needing a running Newton simulation: + +```python +# 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) + # No crash, no calls + 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) + + # Only elbow shape should have build_sdf called + 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) + + # Both get SDF built + shapes[0]["source"].build_sdf.assert_called_once() + shapes[1]["source"].build_sdf.assert_called_once() + # Only elbow gets hydroelastic + 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) + # No build_sdf calls (box shape has no source to call on) +``` + +- [ ] **Step 2: Run tests** + +```bash +./isaaclab.sh -p -m pytest source/isaaclab_newton/test/physics/test_sdf_config.py -v +``` + +Expected: All tests pass. + +- [ ] **Step 3: Commit** + +```bash +git add source/isaaclab_newton/test/physics/test_sdf_config.py +git commit -m "Add tests for SDF config and shape preparation" +``` + +--- + +### Task 7: Changelog and version bump + +**Files:** +- Modify: `source/isaaclab_newton/docs/CHANGELOG.rst` +- Modify: `source/isaaclab_newton/config/extension.toml` + +- [ ] **Step 1: Bump version to 0.5.12** + +In `source/isaaclab_newton/config/extension.toml`, change: +``` +version = "0.5.11" +``` +to: +``` +version = "0.5.12" +``` + +- [ ] **Step 2: Add new changelog version** + +In `source/isaaclab_newton/docs/CHANGELOG.rst`, add a new version heading before `0.5.11`: + +```rst +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. + + +``` + +- [ ] **Step 3: Run pre-commit** + +```bash +./isaaclab.sh -f +``` + +- [ ] **Step 4: Commit** + +```bash +git add source/isaaclab_newton/docs/CHANGELOG.rst source/isaaclab_newton/config/extension.toml +git commit -m "Add SDF changelog entries and bump to 0.5.12" +``` + +--- + +### Task 8: Final validation + +- [ ] **Step 1: Run all tests** + +```bash +./isaaclab.sh -p -m pytest source/isaaclab_newton/test/physics/test_sdf_config.py -v +``` + +Expected: All tests pass. + +- [ ] **Step 2: Run pre-commit on all files** + +```bash +./isaaclab.sh -f +``` + +Expected: All checks pass. + +- [ ] **Step 3: Verify diff is clean** + +```bash +git diff +git status +``` + +Expected: No unstaged changes. diff --git a/docs/superpowers/specs/2026-04-10-newton-sdf-hydroelastic-config-design.md b/docs/superpowers/specs/2026-04-10-newton-sdf-hydroelastic-config-design.md new file mode 100644 index 000000000000..7be30b84f909 --- /dev/null +++ b/docs/superpowers/specs/2026-04-10-newton-sdf-hydroelastic-config-design.md @@ -0,0 +1,156 @@ +# Newton SDF & Hydroelastic Configuration + +**Date:** 2026-04-10 +**Built on:** PR #5219 (`expose_newton_collision_pipeline`) +**Ports from:** PR #5160 (`vidur/feature/sdf-collision`) +**Target:** New PR → `develop` + +## Summary + +Port SDF collision and hydroelastic shape preparation from PR #5160 into +PR #5219's config design. #5219 owns the collision pipeline config layer; +this PR adds the shape-preparation machinery that makes hydroelastic +contacts actually work end-to-end. + +## Design Decisions + +**Keep #5219's config hierarchy.** `HydroelasticSDFCfg` (pipeline processing +params) stays nested under `NewtonCollisionPipelineCfg.sdf_hydroelastic_config`. + +**Add `SDFCfg` as a new top-level field on `NewtonCfg`.** Shape-level concerns +(which meshes get SDF, resolution, hydroelastic flag enablement) live here, +separate from pipeline configuration. + +**Flatten hydroelastic shape enablement into `SDFCfg`.** #5160 used a nested +`HydroelasticCfg` object. We put `k_hydro` and `hydroelastic_shape_patterns` +directly on `SDFCfg` since they're simple fields that control shape flags, +not a separate subsystem. + +## Config Hierarchy + +``` +NewtonCfg +├── solver_cfg: NewtonSolverCfg +├── collision_cfg: NewtonCollisionPipelineCfg | None +│ ├── broad_phase, reduce_contacts, rigid_contact_max, ... +│ └── sdf_hydroelastic_config: HydroelasticSDFCfg | None +│ ├── reduce_contacts, normal_matching, anchor_contact +│ ├── moment_matching, margin_contact_area +│ ├── buffer_fraction, buffer_mult_broad/iso/contact +│ ├── grid_size, output_contact_surface +│ └── (maps 1:1 to HydroelasticSDF.Config via to_pipeline_args()) +└── sdf_cfg: SDFCfg | None + ├── max_resolution: int | None + ├── target_voxel_size: float | None + ├── narrow_band_range: tuple[float, float] + ├── margin: float | None + ├── body_patterns: list[str] | None + ├── shape_patterns: list[str] | None + ├── pattern_resolutions: dict[str, int] | None + ├── use_visual_meshes: bool + ├── k_hydro: float (shape-level stiffness) + └── hydroelastic_shape_patterns: list[str] | None +``` + +## Components + +### 1. Config additions (`newton_collision_cfg.py`) + +**`SDFCfg`** — new configclass in the existing file. + +Fields ported from #5160's `SDFCfg`: +- `max_resolution`, `target_voxel_size`, `narrow_band_range`, `margin` +- `body_patterns`, `shape_patterns`, `pattern_resolutions` +- `use_visual_meshes` + +Shape-level hydroelastic fields (flattened from #5160's `HydroelasticCfg`): +- `k_hydro: float = 1e10` — stiffness applied to shapes via `shape_material_kh` +- `hydroelastic_shape_patterns: list[str] | None = None` — if None, all + SDF shapes get HYDROELASTIC flag; if set, only matching shapes + +**`HydroelasticSDFCfg`** — add missing pipeline params from #5160: +- `moment_matching: bool = False` +- `buffer_mult_broad: int = 1` +- `buffer_mult_iso: int = 1` +- `buffer_mult_contact: int = 1` +- `grid_size: int = 256 * 8 * 128` + +**`NewtonCfg`** — add `sdf_cfg: SDFCfg | None = None` in `newton_manager_cfg.py`. + +### 2. Manager methods (`newton_manager.py`) + +**`_build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, label)`** — static method. +Builds SDF on a mesh. Clears existing SDF first. Applies per-pattern +resolution overrides. Passes `narrow_band_range`, `max_resolution`, +`target_voxel_size` to `mesh.build_sdf()`. + +**`_apply_sdf_config(builder)`** — classmethod. +1. Read `sdf_cfg` from `PhysicsManager._cfg`; return early if None. +2. Validate that at least one of `max_resolution`/`target_voxel_size` is set. +3. Compile body/shape/hydroelastic regex patterns. +4. Collect matching shape indices from builder. +5. For each matching collision shape: build SDF, optionally set HYDROELASTIC + flag + `k_hydro`. +6. If `use_visual_meshes`, call `_create_sdf_collision_from_visual()`. +7. Log summary. + +**`_create_sdf_collision_from_visual(builder, sdf_shape_indices, sdf_cfg, res_overrides)`** +— classmethod. For matched bodies that lack collision geometry, creates a +collision shape from the first visual mesh with SDF built on it. + +**`initialize_solver()` changes:** +- After determining `_needs_collision_pipeline`, force it `True` when + `collision_cfg is not None` or when `sdf_cfg` has valid patterns + resolution. +- Log a warning when overriding. + +**`_initialize_contacts()` changes:** +- After creating pipeline, if hydroelastic was configured + (`collision_cfg.sdf_hydroelastic_config is not None`) but + `pipeline.hydroelastic_sdf is None`, log a warning. + +**`instantiate_builder_from_stage()` change:** +- Call `cls._apply_sdf_config(builder)` before `cls.set_builder(builder)`. + +### 3. Cloner integration (`newton_replicate.py`) + +In `_build_newton_builder_from_mapping()`: +1. Read `sdf_cfg` from `PhysicsManager._cfg`. +2. Compile body/shape patterns if present. +3. When `simplify_meshes` is True and SDF patterns exist, skip convex hull + approximation for shapes matching SDF patterns (preserves triangle meshes). +4. After prototype building, call `NewtonManager._apply_sdf_config(prototype)` + on each prototype before `add_builder` replication. + +### 4. Exports (`__init__.pyi`) + +Add `SDFCfg` to `__all__` and import list. + +### 5. Tests (`test_sdf_config.py`) + +Port from #5160, adapted for new config structure: +- `TestBuildSdfOnMesh` — None mesh, max_resolution, clear existing SDF, + target_voxel_size precedence, pattern resolution overrides +- `TestApplySdfConfig` — shape index collection by body/shape patterns, + hydroelastic flag setting, visual mesh fallback, edge cases + +### 6. Changelog + +Add to existing `0.5.11` entry (or bump to `0.5.12`): +- Added `SDFCfg` for SDF mesh collision configuration +- Added SDF pattern skip in Newton cloner +- Added missing hydroelastic pipeline params to `HydroelasticSDFCfg` + +## Execution Order + +1. Config additions (SDFCfg, HydroelasticSDFCfg fields, NewtonCfg.sdf_cfg) +2. Manager methods (_build_sdf_on_mesh, _apply_sdf_config, _create_sdf_collision_from_visual) +3. Manager integration (initialize_solver, _initialize_contacts, instantiate_builder_from_stage) +4. Cloner integration (newton_replicate.py) +5. Exports (__init__.pyi) +6. Tests +7. Changelog + version bump + +## Out of Scope + +- Visualizer additions from #5160 (newton_visualizer.py changes) +- Runtime SDF rebuild / dynamic pattern updates From a1067147332d298b95ecdcc8d06fca0b8d8318e5 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 18:27:40 +0200 Subject: [PATCH 06/35] Add SDFCfg configclass and missing HydroelasticSDFCfg fields 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. --- .../physics/newton_collision_cfg.py | 145 ++++++++++++++++++ 1 file changed, 145 insertions(+) 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 c8b0db0b3f4a..dd502aad7204 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,116 @@ 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. + Ignored when :attr:`target_voxel_size` is set. + + Defaults to ``None``. + """ + + target_voxel_size: float | None = None + """Target voxel size [m] for the SDF grid. + + When set, takes precedence over :attr:`max_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``. + """ From a01f9fdc749437968a48f52c3dde6972d7b74bbb Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 18:30:29 +0200 Subject: [PATCH 07/35] Wire SDFCfg into NewtonCfg and update exports 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. --- .../isaaclab_newton/physics/__init__.pyi | 3 ++- .../isaaclab_newton/physics/newton_manager_cfg.py | 14 +++++++++++++- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi index 1b18da3838e1..45439af1d6ba 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi +++ b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi @@ -11,10 +11,11 @@ __all__ = [ "NewtonCollisionPipelineCfg", "NewtonManager", "NewtonSolverCfg", + "SDFCfg", "XPBDSolverCfg", ] -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 ( FeatherstoneSolverCfg, 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 942a6dc2f49d..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,7 +12,7 @@ from isaaclab.physics import PhysicsCfg from isaaclab.utils import configclass -from .newton_collision_cfg import NewtonCollisionPipelineCfg +from .newton_collision_cfg import NewtonCollisionPipelineCfg, SDFCfg if TYPE_CHECKING: from isaaclab_newton.physics import NewtonManager @@ -257,3 +257,15 @@ class NewtonCfg(PhysicsCfg): .. 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. + """ From 398f666fd8e7185b7eddaeae5c747122ea5d203f Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 18:32:32 +0200 Subject: [PATCH 08/35] Add SDF manager methods for shape preparation 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. --- .../isaaclab_newton/physics/newton_manager.py | 185 ++++++++++++++++++ 1 file changed, 185 insertions(+) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index e7c191aab067..a643e9ef23c3 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 @@ -391,6 +392,190 @@ 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): + """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. + """ + if mesh is None: + return + 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) + + @classmethod + def _create_sdf_collision_from_visual( + cls, builder: ModelBuilder, sdf_shape_indices: set[int], sdf_cfg, res_overrides + ): + """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``. + + 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]) + + 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 sdf_cfg.k_hydro is not None: + shape_cfg_kwargs["is_hydroelastic"] = True + shape_cfg_kwargs["kh"] = sdf_cfg.k_hydro + sdf_shape_cfg = ModelBuilder.ShapeConfig(**shape_cfg_kwargs) + + 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]) + + 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 + + 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 :attr:`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 + + # Compile patterns + body_patterns = [re.compile(p) for p in sdf_cfg.body_patterns] if sdf_cfg.body_patterns else None + shape_patterns = [re.compile(p) for p in sdf_cfg.shape_patterns] if sdf_cfg.shape_patterns else None + res_overrides = ( + [(re.compile(p), r) for p, r in sdf_cfg.pattern_resolutions.items()] + if sdf_cfg.pattern_resolutions + else None + ) + hydro_patterns = None + if sdf_cfg.k_hydro is not None and sdf_cfg.hydroelastic_shape_patterns is not None: + hydro_patterns = [re.compile(p) for p in sdf_cfg.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 + cls._build_sdf_on_mesh(builder.shape_source[si], sdf_cfg, res_overrides, builder.shape_label[si]) + 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 + ) + 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. From 07f71ac2ee840d79ff32c91d07c0c330a516061f Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 18:34:46 +0200 Subject: [PATCH 09/35] Integrate SDF config into manager lifecycle --- .../isaaclab_newton/physics/newton_manager.py | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index a643e9ef23c3..d70c314bfc74 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -709,6 +709,7 @@ def instantiate_builder_from_stage(cls): cls._num_envs = len(env_paths) + cls._apply_sdf_config(builder) cls.set_builder(builder) @classmethod @@ -726,6 +727,17 @@ def _initialize_contacts(cls) -> None: 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() @@ -802,6 +814,21 @@ def initialize_solver(cls) -> None: else: cls._needs_collision_pipeline = True + # 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 + + sdf_cfg = getattr(cfg, "sdf_cfg", None) + 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() From d7e93d15883e55216da1b2db86f8d8c6962cb860 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 18:36:45 +0200 Subject: [PATCH 10/35] Skip convex hull for SDF shapes and apply SDF on cloner prototypes 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. --- .../cloner/newton_replicate.py | 43 ++++++++++++++++++- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py index 59e5d2476ccc..69f255587cf9 100644 --- a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py +++ b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py @@ -5,15 +5,17 @@ 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 @@ -62,6 +64,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 = getattr(cfg, "sdf_cfg", None) if cfg is not None else None + body_pats = [re.compile(x) for x in sdf_cfg.body_patterns] if sdf_cfg and sdf_cfg.body_patterns else None + shape_pats = [re.compile(x) for x in 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 +87,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) From 13508bb2834eb607aa523f575c05d6ac14e79e73 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 18:38:44 +0200 Subject: [PATCH 11/35] Add tests for SDF config and shape preparation 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. --- .../test/physics/test_sdf_config.py | 327 ++++++++++++++++++ 1 file changed, 327 insertions(+) create mode 100644 source/isaaclab_newton/test/physics/test_sdf_config.py 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..a58b3e2c6d31 --- /dev/null +++ b/source/isaaclab_newton/test/physics/test_sdf_config.py @@ -0,0 +1,327 @@ +# 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) From 0d602a6d00fe719ece7dc65d96ddc72eac9ffdc5 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 18:39:34 +0200 Subject: [PATCH 12/35] Add SDF changelog entries and bump to 0.5.12 --- source/isaaclab_newton/config/extension.toml | 2 +- source/isaaclab_newton/docs/CHANGELOG.rst | 21 ++++++++++++++++++++ 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/source/isaaclab_newton/config/extension.toml b/source/isaaclab_newton/config/extension.toml index 9cfd4c5fc105..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.11" +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 ac613c0e3d34..001356c02266 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,27 @@ 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) ~~~~~~~~~~~~~~~~~~~ From 8d24953c38e7d41e83e0646efd7465ffb9489757 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 18:49:03 +0200 Subject: [PATCH 13/35] Remove design spec and plan docs from PR --- .../2026-04-10-newton-sdf-hydroelastic.md | 1106 ----------------- ...0-newton-sdf-hydroelastic-config-design.md | 156 --- 2 files changed, 1262 deletions(-) delete mode 100644 docs/superpowers/plans/2026-04-10-newton-sdf-hydroelastic.md delete mode 100644 docs/superpowers/specs/2026-04-10-newton-sdf-hydroelastic-config-design.md diff --git a/docs/superpowers/plans/2026-04-10-newton-sdf-hydroelastic.md b/docs/superpowers/plans/2026-04-10-newton-sdf-hydroelastic.md deleted file mode 100644 index 73d5e5039ceb..000000000000 --- a/docs/superpowers/plans/2026-04-10-newton-sdf-hydroelastic.md +++ /dev/null @@ -1,1106 +0,0 @@ -# Newton SDF & Hydroelastic Config Implementation Plan - -> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. - -**Goal:** Port SDF collision and hydroelastic shape preparation from PR #5160 into PR #5219's config design, as a new PR built on top of #5219's branch. - -**Architecture:** Add `SDFCfg` configclass for SDF mesh preparation (patterns, resolution, hydroelastic flags). Add manager methods to build SDF on matching shapes before model finalization. Integrate with the Newton cloner to apply SDF on prototypes before replication. - -**Tech Stack:** Python, Newton physics engine, Warp, IsaacLab configclass system - ---- - -### Task 0: Create feature branch - -**Files:** None - -- [ ] **Step 1: Create branch off PR #5219** - -```bash -git checkout expose_newton_collision_pipeline -git checkout -b antoiner/newton-sdf-config -``` - -- [ ] **Step 2: Commit the spec and plan docs** - -```bash -git add docs/superpowers/specs/2026-04-10-newton-sdf-hydroelastic-config-design.md docs/superpowers/plans/2026-04-10-newton-sdf-hydroelastic.md -git commit -m "Add SDF/hydroelastic config design spec and implementation plan" -``` - ---- - -### Task 1: Add `SDFCfg` configclass - -**Files:** -- Modify: `source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py` - -- [ ] **Step 1: Add `SDFCfg` class after `NewtonCollisionPipelineCfg`** - -Add this class at the end of `newton_collision_cfg.py`: - -```python -@configclass -class SDFCfg: - """Configuration for SDF (Signed Distance Field) collision on Newton meshes. - - When provided as :attr:`~isaaclab_newton.physics.NewtonCfg.sdf_cfg`, mesh - collision shapes matching the configured patterns will have SDF built via - Newton's ``mesh.build_sdf()`` API before model finalization. - - At least one of :attr:`max_resolution` or :attr:`target_voxel_size` must be - set for SDF to be built. At least one of :attr:`body_patterns` or - :attr:`shape_patterns` must be set to select which shapes receive SDF. - - .. note:: - For hydroelastic contacts to be generated, shapes must have SDF built - and the ``HYDROELASTIC`` flag set. Set :attr:`k_hydro` to enable - hydroelastic on all matched shapes, or use - :attr:`hydroelastic_shape_patterns` to limit which shapes get the flag. - The pipeline-level hydroelastic processing parameters are configured - separately via - :attr:`NewtonCollisionPipelineCfg.sdf_hydroelastic_config`. - """ - - max_resolution: int | None = None - """Maximum dimension [voxels] for sparse SDF grid (must be divisible by 8). - - Typical values: 128, 256, 512. - """ - - target_voxel_size: float | None = None - """Target voxel size [m] for sparse SDF grid. - - If provided, takes precedence over :attr:`max_resolution`. - """ - - narrow_band_range: tuple[float, float] = (-0.1, 0.1) - """Narrow band distance range (inner, outer) [m] for SDF computation.""" - - margin: float | None = None - """Collision margin [m] for SDF shapes. If ``None``, uses the builder's default.""" - - body_patterns: list[str] | None = None - """Regex patterns to match body labels (USD prim paths) for SDF. - - Bodies whose label matches at least one pattern will have SDF applied to - all their mesh shapes. Example: ``[".*elbow.*", ".*wrist.*"]``. - """ - - shape_patterns: list[str] | None = None - """Regex patterns to match shape labels (USD prim paths) for SDF. - - Only shapes whose label matches at least one pattern get SDF. - Example: ``[".*Gear.*", ".*gear.*"]``. - - .. note:: - At least one of :attr:`body_patterns` or :attr:`shape_patterns` must - be set for SDF to be applied. - """ - - pattern_resolutions: dict[str, int] | None = None - """Per-pattern SDF resolution overrides. - - Maps regex pattern to ``max_resolution`` for matching shapes. Shapes not - matching any pattern use the global :attr:`max_resolution`. First matching - pattern wins. Example: ``{".*elbow.*": 128, ".*power_supply.*": 512}``. - """ - - use_visual_meshes: bool = False - """Whether to create collision shapes from visual meshes for matched bodies - that lack collision geometry. - - When ``False`` (default), only existing collision meshes are patched with - SDF. When ``True``, bodies matching the configured patterns but lacking - collision shapes get a new collision shape created from their first visual - mesh. - """ - - k_hydro: float | None = None - """Hydroelastic stiffness coefficient [Pa] applied to matched shapes. - - If ``None`` (default), the ``HYDROELASTIC`` flag is not set on any shapes. - If set, matched shapes (optionally filtered by - :attr:`hydroelastic_shape_patterns`) get the ``HYDROELASTIC`` flag and - this stiffness value. - - .. note:: - Pipeline-level hydroelastic processing parameters (contact reduction, - buffer sizes, etc.) are configured separately via - :attr:`NewtonCollisionPipelineCfg.sdf_hydroelastic_config`. - """ - - hydroelastic_shape_patterns: list[str] | None = None - """Regex patterns to select which SDF shapes also get hydroelastic contacts. - - If ``None`` and :attr:`k_hydro` is set, all shapes matching the SDF - patterns get hydroelastic. If provided, only shapes whose label matches at - least one pattern get the ``HYDROELASTIC`` flag. - """ -``` - -- [ ] **Step 2: Add missing pipeline params to `HydroelasticSDFCfg`** - -Add these fields to `HydroelasticSDFCfg`, after the existing `output_contact_surface` field: - -```python - moment_matching: bool = False - """Whether to adjust reduced contact friction so net maximum moment matches - the unreduced reference. - - 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. - - Increase if a broadphase overflow warning is issued. - - Defaults to ``1`` (same as Newton's default). - """ - - buffer_mult_iso: int = 1 - """Multiplier for preallocated iso-surface extraction buffers. - - Increase if an iso buffer overflow warning is issued. - - Defaults to ``1`` (same as Newton's default). - """ - - buffer_mult_contact: int = 1 - """Multiplier for the preallocated face contact buffer. - - Increase if a face contact overflow warning is issued. - - Defaults to ``1`` (same as Newton's default). - """ - - grid_size: int = 262144 - """Grid size for hydroelastic contact handling. - - Defaults to ``262144`` (``256 * 8 * 128``, same as Newton's default). - """ -``` - -- [ ] **Step 3: Run pre-commit** - -```bash -./isaaclab.sh -f -``` - -Expected: All checks pass. - -- [ ] **Step 4: Commit** - -```bash -git add source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py -git commit -m "Add SDFCfg configclass and missing HydroelasticSDFCfg fields" -``` - ---- - -### Task 2: Wire `SDFCfg` into `NewtonCfg` and exports - -**Files:** -- Modify: `source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py` -- Modify: `source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi` - -- [ ] **Step 1: Add `sdf_cfg` field to `NewtonCfg`** - -In `newton_manager_cfg.py`, add this import at the top (after the existing `NewtonCollisionPipelineCfg` import): - -```python -from .newton_collision_cfg import NewtonCollisionPipelineCfg, SDFCfg -``` - -Then add this field to the `NewtonCfg` class, after the existing `collision_cfg` field: - -```python - 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. - """ -``` - -- [ ] **Step 2: Update `__init__.pyi`** - -Add `SDFCfg` to `__all__` and the import from `newton_collision_cfg`: - -```python -__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, -) -``` - -- [ ] **Step 3: Run pre-commit** - -```bash -./isaaclab.sh -f -``` - -- [ ] **Step 4: Commit** - -```bash -git add source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi -git commit -m "Wire SDFCfg into NewtonCfg and update exports" -``` - ---- - -### Task 3: Add SDF manager methods - -**Files:** -- Modify: `source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py` - -- [ ] **Step 1: Add `re` import** - -Add `import re` to the imports at the top of `newton_manager.py` (after `import logging`). - -- [ ] **Step 2: Add `_build_sdf_on_mesh` static method** - -Add this method to `NewtonManager`, after the `add_model_change` method (around line 392): - -```python - @staticmethod - def _build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, label: str): - """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. - """ - if mesh is None: - return - 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) -``` - -- [ ] **Step 3: Add `_create_sdf_collision_from_visual` classmethod** - -Add this method right after `_build_sdf_on_mesh`: - -```python - @classmethod - def _create_sdf_collision_from_visual( - cls, builder: ModelBuilder, sdf_shape_indices: set[int], sdf_cfg, res_overrides - ): - """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``. - - 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]) - - 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 sdf_cfg.k_hydro is not None: - shape_cfg_kwargs["is_hydroelastic"] = True - shape_cfg_kwargs["kh"] = sdf_cfg.k_hydro - sdf_shape_cfg = ModelBuilder.ShapeConfig(**shape_cfg_kwargs) - - 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]) - - 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 - - return num_added, num_hydro -``` - -- [ ] **Step 4: Add `_apply_sdf_config` classmethod** - -Add this method right after `_create_sdf_collision_from_visual`: - -```python - @classmethod - def _apply_sdf_config(cls, builder: ModelBuilder): - """Apply SDF collision and optional hydroelastic flags to matching mesh shapes. - - Reads :attr:`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 - - # Compile patterns - body_patterns = [re.compile(p) for p in sdf_cfg.body_patterns] if sdf_cfg.body_patterns else None - shape_patterns = [re.compile(p) for p in sdf_cfg.shape_patterns] if sdf_cfg.shape_patterns else None - res_overrides = ( - [(re.compile(p), r) for p, r in sdf_cfg.pattern_resolutions.items()] - if sdf_cfg.pattern_resolutions - else None - ) - hydro_patterns = None - if sdf_cfg.k_hydro is not None and sdf_cfg.hydroelastic_shape_patterns is not None: - hydro_patterns = [re.compile(p) for p in sdf_cfg.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 - cls._build_sdf_on_mesh(builder.shape_source[si], sdf_cfg, res_overrides, builder.shape_label[si]) - 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 - ) - 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})" - ) -``` - -- [ ] **Step 5: Run pre-commit** - -```bash -./isaaclab.sh -f -``` - -- [ ] **Step 6: Commit** - -```bash -git add source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py -git commit -m "Add SDF manager methods for shape preparation" -``` - ---- - -### Task 4: Integrate SDF into manager lifecycle - -**Files:** -- Modify: `source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py` - -- [ ] **Step 1: Call `_apply_sdf_config` in `instantiate_builder_from_stage`** - -In `instantiate_builder_from_stage()`, add a call to `_apply_sdf_config` just before `cls.set_builder(builder)` (around line 527): - -```python - cls._apply_sdf_config(builder) - cls.set_builder(builder) -``` - -- [ ] **Step 2: Force collision pipeline in `initialize_solver` when SDF is configured** - -In `initialize_solver()`, after the existing `if isinstance(cls._solver, SolverMuJoCo):` / `else:` block that sets `cls._needs_collision_pipeline` (after line 618), add: - -```python - # 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 - - sdf_cfg = getattr(cfg, "sdf_cfg", None) - 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 -``` - -- [ ] **Step 3: Add hydroelastic warning in `_initialize_contacts`** - -In `_initialize_contacts()`, after the pipeline is created (after the `cls._collision_pipeline = CollisionPipeline(...)` lines, around line 542), add a warning check: - -```python - # 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." - ) -``` - -This goes after the `else: cls._collision_pipeline = CollisionPipeline(...)` branch but before the `if cls._contacts is None:` line, so it applies regardless of whether `_collision_cfg` was set or not. The check should be at the same indent level as the `if cls._collision_cfg is not None:` block: - -```python - if cls._needs_collision_pipeline: - # Newton collision pipeline: create pipeline and generate contacts - if cls._collision_pipeline is None: - 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() -``` - -- [ ] **Step 4: Run pre-commit** - -```bash -./isaaclab.sh -f -``` - -- [ ] **Step 5: Commit** - -```bash -git add source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py -git commit -m "Integrate SDF config into manager lifecycle" -``` - ---- - -### Task 5: Cloner integration - -**Files:** -- Modify: `source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py` - -- [ ] **Step 1: Add imports** - -Add `re` and `GeoType` imports at the top of `newton_replicate.py`: - -```python -import re -``` - -(after `from __future__ import annotations`) - -And update the newton import: - -```python -from newton import GeoType, ModelBuilder, solvers -``` - -And add the `PhysicsManager` import: - -```python -from isaaclab.physics import PhysicsManager -``` - -- [ ] **Step 2: Add SDF pattern skip and prototype SDF application** - -In `_build_newton_builder_from_mapping`, after the `env0_pos = positions[0]` line (line 64), add SDF pattern compilation: - -```python - # 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 = getattr(cfg, "sdf_cfg", None) if cfg is not None else None - body_pats = [re.compile(x) for x in sdf_cfg.body_patterns] if sdf_cfg and sdf_cfg.body_patterns else None - shape_pats = [re.compile(x) for x in 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 -``` - -Then replace the existing `if simplify_meshes:` block (line 76-77): - -```python - if simplify_meshes: - p.approximate_meshes("convex_hull", keep_visual_shapes=True) - protos[src_path] = p -``` - -with: - -```python - if simplify_meshes: - 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 -``` - -- [ ] **Step 3: Run pre-commit** - -```bash -./isaaclab.sh -f -``` - -- [ ] **Step 4: Commit** - -```bash -git add source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py -git commit -m "Skip convex hull for SDF shapes and apply SDF on cloner prototypes" -``` - ---- - -### Task 6: Tests - -**Files:** -- Create: `source/isaaclab_newton/test/physics/test_sdf_config.py` - -- [ ] **Step 1: Create test file** - -Create `source/isaaclab_newton/test/physics/test_sdf_config.py` with the following content. Tests use `unittest.mock` to avoid needing a running Newton simulation: - -```python -# 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) - # No crash, no calls - 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) - - # Only elbow shape should have build_sdf called - 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) - - # Both get SDF built - shapes[0]["source"].build_sdf.assert_called_once() - shapes[1]["source"].build_sdf.assert_called_once() - # Only elbow gets hydroelastic - 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) - # No build_sdf calls (box shape has no source to call on) -``` - -- [ ] **Step 2: Run tests** - -```bash -./isaaclab.sh -p -m pytest source/isaaclab_newton/test/physics/test_sdf_config.py -v -``` - -Expected: All tests pass. - -- [ ] **Step 3: Commit** - -```bash -git add source/isaaclab_newton/test/physics/test_sdf_config.py -git commit -m "Add tests for SDF config and shape preparation" -``` - ---- - -### Task 7: Changelog and version bump - -**Files:** -- Modify: `source/isaaclab_newton/docs/CHANGELOG.rst` -- Modify: `source/isaaclab_newton/config/extension.toml` - -- [ ] **Step 1: Bump version to 0.5.12** - -In `source/isaaclab_newton/config/extension.toml`, change: -``` -version = "0.5.11" -``` -to: -``` -version = "0.5.12" -``` - -- [ ] **Step 2: Add new changelog version** - -In `source/isaaclab_newton/docs/CHANGELOG.rst`, add a new version heading before `0.5.11`: - -```rst -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. - - -``` - -- [ ] **Step 3: Run pre-commit** - -```bash -./isaaclab.sh -f -``` - -- [ ] **Step 4: Commit** - -```bash -git add source/isaaclab_newton/docs/CHANGELOG.rst source/isaaclab_newton/config/extension.toml -git commit -m "Add SDF changelog entries and bump to 0.5.12" -``` - ---- - -### Task 8: Final validation - -- [ ] **Step 1: Run all tests** - -```bash -./isaaclab.sh -p -m pytest source/isaaclab_newton/test/physics/test_sdf_config.py -v -``` - -Expected: All tests pass. - -- [ ] **Step 2: Run pre-commit on all files** - -```bash -./isaaclab.sh -f -``` - -Expected: All checks pass. - -- [ ] **Step 3: Verify diff is clean** - -```bash -git diff -git status -``` - -Expected: No unstaged changes. diff --git a/docs/superpowers/specs/2026-04-10-newton-sdf-hydroelastic-config-design.md b/docs/superpowers/specs/2026-04-10-newton-sdf-hydroelastic-config-design.md deleted file mode 100644 index 7be30b84f909..000000000000 --- a/docs/superpowers/specs/2026-04-10-newton-sdf-hydroelastic-config-design.md +++ /dev/null @@ -1,156 +0,0 @@ -# Newton SDF & Hydroelastic Configuration - -**Date:** 2026-04-10 -**Built on:** PR #5219 (`expose_newton_collision_pipeline`) -**Ports from:** PR #5160 (`vidur/feature/sdf-collision`) -**Target:** New PR → `develop` - -## Summary - -Port SDF collision and hydroelastic shape preparation from PR #5160 into -PR #5219's config design. #5219 owns the collision pipeline config layer; -this PR adds the shape-preparation machinery that makes hydroelastic -contacts actually work end-to-end. - -## Design Decisions - -**Keep #5219's config hierarchy.** `HydroelasticSDFCfg` (pipeline processing -params) stays nested under `NewtonCollisionPipelineCfg.sdf_hydroelastic_config`. - -**Add `SDFCfg` as a new top-level field on `NewtonCfg`.** Shape-level concerns -(which meshes get SDF, resolution, hydroelastic flag enablement) live here, -separate from pipeline configuration. - -**Flatten hydroelastic shape enablement into `SDFCfg`.** #5160 used a nested -`HydroelasticCfg` object. We put `k_hydro` and `hydroelastic_shape_patterns` -directly on `SDFCfg` since they're simple fields that control shape flags, -not a separate subsystem. - -## Config Hierarchy - -``` -NewtonCfg -├── solver_cfg: NewtonSolverCfg -├── collision_cfg: NewtonCollisionPipelineCfg | None -│ ├── broad_phase, reduce_contacts, rigid_contact_max, ... -│ └── sdf_hydroelastic_config: HydroelasticSDFCfg | None -│ ├── reduce_contacts, normal_matching, anchor_contact -│ ├── moment_matching, margin_contact_area -│ ├── buffer_fraction, buffer_mult_broad/iso/contact -│ ├── grid_size, output_contact_surface -│ └── (maps 1:1 to HydroelasticSDF.Config via to_pipeline_args()) -└── sdf_cfg: SDFCfg | None - ├── max_resolution: int | None - ├── target_voxel_size: float | None - ├── narrow_band_range: tuple[float, float] - ├── margin: float | None - ├── body_patterns: list[str] | None - ├── shape_patterns: list[str] | None - ├── pattern_resolutions: dict[str, int] | None - ├── use_visual_meshes: bool - ├── k_hydro: float (shape-level stiffness) - └── hydroelastic_shape_patterns: list[str] | None -``` - -## Components - -### 1. Config additions (`newton_collision_cfg.py`) - -**`SDFCfg`** — new configclass in the existing file. - -Fields ported from #5160's `SDFCfg`: -- `max_resolution`, `target_voxel_size`, `narrow_band_range`, `margin` -- `body_patterns`, `shape_patterns`, `pattern_resolutions` -- `use_visual_meshes` - -Shape-level hydroelastic fields (flattened from #5160's `HydroelasticCfg`): -- `k_hydro: float = 1e10` — stiffness applied to shapes via `shape_material_kh` -- `hydroelastic_shape_patterns: list[str] | None = None` — if None, all - SDF shapes get HYDROELASTIC flag; if set, only matching shapes - -**`HydroelasticSDFCfg`** — add missing pipeline params from #5160: -- `moment_matching: bool = False` -- `buffer_mult_broad: int = 1` -- `buffer_mult_iso: int = 1` -- `buffer_mult_contact: int = 1` -- `grid_size: int = 256 * 8 * 128` - -**`NewtonCfg`** — add `sdf_cfg: SDFCfg | None = None` in `newton_manager_cfg.py`. - -### 2. Manager methods (`newton_manager.py`) - -**`_build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, label)`** — static method. -Builds SDF on a mesh. Clears existing SDF first. Applies per-pattern -resolution overrides. Passes `narrow_band_range`, `max_resolution`, -`target_voxel_size` to `mesh.build_sdf()`. - -**`_apply_sdf_config(builder)`** — classmethod. -1. Read `sdf_cfg` from `PhysicsManager._cfg`; return early if None. -2. Validate that at least one of `max_resolution`/`target_voxel_size` is set. -3. Compile body/shape/hydroelastic regex patterns. -4. Collect matching shape indices from builder. -5. For each matching collision shape: build SDF, optionally set HYDROELASTIC - flag + `k_hydro`. -6. If `use_visual_meshes`, call `_create_sdf_collision_from_visual()`. -7. Log summary. - -**`_create_sdf_collision_from_visual(builder, sdf_shape_indices, sdf_cfg, res_overrides)`** -— classmethod. For matched bodies that lack collision geometry, creates a -collision shape from the first visual mesh with SDF built on it. - -**`initialize_solver()` changes:** -- After determining `_needs_collision_pipeline`, force it `True` when - `collision_cfg is not None` or when `sdf_cfg` has valid patterns + resolution. -- Log a warning when overriding. - -**`_initialize_contacts()` changes:** -- After creating pipeline, if hydroelastic was configured - (`collision_cfg.sdf_hydroelastic_config is not None`) but - `pipeline.hydroelastic_sdf is None`, log a warning. - -**`instantiate_builder_from_stage()` change:** -- Call `cls._apply_sdf_config(builder)` before `cls.set_builder(builder)`. - -### 3. Cloner integration (`newton_replicate.py`) - -In `_build_newton_builder_from_mapping()`: -1. Read `sdf_cfg` from `PhysicsManager._cfg`. -2. Compile body/shape patterns if present. -3. When `simplify_meshes` is True and SDF patterns exist, skip convex hull - approximation for shapes matching SDF patterns (preserves triangle meshes). -4. After prototype building, call `NewtonManager._apply_sdf_config(prototype)` - on each prototype before `add_builder` replication. - -### 4. Exports (`__init__.pyi`) - -Add `SDFCfg` to `__all__` and import list. - -### 5. Tests (`test_sdf_config.py`) - -Port from #5160, adapted for new config structure: -- `TestBuildSdfOnMesh` — None mesh, max_resolution, clear existing SDF, - target_voxel_size precedence, pattern resolution overrides -- `TestApplySdfConfig` — shape index collection by body/shape patterns, - hydroelastic flag setting, visual mesh fallback, edge cases - -### 6. Changelog - -Add to existing `0.5.11` entry (or bump to `0.5.12`): -- Added `SDFCfg` for SDF mesh collision configuration -- Added SDF pattern skip in Newton cloner -- Added missing hydroelastic pipeline params to `HydroelasticSDFCfg` - -## Execution Order - -1. Config additions (SDFCfg, HydroelasticSDFCfg fields, NewtonCfg.sdf_cfg) -2. Manager methods (_build_sdf_on_mesh, _apply_sdf_config, _create_sdf_collision_from_visual) -3. Manager integration (initialize_solver, _initialize_contacts, instantiate_builder_from_stage) -4. Cloner integration (newton_replicate.py) -5. Exports (__init__.pyi) -6. Tests -7. Changelog + version bump - -## Out of Scope - -- Visualizer additions from #5160 (newton_visualizer.py changes) -- Runtime SDF rebuild / dynamic pattern updates From 9d630a51bef325d90d720ea7ade966a30f25fd4d Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 19:00:07 +0200 Subject: [PATCH 14/35] Fix docstring/code contradiction and remove dead code 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. --- .../isaaclab_newton/physics/newton_collision_cfg.py | 9 ++++++--- .../isaaclab_newton/physics/newton_manager.py | 6 +----- 2 files changed, 7 insertions(+), 8 deletions(-) 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 dd502aad7204..35a0094911ef 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -241,8 +241,10 @@ class SDFCfg: max_resolution: int | None = None """Maximum voxel dimension for the SDF grid. - Must be divisible by 8. Typical values: 128, 256, 512. - Ignored when :attr:`target_voxel_size` is set. + 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``. """ @@ -250,7 +252,8 @@ class SDFCfg: target_voxel_size: float | None = None """Target voxel size [m] for the SDF grid. - When set, takes precedence over :attr:`max_resolution`. + 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``. """ diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index d70c314bfc74..80d2bff72ea3 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -814,11 +814,7 @@ def initialize_solver(cls) -> None: else: cls._needs_collision_pipeline = True - # 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 - + # Force Newton pipeline when SDF is configured sdf_cfg = getattr(cfg, "sdf_cfg", None) has_sdf = ( sdf_cfg is not None From 87af9c5cfea73aac11d63e34387f821ac4e06dbe Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 19:03:21 +0200 Subject: [PATCH 15/35] Address important review findings - 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 --- .../cloner/newton_replicate.py | 17 +++- .../isaaclab_newton/physics/newton_manager.py | 93 ++++++++++++------- 2 files changed, 74 insertions(+), 36 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py index 69f255587cf9..0e166eb5afdd 100644 --- a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py +++ b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py @@ -21,6 +21,17 @@ 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], @@ -70,9 +81,9 @@ def _build_newton_builder_from_mapping( # 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 = getattr(cfg, "sdf_cfg", None) if cfg is not None else None - body_pats = [re.compile(x) for x in sdf_cfg.body_patterns] if sdf_cfg and sdf_cfg.body_patterns else None - shape_pats = [re.compile(x) for x in sdf_cfg.shape_patterns] if sdf_cfg and sdf_cfg.shape_patterns else None + 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] = {} diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 80d2bff72ea3..de4d8d52eedf 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -393,7 +393,7 @@ def add_model_change(cls, change: SolverNotifyFlags) -> None: cls._model_changes.add(change) @staticmethod - def _build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, label: str): + def _build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, label: str) -> bool: """Build SDF on a mesh, resolving per-pattern resolution overrides. Args: @@ -401,9 +401,13 @@ def _build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, label: str): 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: - return + 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 @@ -418,10 +422,11 @@ def _build_sdf_on_mesh(mesh, sdf_cfg, res_overrides, label: str): 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 + 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. @@ -430,6 +435,8 @@ def _create_sdf_collision_from_visual( 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. @@ -442,19 +449,6 @@ def _create_sdf_collision_from_visual( if builder.shape_flags[si] & ShapeFlags.COLLIDE_SHAPES and builder.shape_body[si] in matched_bodies: bodies_with_collision.add(builder.shape_body[si]) - 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 sdf_cfg.k_hydro is not None: - shape_cfg_kwargs["is_hydroelastic"] = True - shape_cfg_kwargs["kh"] = sdf_cfg.k_hydro - sdf_shape_cfg = ModelBuilder.ShapeConfig(**shape_cfg_kwargs) - num_added = 0 num_hydro = 0 for body_idx in matched_bodies - bodies_with_collision: @@ -471,17 +465,35 @@ def _create_sdf_collision_from_visual( 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=sdf_shape_cfg, + cfg=ModelBuilder.ShapeConfig(**shape_cfg_kwargs), label=f"{body_lbl}/sdf_collision", ) num_added += 1 - if sdf_cfg.k_hydro is not None: + if enable_hydro: num_hydro += 1 return num_added, num_hydro @@ -490,7 +502,7 @@ def _create_sdf_collision_from_visual( def _apply_sdf_config(cls, builder: ModelBuilder): """Apply SDF collision and optional hydroelastic flags to matching mesh shapes. - Reads :attr:`SDFCfg` from the active physics config. Collects 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`. @@ -502,7 +514,7 @@ def _apply_sdf_config(cls, builder: ModelBuilder): cfg = PhysicsManager._cfg if cfg is None: return - sdf_cfg = getattr(cfg, "sdf_cfg", None) + sdf_cfg = cfg.sdf_cfg # type: ignore[union-attr] if sdf_cfg is None: return @@ -510,17 +522,31 @@ def _apply_sdf_config(cls, builder: ModelBuilder): logger.warning("SDFCfg provided but neither max_resolution nor target_voxel_size is set. SDF disabled.") return - # Compile patterns - body_patterns = [re.compile(p) for p in sdf_cfg.body_patterns] if sdf_cfg.body_patterns else None - shape_patterns = [re.compile(p) for p in sdf_cfg.shape_patterns] if sdf_cfg.shape_patterns else None - res_overrides = ( - [(re.compile(p), r) for p, r in sdf_cfg.pattern_resolutions.items()] - if sdf_cfg.pattern_resolutions - else None - ) + # 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 and sdf_cfg.hydroelastic_shape_patterns is not None: - hydro_patterns = [re.compile(p) for p in sdf_cfg.hydroelastic_shape_patterns] + 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.") @@ -551,7 +577,8 @@ def _apply_sdf_config(cls, builder: ModelBuilder): for si in sdf_shape_indices: if not (builder.shape_flags[si] & ShapeFlags.COLLIDE_SHAPES): continue - cls._build_sdf_on_mesh(builder.shape_source[si], sdf_cfg, res_overrides, builder.shape_label[si]) + 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: @@ -566,7 +593,7 @@ def _apply_sdf_config(cls, builder: ModelBuilder): 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 + builder, sdf_shape_indices, sdf_cfg, res_overrides, hydro_patterns ) num_hydro += hydro_from_visual @@ -815,7 +842,7 @@ def initialize_solver(cls) -> None: cls._needs_collision_pipeline = True # Force Newton pipeline when SDF is configured - sdf_cfg = getattr(cfg, "sdf_cfg", None) + 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) From 1d09ba155193bb9ec5ab9fc89e34a06863f75d3f Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 19:04:59 +0200 Subject: [PATCH 16/35] Add tests for _create_sdf_collision_from_visual --- .../test/physics/test_sdf_config.py | 160 ++++++++++++++++++ 1 file changed, 160 insertions(+) diff --git a/source/isaaclab_newton/test/physics/test_sdf_config.py b/source/isaaclab_newton/test/physics/test_sdf_config.py index a58b3e2c6d31..b2ed547753e8 100644 --- a/source/isaaclab_newton/test/physics/test_sdf_config.py +++ b/source/isaaclab_newton/test/physics/test_sdf_config.py @@ -325,3 +325,163 @@ def test_non_mesh_shapes_skipped(self): 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 From 1e08da89666be3aff1a1a1b9a59c7b26f6ff98e4 Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Thu, 21 May 2026 15:34:47 +0200 Subject: [PATCH 17/35] isaaclab_newton: add collision_decimation knob for mid-tick re-collide MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ## Summary - Adds ``NewtonCfg.collision_decimation`` (default ``0`` = legacy one-collide-per-tick behaviour). - When positive, ``_run_solver_substeps`` re-invokes the collision pipeline every ``collision_decimation`` substeps so contact normals reflect the bodies' just-integrated poses instead of the poses from the top of the tick. - The last substep is intentionally skipped — its contact set would only affect the next tick, which the top-of-tick ``collide()`` in ``_simulate_physics_only`` / ``_simulate_full`` owns. - ``NewtonCfg.__post_init__`` warns when ``collision_decimation >= num_substeps`` (silently equivalent to ``0``). ## Test plan - [x] ``./isaaclab.sh -p -m pytest source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py`` — 39/39 pass - [x] ``./isaaclab.sh -f`` — pre-commit clean --- .../isaaclab_newton/physics/newton_manager.py | 13 ++++++++++++- .../isaaclab_newton/physics/newton_manager_cfg.py | 15 +++++++++++++++ 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 500d07b726e7..21a37d45d0b7 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -199,6 +199,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 @@ -1218,6 +1219,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 +1441,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 +1462,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: 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..feb9b28db386 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py @@ -7,6 +7,7 @@ from __future__ import annotations +import logging from typing import TYPE_CHECKING from isaaclab.physics import PhysicsCfg @@ -17,6 +18,8 @@ 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.""" @@ -149,3 +155,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, + ) From c2df943719adf3523bb4ab99c9b31d94ce10181a Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Thu, 21 May 2026 16:11:44 +0200 Subject: [PATCH 18/35] isaaclab_newton: tests + changelog for collision_decimation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ## Summary - Adds ``test_newton_cfg_collision_decimation_warning`` (parametrized over ``(num_substeps, collision_decimation)``) for the ``__post_init__`` warning gate. - Adds ``test_collision_decimation_invokes_mid_loop_collide`` that wraps ``NewtonManager._collision_pipeline.collide`` with a counter, runs one physics tick with a sphere falling onto a ground plane, and asserts the call count equals ``1 + floor((num_substeps - 1) / collision_decimation)``. - Adds ``source/isaaclab_newton/changelog.d/mzamoramora-collision-decimation.minor.rst``. ## Test plan - [x] ``./isaaclab.sh -p -m pytest source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py`` — 39/39 pass (28 existing + 11 new) - [x] ``./isaaclab.sh -f`` — pre-commit clean --- ...mzamoramora-collision-decimation.minor.rst | 13 +++ .../test_newton_manager_abstraction.py | 88 +++++++++++++++++++ 2 files changed, 101 insertions(+) create mode 100644 source/isaaclab_newton/changelog.d/mzamoramora-collision-decimation.minor.rst 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/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 From a028b0eb5297b9418d8d92a9f6b2396ce8e0f395 Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Tue, 12 May 2026 14:10:34 +0200 Subject: [PATCH 19/35] Newton: fix RigidObjectData shape for kinematic single-body assets MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ``RigidObjectData._create_simulation_bindings`` crashed with ``IndexError: tuple index out of range`` whenever the rigid object was fixed-base AND its root view only contained a single body — e.g. a kinematic-enabled rigid object built from a USD that authors a ``PhysicsFixedJoint``. Newton's ``get_root_transforms`` returns a 2D ``(count, links)`` array in that scenario, but the legacy ``is_fixed_base`` branch unconditionally indexed ``[:, 0, 0]`` assuming a 3D ``(count, links, 1)`` layout. Dispatch on actual ``ndim`` so both fixed-base multi-link articulations and single-body kinematic rigid objects are handled. ``_create_buffers`` similarly allocated the no-velocity fallback for ``_sim_bind_body_com_vel_w`` as 1D, which the ``derive_body_acceleration_from_body_com_velocities`` kernel rejected at launch time (expects 2D). Allocate it as ``(num_instances, 1)`` to match the kernel signature. --- ...mzamoramora-rigid-object-kinematic-fix.rst | 16 ++++++++++ .../assets/rigid_object/rigid_object_data.py | 32 ++++++++++++------- 2 files changed, 37 insertions(+), 11 deletions(-) create mode 100644 source/isaaclab_newton/changelog.d/mzamoramora-rigid-object-kinematic-fix.rst 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) From e0f975534ae8b815fd0c6806a0a0846be1220495 Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Tue, 26 May 2026 14:41:17 +0200 Subject: [PATCH 20/35] newton_manager: add sanitize_world_state hook for NaN recovery Add classmethod `NewtonManager.sanitize_world_state(env_ids)` that zeroes per-world MJWarp solver scratch (qacc_warmstart, qfrc_*, cacc, cfrc_*) and Newton State velocity/force buffers (joint_qd, body_qd, body_f, body_qdd, body_parent_f) for the given worlds, then runs eval_fk to re-derive body_q. Generalizes the in-tree Factory NaN scrubber so any task on Newton can recover NaN-divergent worlds in place via: if has_nan_envs: NewtonManager.sanitize_world_state(nan_env_ids) Compared to mjwarp.reset_data: closes the gap on derived qfrc_* / cacc / cfrc_int / cfrc_ext that reset_data doesn't touch. cfrc_int in particular is read-modify-written via wp.atomic_add in mujoco_warp/_src/smooth.py:_cfrc_backward, so a stale NaN there survives the next rne() call and re-contaminates qfrc_bias. No-op for solvers without mjw_data (XPBD, Featherstone, Kamino) since those don't exhibit the MuJoCo warm-start contamination pattern. --- .../isaaclab_newton/physics/newton_manager.py | 141 ++++++++++++++++++ 1 file changed, 141 insertions(+) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 5b33a5f5c02c..77d3fc6c7835 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -15,6 +15,7 @@ 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). @@ -232,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 @@ -679,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 @@ -1687,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 # ------------------------------------------------------------------ From 024a126cede3198104a8093beadd3c9e3daaa658 Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Tue, 26 May 2026 14:40:30 +0200 Subject: [PATCH 21/35] newton_manager: eval_fk before mid-tick re-collide MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Without this, mid-loop cls._collision_pipeline.collide reads stale state.body_q from the previous solver step — accurate for most collision pairs, but for SDF/hydroelastic contact normals on fast-moving fingers it produces visibly wrong penetrations. Re-deriving body_q from joint_q before the re-collide adds one eval_fk per collision_decimation interval, which is cheap relative to the contact solve. --- .../isaaclab_newton/isaaclab_newton/physics/newton_manager.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 77d3fc6c7835..7369da927107 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -1664,6 +1664,7 @@ def _run_solver_substeps(cls, contacts) -> None: 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: + eval_fk(cls._model, cls._state_0.joint_q, cls._state_0.joint_qd, cls._state_0, None) cls._collision_pipeline.collide(cls._state_0, contacts) else: cfg = PhysicsManager._cfg @@ -1676,6 +1677,7 @@ def _run_solver_substeps(cls, contacts) -> None: 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: + eval_fk(cls._model, cls._state_0.joint_q, cls._state_0.joint_qd, cls._state_0, None) cls._collision_pipeline.collide(cls._state_0, contacts) @classmethod From 38f8117b6855099196e33735f1f9ebc05895efb8 Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Fri, 15 May 2026 12:49:21 +0200 Subject: [PATCH 22/35] Factory nut_thread Newton support (rebased on develop, PR #5400 accessors) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Newton dep: newton[sim]@v1.2.0 (was @v1.2.0rc2 with hand-pinned mujoco / mujoco-warp; [sim] extras now supply both). - NewtonCfg.collision_substeps: re-collide every N substeps within a tick. Default 0 keeps current behavior. Used for nut_thread with num_substeps=8 / collision_substeps=2. - HydroelasticSDFCfg.moment_matching: PhysX patch-friction analog. - factory_env: Newton OSC path reads body_link_jacobian_w + mass_matrix from PR #5400 accessors instead of the v02 hand-rolled FK+eval_mass_matrix glue. Velocity stays Newton-specific (data adapter zeros fingertip vel on the zero-mass virtual body). - factory_control_newton: trimmed to just clamp_to_effort_limits — the rest (NewtonOSCBuffers, build_buffers, compute_arm_jacobian/mass_matrix, IK solver) was superseded by PR #5400. - factory_control: gate the null-space term on cfg.ctrl.disable_nullspace. - factory_env_cfg / factory_newton_setup: Newton preset (MJWarp, hydro SDF contacts), num_substeps=8, collision_substeps=2, sim.dt=1/120, decimation=8. --- .../changelog.d/nut-thread-newton.rst | 6 + .../physics/newton_collision_cfg.py | 18 +- .../changelog.d/nut-thread-newton.minor.rst | 15 + .../direct/factory/factory_control.py | 35 +- .../direct/factory/factory_control_newton.py | 46 ++ .../direct/factory/factory_env.py | 279 ++++++++-- .../direct/factory/factory_env_cfg.py | 112 +++- .../direct/factory/factory_newton_setup.py | 487 ++++++++++++++++++ 8 files changed, 921 insertions(+), 77 deletions(-) create mode 100644 source/isaaclab_newton/changelog.d/nut-thread-newton.rst create mode 100644 source/isaaclab_tasks/changelog.d/nut-thread-newton.minor.rst create mode 100644 source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control_newton.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py diff --git a/source/isaaclab_newton/changelog.d/nut-thread-newton.rst b/source/isaaclab_newton/changelog.d/nut-thread-newton.rst new file mode 100644 index 000000000000..5b5123347f81 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/nut-thread-newton.rst @@ -0,0 +1,6 @@ +Added +^^^^^ + +* Added :attr:`~isaaclab_newton.physics.HydroelasticSDFCfg.moment_matching` + to surface Newton's PhysX-patch-friction analog on the IsaacLab + config side. 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 72c33403127d..40b02eabb9cf 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -57,6 +57,16 @@ class HydroelasticSDFCfg: Defaults to ``False`` (same as Newton's default). """ + moment_matching: bool = False + """Whether to redistribute reduced contact forces to preserve moment balance per normal bin. + + PhysX patch-friction analog: keeps friction torque per normal bin under + contact reduction so a held object doesn't spin out under asymmetric + pinch. 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. @@ -69,14 +79,6 @@ 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. diff --git a/source/isaaclab_tasks/changelog.d/nut-thread-newton.minor.rst b/source/isaaclab_tasks/changelog.d/nut-thread-newton.minor.rst new file mode 100644 index 000000000000..a6bd3a538ac9 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/nut-thread-newton.minor.rst @@ -0,0 +1,15 @@ +Added +^^^^^ + +* Added Newton-backend support to :class:`~isaaclab_tasks.direct.factory.FactoryEnv`. + Run ``Isaac-Factory-*-Direct-v0`` tasks under the Newton physics backend with + ``presets=newton``; the existing PhysX path is unchanged. New helper modules: + + * :mod:`isaaclab_tasks.direct.factory.factory_control_newton` adapts + :class:`newton.selection.ArticulationView` to the J/M shapes the + Factory OSC math (``factory_control.compute_dof_torque``) expects, and + folds ``joint_armature`` into the mass-matrix diagonal. + * :mod:`isaaclab_tasks.direct.factory.factory_newton_setup` houses + procedural Newton-only post-load asset patches (POSITION-mode override, + parser-body gravity compensation, finger hydroelastic SDFs, contact-stack + tuning). diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control.py index a43b7ffcc743..138d33ba1b50 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control.py @@ -70,29 +70,30 @@ def compute_dof_torque( task_wrench.sign() * (task_wrench.abs() - dead_zone_thresholds), ) - # Set tau = J^T * tau, i.e., map tau into joint space as desired - jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) - dof_torque[:, 0:7] = (jacobian_T @ task_wrench.unsqueeze(-1)).squeeze(-1) - # adapted from roboticsproceedings.org/rss07/p31.pdf # useful tensors - arm_mass_matrix_inv = torch.inverse(arm_mass_matrix) jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + arm_mass_matrix_inv = torch.inverse(arm_mass_matrix) arm_mass_matrix_task = torch.inverse( - jacobian @ torch.inverse(arm_mass_matrix) @ jacobian_T + jacobian @ arm_mass_matrix_inv @ jacobian_T ) # ETH eq. 3.86; geometric Jacobian is assumed - j_eef_inv = arm_mass_matrix_task @ jacobian @ arm_mass_matrix_inv - default_dof_pos_tensor = torch.tensor(cfg.ctrl.default_dof_pos_tensor, device=device).repeat((num_envs, 1)) - # nullspace computation - distance_to_default_dof_pos = default_dof_pos_tensor - dof_pos[:, :7] - distance_to_default_dof_pos = (distance_to_default_dof_pos + math.pi) % ( - 2 * math.pi - ) - math.pi # normalize to [-pi, pi] - u_null = cfg.ctrl.kd_null * -dof_vel[:, :7] + cfg.ctrl.kp_null * distance_to_default_dof_pos - u_null = arm_mass_matrix @ u_null.unsqueeze(-1) - torque_null = (torch.eye(7, device=device).unsqueeze(0) - torch.transpose(jacobian, 1, 2) @ j_eef_inv) @ u_null - dof_torque[:, 0:7] += torque_null.squeeze(-1) + + # Set tau = J^T * tau, i.e., map tau into joint space as desired + dof_torque[:, 0:7] = (jacobian_T @ task_wrench.unsqueeze(-1)).squeeze(-1) + + if not getattr(cfg.ctrl, "disable_nullspace", False): + j_eef_inv = arm_mass_matrix_task @ jacobian @ arm_mass_matrix_inv + default_dof_pos_tensor = torch.tensor(cfg.ctrl.default_dof_pos_tensor, device=device).repeat((num_envs, 1)) + # nullspace computation + distance_to_default_dof_pos = default_dof_pos_tensor - dof_pos[:, :7] + distance_to_default_dof_pos = (distance_to_default_dof_pos + math.pi) % ( + 2 * math.pi + ) - math.pi # normalize to [-pi, pi] + u_null = cfg.ctrl.kd_null * -dof_vel[:, :7] + cfg.ctrl.kp_null * distance_to_default_dof_pos + u_null = arm_mass_matrix @ u_null.unsqueeze(-1) + torque_null = (torch.eye(7, device=device).unsqueeze(0) - jacobian_T @ j_eef_inv) @ u_null + dof_torque[:, 0:7] += torque_null.squeeze(-1) # TODO: Verify it's okay to no longer do gripper control here. dof_torque = torch.clamp(dof_torque, min=-100.0, max=100.0) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control_newton.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control_newton.py new file mode 100644 index 000000000000..6082d9fc6b65 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control_newton.py @@ -0,0 +1,46 @@ +# 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 + +"""Factory: Newton-specific control helpers. + +Backend-agnostic OSC kinematics + dynamics (Jacobian, mass matrix) now flow +through :class:`BaseArticulationData` accessors added in PR #5400 +(``body_link_jacobian_w``, ``mass_matrix``); the only Factory-specific Newton +glue still needed is the per-joint effort clamp, since Newton's direct +``joint_f`` write does not enforce ``effort_limit_sim`` the way PhysX's +articulation drive does. +""" + +from __future__ import annotations + +import torch + +# Franka FR3 per-joint torque limits [N·m] (datasheet symmetric). +FRANKA_FR3_EFFORT_LIMITS: tuple[float, ...] = (87.0, 87.0, 87.0, 87.0, 12.0, 12.0, 12.0) + + +def clamp_to_effort_limits( + dof_torque: torch.Tensor, limits: tuple[float, ...] = FRANKA_FR3_EFFORT_LIMITS +) -> torch.Tensor: + """Per-joint elementwise torque clamp [N·m]. + + PhysX's articulation drive enforces ``effort_limit_sim`` automatically. + Newton does not on direct ``joint_f`` writes, so the Newton path applies + the clamp explicitly here. The first ``len(limits)`` columns of + ``dof_torque`` are clamped in-place against the symmetric limits; + remaining columns (e.g. gripper DOFs) are left untouched. + + Args: + dof_torque: ``(num_envs, num_dofs)`` torch tensor on any device. + limits: Per-DOF symmetric clamp values, one per arm DOF. Defaults + to :data:`FRANKA_FR3_EFFORT_LIMITS`. + + Returns: + The same ``dof_torque`` tensor with arm columns clamped in-place. + """ + n = len(limits) + lim = torch.as_tensor(limits, device=dof_torque.device, dtype=dof_torque.dtype) + dof_torque[..., :n] = torch.clamp(dof_torque[..., :n], min=-lim, max=lim) + return dof_torque diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py index c38fe071b161..80d7b3282c2a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py @@ -9,7 +9,7 @@ import carb import isaaclab.sim as sim_utils -from isaaclab.assets import Articulation +from isaaclab.assets import Articulation, RigidObject, RigidObjectCfg from isaaclab.envs import DirectRLEnv from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane from isaaclab.utils import math as torch_utils @@ -19,6 +19,48 @@ from .factory_env_cfg import OBS_DIM_CFG, STATE_DIM_CFG, FactoryEnvCfg +def _set_sim_gravity(cfg: FactoryEnvCfg, gravity: tuple[float, float, float]) -> None: + """Set the live simulator gravity vector, dispatching by backend. + + Factory's reset path temporarily zeroes gravity to settle assets, then + restores the cfg value. PhysX exposes ``physics_sim_view.set_gravity`` + via ``carb.Float3``; Newton has no equivalent on ``physics_sim_view`` + (which is a plain list of registered views), but exposes + ``NewtonManager._model.set_gravity`` directly. This helper hides the + difference at the two Factory call sites. + """ + if _is_newton_backend(cfg): + from isaaclab_newton.physics import NewtonManager + from newton.solvers import SolverNotifyFlags + + if NewtonManager._model is not None: + NewtonManager._model.set_gravity(gravity) + # Newton's ``set_gravity`` only updates the host-side model; the + # active solver (e.g. mjwarp) keeps the previous gravity inside + # its ``opt.gravity`` device buffer until ``notify_model_changed`` + # re-uploads model properties. Without this, "disable gravity" + # in the debug panel has no visible effect during stepping. + NewtonManager.add_model_change(SolverNotifyFlags.MODEL_PROPERTIES) + return + physics_sim_view = sim_utils.SimulationContext.instance().physics_sim_view + physics_sim_view.set_gravity(carb.Float3(*gravity)) + + +def _is_newton_backend(cfg: FactoryEnvCfg) -> bool: + """Return True when the cfg has been resolved to the Newton physics backend. + + PresetCfg resolution swaps :attr:`FactoryEnvCfg.sim.physics` for either a + :class:`~isaaclab_physx.physics.PhysxCfg` or :class:`~isaaclab_newton.physics.NewtonCfg` + instance before the env is constructed. We dispatch on the runtime type of + ``cfg.sim.physics`` to avoid hard-importing :mod:`isaaclab_newton` on the + PhysX path. + """ + physics = getattr(cfg.sim, "physics", None) + if physics is None: + return False + return type(physics).__module__.startswith("isaaclab_newton") + + class FactoryEnv(DirectRLEnv): cfg: FactoryEnvCfg @@ -29,13 +71,30 @@ def __init__(self, cfg: FactoryEnvCfg, render_mode: str | None = None, **kwargs) cfg.observation_space += cfg.action_space cfg.state_space += cfg.action_space self.cfg_task = cfg.task + # Cache once: every other method reads ``self._is_newton`` instead of + # re-running the predicate. + self._is_newton = _is_newton_backend(cfg) + + if self._is_newton: + from . import factory_newton_setup + + factory_newton_setup.apply_cfg_overrides(cfg) super().__init__(cfg, render_mode, **kwargs) - factory_utils.set_body_inertias(self._robot, self.scene.num_envs) + # ``factory_utils.set_body_inertias`` / ``set_friction`` reach into + # PhysX's ``root_view`` API, which Newton's adapter doesn't expose; the + # friction value from the spawn cfg / USD is in effect on Newton. + if not self._is_newton: + factory_utils.set_body_inertias(self._robot, self.scene.num_envs) self._init_tensors() self._set_default_dynamics_parameters() + if self._is_newton: + from . import factory_newton_setup + + factory_newton_setup.warm_up_kernels(self) + def _set_default_dynamics_parameters(self): """Set parameters defining dynamic interactions.""" self.default_gains = torch.tensor(self.cfg.ctrl.default_task_prop_gains, device=self.device).repeat( @@ -49,10 +108,11 @@ def _set_default_dynamics_parameters(self): (self.num_envs, 1) ) - # Set masses and frictions. - factory_utils.set_friction(self._held_asset, self.cfg_task.held_asset_cfg.friction, self.scene.num_envs) - factory_utils.set_friction(self._fixed_asset, self.cfg_task.fixed_asset_cfg.friction, self.scene.num_envs) - factory_utils.set_friction(self._robot, self.cfg_task.robot_cfg.friction, self.scene.num_envs) + # Set masses and frictions. See note in ``__init__`` on Newton. + if not self._is_newton: + factory_utils.set_friction(self._held_asset, self.cfg_task.held_asset_cfg.friction, self.scene.num_envs) + factory_utils.set_friction(self._fixed_asset, self.cfg_task.fixed_asset_cfg.friction, self.scene.num_envs) + factory_utils.set_friction(self._robot, self.cfg_task.robot_cfg.friction, self.scene.num_envs) def _init_tensors(self): """Initialize tensors once.""" @@ -85,18 +145,41 @@ def _setup_scene(self): """Initialize simulation scene.""" spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg(), translation=(0.0, 0.0, -1.05)) - # spawn a usd file of a table into the scene - cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd") - cfg.func( - "/World/envs/env_.*/Table", cfg, translation=(0.55, 0.0, 0.0), orientation=(0.0, 0.0, 0.70711, 0.70711) - ) + # Newton: spawn a thin kinematic cuboid the size of the table top. + # The instanceable Seattle-lab-table USD has no + # ``UsdPhysics.RigidBodyAPI`` on its root so it can't be wrapped as a + # ``RigidObjectCfg`` directly. ``MeshCuboidCfg`` (not ``CuboidCfg``) so + # ``_build_collision_sdfs`` can bake a voxel SDF for "Show Collision". + # Top surface at z=0; bolt init_state.pos.z is remapped to match. + if self._is_newton: + table_cfg = RigidObjectCfg( + prim_path="/World/envs/env_.*/Table", + spawn=sim_utils.MeshCuboidCfg( + size=(1.2, 0.6, 0.04), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.55, 0.0, -0.02), rot=(1.0, 0.0, 0.0, 0.0)), + ) + self._table = RigidObject(table_cfg) + else: + # PhysX path: keep the original Seattle-lab-table USD spawn. + cfg = sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd" + ) + cfg.func( + "/World/envs/env_.*/Table", cfg, translation=(0.55, 0.0, 0.0), orientation=(0.0, 0.0, 0.70711, 0.70711) + ) self._robot = Articulation(self.cfg.robot) - self._fixed_asset = Articulation(self.cfg_task.fixed_asset) - self._held_asset = Articulation(self.cfg_task.held_asset) + # Joint-less assets dispatch via cfg.class_type so PhysX gets + # Articulation and Newton gets RigidObject (after the Newton-only + # cfg conversion in __init__). Same call site, both backends. + self._fixed_asset = self.cfg_task.fixed_asset.class_type(self.cfg_task.fixed_asset) + self._held_asset = self.cfg_task.held_asset.class_type(self.cfg_task.held_asset) if self.cfg_task.name == "gear_mesh": - self._small_gear_asset = Articulation(self.cfg_task.small_gear_cfg) - self._large_gear_asset = Articulation(self.cfg_task.large_gear_cfg) + self._small_gear_asset = self.cfg_task.small_gear_cfg.class_type(self.cfg_task.small_gear_cfg) + self._large_gear_asset = self.cfg_task.large_gear_cfg.class_type(self.cfg_task.large_gear_cfg) self.scene.clone_environments(copy_from_source=False) if self.device == "cpu": @@ -104,16 +187,94 @@ def _setup_scene(self): self.scene.filter_collisions() self.scene.articulations["robot"] = self._robot - self.scene.articulations["fixed_asset"] = self._fixed_asset - self.scene.articulations["held_asset"] = self._held_asset + # Joint-less assets register on rigid_objects (Newton: RigidObject, + # PhysX: Articulation also satisfies the rigid-object dict contract; + # mirrors what shadow_hand_vision does). Either backend can read its + # own object back via the dict on reset/randomization paths. + asset_registry = self.scene.rigid_objects if self._is_newton else self.scene.articulations + asset_registry["fixed_asset"] = self._fixed_asset + asset_registry["held_asset"] = self._held_asset if self.cfg_task.name == "gear_mesh": - self.scene.articulations["small_gear"] = self._small_gear_asset - self.scene.articulations["large_gear"] = self._large_gear_asset + asset_registry["small_gear"] = self._small_gear_asset + asset_registry["large_gear"] = self._large_gear_asset + + # Newton-only: register the kinematic table RigidObject so the scene + # can clone/bind it and the data layer can resolve its body. PhysX + # spawned the table as a plain static prim and doesn't need this. + if self._is_newton: + self.scene.rigid_objects["table"] = self._table # add lights light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) light_cfg.func("/World/Light", light_cfg) + # Register Newton's MODEL_INIT callback here (before model finalization) + # rather than in __init__ after super().__init__. + if self._is_newton: + from . import factory_newton_setup + + factory_newton_setup.register_model_init_callback() + + def _compute_fingertip_velocity_from_newton_state(self) -> tuple[torch.Tensor, torch.Tensor]: + """Read fingertip linear + angular velocity directly from mjwarp state. + + The IsaacLab Newton articulation data adapter returns 0 for + ``body_lin_vel_w`` / ``body_link_lin_vel_w`` on the + ``panda_fingertip_centered`` body (a zero-mass virtual link). + That kills the OSC's task-space Kd*e_dot damping term. The raw + mjwarp ``state.body_qd`` has the correct velocity for that body, + so we read it directly and apply the COM->link transport: + + v_link = v_com + omega x (p_link - p_com) + + Returns: + Tuple ``(linvel, angvel)``, each ``(num_envs, 3)`` torch + tensors on the robot device, in world frame at the + fingertip body origin. + """ + from isaaclab_newton.physics import NewtonManager + + state = NewtonManager._state_0 + model = NewtonManager._model + if state is None or model is None: + zero = torch.zeros((self.num_envs, 3), device=self.device) + return zero, zero + + # Cache the per-env fingertip body global indices on first call. + if not hasattr(self, "_fingertip_body_global_idx"): + labels = list(model.body_label) + idxs = [] + for env_idx in range(self.num_envs): + env_token = f"/env_{env_idx}/" + for i, lab in enumerate(labels): + if lab.endswith("/panda_fingertip_centered") and env_token in lab: + idxs.append(i) + break + if len(idxs) != self.num_envs: + raise RuntimeError( + f"Could not resolve panda_fingertip_centered for all envs (found {len(idxs)} of {self.num_envs})." + ) + self._fingertip_body_global_idx = torch.tensor(idxs, dtype=torch.long, device=self.device) + + # body_qd layout: [v_com_x, v_com_y, v_com_z, w_x, w_y, w_z] in world frame. + body_qd_t = wp.to_torch(state.body_qd) # (n_bodies, 6) + ft_idx = self._fingertip_body_global_idx + v_com = body_qd_t[ft_idx, 0:3] + omega = body_qd_t[ft_idx, 3:6] + + # Transport from body COM to body link origin: v_link = v_com + omega x (p_link - p_com). + # body_q[fingertip] is the link world transform; body_com is local COM offset. + body_q_t = wp.to_torch(state.body_q) # (n_bodies, 7) = (px, py, pz, qx, qy, qz, qw) + body_com_t = wp.to_torch(model.body_com) # (n_bodies, 3) local-frame COM offset + link_pos = body_q_t[ft_idx, 0:3] + link_quat_xyzw = body_q_t[ft_idx, 3:7] + com_local = body_com_t[ft_idx] + # Rotate com_local into world frame using link_quat. + com_world_offset = torch_utils.quat_apply(link_quat_xyzw, com_local) + r_com_w = link_pos + com_world_offset + v_link = v_com + torch.cross(omega, link_pos - r_com_w, dim=-1) + return v_link, omega + def _compute_intermediate_values(self, dt): """Get values computed from raw tensors. This includes adding noise.""" # TODO: A lot of these can probably only be set once? @@ -127,11 +288,19 @@ def _compute_intermediate_values(self, dt): self._robot.data.body_pos_w.torch[:, self.fingertip_body_idx] - self.scene.env_origins ) self.fingertip_midpoint_quat = self._robot.data.body_quat_w.torch[:, self.fingertip_body_idx] - self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w.torch[:, self.fingertip_body_idx] - self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w.torch[:, self.fingertip_body_idx] + if self._is_newton: + # Newton's data adapter zeros body_lin_vel_w on the zero-mass + # virtual fingertip body, which kills OSC task-space damping. + # Read the raw mjwarp state instead and transport COM→link origin. + self.fingertip_midpoint_linvel, self.fingertip_midpoint_angvel = ( + self._compute_fingertip_velocity_from_newton_state() + ) + else: + self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w.torch[:, self.fingertip_body_idx] + self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w.torch[:, self.fingertip_body_idx] + # Backend-agnostic OSC inputs via PR #5400 accessors. jacobians = self._robot.data.body_link_jacobian_w.torch - self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7] self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7] self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5 @@ -328,6 +497,15 @@ def generate_ctrl_signals( self.ctrl_target_joint_pos[:, 7:9] = ctrl_target_gripper_dof_pos self.joint_torque[:, 7:9] = 0.0 + # Newton's articulation drive does not enforce per-joint effort_limit_sim + # on direct joint_f writes (PhysX does). Without this clamp, OSC torque + # saturation produces 100 N·m on the wrist (factory_control.compute_dof_torque's + # global ±100 ceiling) instead of the FR3 datasheet 12 N·m. + if self._is_newton: + from . import factory_control_newton + + factory_control_newton.clamp_to_effort_limits(self.joint_torque) + self._robot.set_joint_position_target_index(target=self.ctrl_target_joint_pos) self._robot.set_joint_effort_target_index(target=self.joint_torque) @@ -504,16 +682,16 @@ def _set_assets_to_default_pose(self, env_ids): held_vel = self._held_asset.data.default_root_vel.torch.clone()[env_ids] held_pose[:, 0:3] += self.scene.env_origins[env_ids] held_vel[:] = 0.0 - self._held_asset.write_root_pose_to_sim_index(root_pose=held_pose, env_ids=env_ids) - self._held_asset.write_root_velocity_to_sim_index(root_velocity=held_vel, env_ids=env_ids) + self._held_asset.write_root_link_pose_to_sim_index(root_pose=held_pose, env_ids=env_ids) + self._held_asset.write_root_link_velocity_to_sim_index(root_velocity=held_vel, env_ids=env_ids) self._held_asset.reset() fixed_pose = self._fixed_asset.data.default_root_pose.torch.clone()[env_ids] fixed_vel = self._fixed_asset.data.default_root_vel.torch.clone()[env_ids] fixed_pose[:, 0:3] += self.scene.env_origins[env_ids] fixed_vel[:] = 0.0 - self._fixed_asset.write_root_pose_to_sim_index(root_pose=fixed_pose, env_ids=env_ids) - self._fixed_asset.write_root_velocity_to_sim_index(root_velocity=fixed_vel, env_ids=env_ids) + self._fixed_asset.write_root_link_pose_to_sim_index(root_pose=fixed_pose, env_ids=env_ids) + self._fixed_asset.write_root_link_velocity_to_sim_index(root_velocity=fixed_vel, env_ids=env_ids) self._fixed_asset.reset() def set_pos_inverse_kinematics( @@ -550,7 +728,10 @@ def set_pos_inverse_kinematics( self._robot.write_joint_velocity_to_sim_index(velocity=self.joint_vel) self._robot.set_joint_position_target_index(target=self.ctrl_target_joint_pos) - # Simulate and update tensors. + # Simulate and update tensors. ``step_sim_no_action`` itself + # dispatches per backend: PhysX runs the full physics step; + # Newton skips the integrator and only refreshes FK + Jacobian + # (see :meth:`step_sim_no_action`). self.step_sim_no_action() ik_time += self.physics_dt @@ -612,6 +793,16 @@ def step_sim_no_action(self): This method should only be called during resets when all environments reset at the same time. + + Both backends now run the full ``sim.step``. An earlier optimization + attempt skipped the integrator on Newton in favor of a direct + ``eval_fk`` refresh, but that bypassed Newton's captured-CUDA-graph + scatter/gather between staging arrays and ``state_0``, leaving the + IsaacLab data-layer bindings (``body_pos_w`` etc.) reading stale + values. DLS IK then diverged because the Jacobian and the cached + fingertip pose disagreed about the current state. The captured + graph is fast (~1.5 ms after warm-up), so paying it per IK + iteration is acceptable. """ self.scene.write_data_to_sim() self.sim.step(render=False) @@ -620,9 +811,13 @@ def step_sim_no_action(self): def randomize_initial_state(self, env_ids): """Randomize initial state and perform any episode-level randomization.""" - # Disable gravity. - physics_sim_view = sim_utils.SimulationContext.instance().physics_sim_view - physics_sim_view.set_gravity(carb.Float3(0.0, 0.0, 0.0)) + self._full_reset(env_ids) + + def _full_reset(self, env_ids): + """Original PhysX reset path: IK + asset randomization + grasp settle.""" + + # Disable gravity (PhysX/Newton-portable). + _set_sim_gravity(self.cfg, (0.0, 0.0, 0.0)) # (1.) Randomize fixed asset pose. fixed_pose = self._fixed_asset.data.default_root_pose.torch.clone()[env_ids] @@ -648,8 +843,8 @@ def randomize_initial_state(self, env_ids): # (1.c.) Velocity fixed_vel[:] = 0.0 # vel # (1.d.) Update values. - self._fixed_asset.write_root_pose_to_sim_index(root_pose=fixed_pose, env_ids=env_ids) - self._fixed_asset.write_root_velocity_to_sim_index(root_velocity=fixed_vel, env_ids=env_ids) + self._fixed_asset.write_root_link_pose_to_sim_index(root_pose=fixed_pose, env_ids=env_ids) + self._fixed_asset.write_root_link_velocity_to_sim_index(root_velocity=fixed_vel, env_ids=env_ids) self._fixed_asset.reset() # (1.e.) Noisy position observation. @@ -737,16 +932,16 @@ def randomize_initial_state(self, env_ids): small_gear_vel = self._small_gear_asset.data.default_root_vel.torch.clone()[env_ids] small_gear_pose[:, 0:7] = fixed_pose[:, 0:7] small_gear_vel[:] = 0.0 # vel - self._small_gear_asset.write_root_pose_to_sim_index(root_pose=small_gear_pose, env_ids=env_ids) - self._small_gear_asset.write_root_velocity_to_sim_index(root_velocity=small_gear_vel, env_ids=env_ids) + self._small_gear_asset.write_root_link_pose_to_sim_index(root_pose=small_gear_pose, env_ids=env_ids) + self._small_gear_asset.write_root_link_velocity_to_sim_index(root_velocity=small_gear_vel, env_ids=env_ids) self._small_gear_asset.reset() large_gear_pose = self._large_gear_asset.data.default_root_pose.torch.clone()[env_ids] large_gear_vel = self._large_gear_asset.data.default_root_vel.torch.clone()[env_ids] large_gear_pose[:, 0:7] = fixed_pose[:, 0:7] large_gear_vel[:] = 0.0 # vel - self._large_gear_asset.write_root_pose_to_sim_index(root_pose=large_gear_pose, env_ids=env_ids) - self._large_gear_asset.write_root_velocity_to_sim_index(root_velocity=large_gear_vel, env_ids=env_ids) + self._large_gear_asset.write_root_link_pose_to_sim_index(root_pose=large_gear_pose, env_ids=env_ids) + self._large_gear_asset.write_root_link_velocity_to_sim_index(root_velocity=large_gear_vel, env_ids=env_ids) self._large_gear_asset.reset() # (3) Randomize asset-in-gripper location. @@ -789,8 +984,8 @@ def randomize_initial_state(self, env_ids): held_pose[:, 0:3] = translated_held_asset_pos + self.scene.env_origins held_pose[:, 3:7] = translated_held_asset_quat held_vel[:] = 0.0 - self._held_asset.write_root_pose_to_sim_index(root_pose=held_pose) - self._held_asset.write_root_velocity_to_sim_index(root_velocity=held_vel) + self._held_asset.write_root_link_pose_to_sim_index(root_pose=held_pose) + self._held_asset.write_root_link_velocity_to_sim_index(root_velocity=held_vel) self._held_asset.reset() # Close hand @@ -828,4 +1023,10 @@ def randomize_initial_state(self, env_ids): self.task_prop_gains = self.default_gains self.task_deriv_gains = factory_utils.get_deriv_gains(self.default_gains) - physics_sim_view.set_gravity(carb.Float3(*self.cfg.sim.gravity)) + # Restore gravity (PhysX). Newton's adapter doesn't honour the + # per-body ``disable_gravity=True`` flags set on the robot / held nut, + # so keep global gravity at zero post-reset to match PhysX behaviour. + if self._is_newton: + _set_sim_gravity(self.cfg, (0.0, 0.0, 0.0)) + else: + _set_sim_gravity(self.cfg, tuple(self.cfg.sim.gravity)) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py index a250380e2566..d768883a5a0c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py @@ -3,6 +3,12 @@ # # SPDX-License-Identifier: BSD-3-Clause +from isaaclab_newton.physics import ( + HydroelasticSDFCfg, + MJWarpSolverCfg, + NewtonCfg, + NewtonCollisionPipelineCfg, +) from isaaclab_physx.physics import PhysxCfg import isaaclab.sim as sim_utils @@ -14,6 +20,8 @@ from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg from isaaclab.utils.configclass import configclass +from isaaclab_tasks.utils import PresetCfg + from .factory_tasks_cfg import ASSET_DIR, FactoryTask, GearMesh, NutThread, PegInsert OBS_DIM_CFG = { @@ -68,6 +76,95 @@ class CtrlCfg: kp_null = 10.0 kd_null = 6.3246 + # When True, skip the OSC null-space term in :func:`factory_control.compute_dof_torque`. + # Default ``False`` keeps the original Factory PhysX behaviour (null-space on); + # :meth:`FactoryEnv.__init__` overrides it to ``True`` on Newton because the + # null-space target ``default_dof_pos_tensor`` does not match the IK-converged + # "above bolt" arm config the env actually holds, so leaving null-space on + # leaks a constant TCP-direction torque under Newton's mjwarp OSC integration. + disable_nullspace: bool = False + + +@configclass +class FactoryPhysicsCfg(PresetCfg): + """Per-backend physics cfg for Factory tasks. + + PhysX preserves the original Factory tuning. Newton uses the MuJoCo-Warp + solver: + + * ``integrator='implicitfast'`` — better stability for contact-rich scenes. + * ``cone='elliptic'``, ``impratio=10`` — friction-vs-normal coupling tuned + for finger-on-nut/bolt threading. + * ``use_mujoco_contacts=False`` — routes hydroelastic contacts into the + MuJoCo solver so ``moment_matching`` / ``anchor_contact`` actually engage. + * ``njmax`` / ``nconmax`` raised vs default to fit threading-pair contacts. + """ + + physx = PhysxCfg( + solver_type=1, + max_position_iteration_count=192, # Important to avoid interpenetration. + max_velocity_iteration_count=1, + bounce_threshold_velocity=0.2, + friction_offset_threshold=0.01, + friction_correlation_distance=0.00625, + gpu_max_rigid_contact_count=2**23, + gpu_max_rigid_patch_count=2**23, + gpu_collision_stack_size=2**28, + gpu_max_num_partitions=1, # Important for stable simulation. + ) + newton = NewtonCfg( + solver_cfg=MJWarpSolverCfg( + solver="newton", + integrator="implicitfast", + njmax=4000, + nconmax=4000, + # impratio=10 keeps gripper-vs-nut friction loose enough for the + # fingers to close on the nut; higher values over-constrain the pad + # against the nut surface. + impratio=10.0, + cone="elliptic", + # Route hydroelastic contacts through MuJoCo so anchor_contact / + # moment_matching engage; the static value here is overridden by + # FactoryEnv.__init__ scaling rigid_contact_max with num_envs. + use_mujoco_contacts=False, + iterations=10, + ls_iterations=100, + ), + collision_cfg=NewtonCollisionPipelineCfg( + broad_phase="explicit", + rigid_contact_max=32768, + sdf_hydroelastic_config=HydroelasticSDFCfg( + # PhysX patch-friction analogs — preserve force + torque balance + # per normal bin under contact reduction so the nut doesn't spin + # out under asymmetric finger pinch. + anchor_contact=True, + moment_matching=True, + output_contact_surface=False, + ), + ), + # 8 solver substeps per 8.33 ms physics tick (Newton runs at + # 120 Hz; see :func:`apply_cfg_overrides`) → 1.04 ms substep + # dt. Re-collide every 2 substeps so contacts update 4× per + # tick. + num_substeps=8, + collision_substeps=2, + use_cuda_graph=True, + ) + default = physx + + +@configclass +class FactorySceneCfg(PresetCfg): + """Per-backend scene cfg. + + PhysX uses Fabric-layer scene cloning for throughput; Newton does not + support ``clone_in_fabric=True`` and must use the per-prim clone path. + """ + + physx: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=128, env_spacing=2.0, clone_in_fabric=True) + newton: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=128, env_spacing=2.0, clone_in_fabric=False) + default: InteractiveSceneCfg = physx + @configclass class FactoryEnvCfg(DirectRLEnvCfg): @@ -100,25 +197,14 @@ class FactoryEnvCfg(DirectRLEnvCfg): device="cuda:0", dt=1 / 120, gravity=(0.0, 0.0, -9.81), - physics=PhysxCfg( - solver_type=1, - max_position_iteration_count=192, # Important to avoid interpenetration. - max_velocity_iteration_count=1, - bounce_threshold_velocity=0.2, - friction_offset_threshold=0.01, - friction_correlation_distance=0.00625, - gpu_max_rigid_contact_count=2**23, - gpu_max_rigid_patch_count=2**23, - gpu_collision_stack_size=2**28, - gpu_max_num_partitions=1, # Important for stable simulation. - ), + physics=FactoryPhysicsCfg(), physics_material=RigidBodyMaterialCfg( static_friction=1.0, dynamic_friction=1.0, ), ) - scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=128, env_spacing=2.0, clone_in_fabric=True) + scene: InteractiveSceneCfg = FactorySceneCfg() robot = ArticulationCfg( prim_path="/World/envs/env_.*/Robot", diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py new file mode 100644 index 000000000000..f8caaf5feae6 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.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 + +"""Factory: procedural Newton-only post-load asset setup. + +Three entry points called from :class:`FactoryEnv`: + +* :func:`apply_cfg_overrides` — runs before ``super().__init__``. Raises + the physics rate, disables the OSC null-space term, wraps joint-less + Factory assets as :class:`RigidObjectCfg`, tunes the arm and finger + actuators, monkey-patches the cloner, and sizes the contact buffer. + +* :func:`register_model_init_callback` — registers a Newton + ``MODEL_INIT`` callback that runs before model finalization. Overrides + joint target mode + ctrl-source on the Franka, filters base ↔ table + contacts, retunes the nut/bolt contact materials, and builds per-shape + hydroelastic SDFs. + +* :func:`warm_up_kernels` — runs after ``_init_tensors``. Pre-JITs the + Newton kernels the reset IK loop will hit. + +PhysX runs never import this module. +""" + +from __future__ import annotations + +import logging +from typing import TYPE_CHECKING + +import newton + +from isaaclab.assets import ArticulationCfg, RigidObjectCfg + +logger = logging.getLogger(__name__) + +if TYPE_CHECKING: + from .factory_env import FactoryEnv + from .factory_env_cfg import FactoryEnvCfg + +_ARM_JOINT_NAMES = [f"panda_joint{i}" for i in range(1, 8)] +_FINGER_JOINT_NAMES = ["panda_finger_joint1", "panda_finger_joint2"] +# Substring matched against ``builder.body_label`` to find every body +# belonging to the Franka under the IsaacLab USD layout. +_ROBOT_BODY_PATH_SUBSTR = "/Robot/" + + +def register_model_init_callback() -> None: + """Wire a Newton MODEL_INIT callback that mutates the builder.""" + from isaaclab_newton.physics import NewtonManager + + from isaaclab.physics import PhysicsEvent + + NewtonManager.register_callback( + lambda _ev: _model_init_callback(), PhysicsEvent.MODEL_INIT, name="factory_newton_setup" + ) + + +def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: + """Apply Newton-only cfg overrides in place, *before* ``super().__init__``. + + Keeps the physics rate at 120 Hz with ``decimation = 8`` to keep the + control rate at 15 Hz, disables the OSC null-space term, wraps joint-less + Factory assets as :class:`RigidObjectCfg` (kinematic for static targets), + bumps arm armature, zeroes arm damping, softens finger PD, monkey-patches + the cloner to skip convex-hull simplification (so nut/bolt thread + hex + SDFs survive cloning), and scales the contact buffer with ``num_envs``. + """ + cfg.sim.dt = 1.0 / 120.0 + cfg.decimation = 8 + cfg.ctrl.disable_nullspace = True + + cfg_task = cfg.task + kinematic_for = {"fixed_asset", "small_gear_cfg", "large_gear_cfg"} + for asset_attr in ("fixed_asset", "held_asset", "small_gear_cfg", "large_gear_cfg"): + asset_cfg = getattr(cfg_task, asset_attr, None) + if isinstance(asset_cfg, ArticulationCfg): + setattr(cfg_task, asset_attr, _to_rigid_object_cfg(asset_cfg, kinematic=asset_attr in kinematic_for)) + + # Zero the bolt reset's Z noise: the bolt is a kinematic body (USD + # PhysicsFixedJoint) so it sticks wherever the reset places it. + noise = getattr(cfg_task, "fixed_asset_init_pos_noise", None) + if noise is not None and len(noise) >= 3: + cfg_task.fixed_asset_init_pos_noise = list(noise[:2]) + [0.0] + + # Bolt init z=0.0 matches the Newton cuboid table top. + bolt = getattr(cfg_task, "fixed_asset", None) + if bolt is not None and hasattr(bolt, "init_state") and hasattr(bolt.init_state, "pos"): + cur_pos = bolt.init_state.pos + new_init = bolt.init_state.replace(pos=(float(cur_pos[0]), float(cur_pos[1]), 0.0)) + cfg_task.fixed_asset = bolt.replace(init_state=new_init) + + # Arm armature stabilises OSC Lambda; must be set before super().__init__ + # so finalize bakes it into model.joint_armature. + arm_actuators = cfg.robot.actuators + arm_actuators["panda_arm1"].armature = 0.3 + arm_actuators["panda_arm2"].armature = 0.11 + arm_actuators["panda_hand"].armature = 0.15 + + # kd=0: OSC Kd=2√Kp + armature already damp the 7th DOF; extra kd brakes + # OSC tracking (kd=10 → +12 mm step error vs PhysX +48 mm). + arm_actuators["panda_arm1"].damping = 0.0 + arm_actuators["panda_arm2"].damping = 0.0 + + # Softer finger PD than PhysX default (7500, 173): with hydroelastic SDFs + # grip force comes from kh × penetration, not the PD spring. + arm_actuators["panda_hand"].stiffness = 1000.0 + arm_actuators["panda_hand"].damping = 10.0 + + _monkey_patch_cloner_no_simplify() + + # Scale the contact buffer with num_envs so the gripper close doesn't + # overflow when the user bumps the world count. + if ( + getattr(cfg.sim.physics, "collision_cfg", None) is not None + and getattr(cfg.sim.physics, "solver_cfg", None) is not None + and hasattr(cfg.sim.physics.solver_cfg, "njmax") + ): + njmax = int(cfg.sim.physics.solver_cfg.njmax) + num_worlds = int(cfg.scene.num_envs) + cfg.sim.physics.collision_cfg.rigid_contact_max = njmax * num_worlds + + +def warm_up_kernels(env: FactoryEnv) -> None: + """Pre-JIT Newton's per-step kernels by running two dummy steps. + + Factory's reset path runs a 30-iteration DLS IK loop where each iteration + calls ``step_sim_no_action``. On the first iteration Newton JIT-compiles + several kernels (``eval_fk``, the actuator-model kernels, + ``eval_jacobian``/``eval_mass_matrix``) that each take 10-30 s; pre-paying + that cost here keeps the 30 IK iterations on warm caches. + """ + for _ in range(2): + env.step_sim_no_action() + + +def _to_rigid_object_cfg(art_cfg: ArticulationCfg, kinematic: bool) -> RigidObjectCfg: + """Adapt a joint-less Factory ArticulationCfg into a RigidObjectCfg. + + Wraps the asset as :class:`RigidObjectCfg` with + ``kinematic_enabled=True`` for static targets so ``cfg.class_type(cfg)`` + constructs a :class:`RigidObject` at scene-setup time. + """ + import isaaclab.sim as sim_utils # noqa: PLC0415 + + rigid_props = art_cfg.spawn.rigid_props + if kinematic: + rigid_props = ( + rigid_props.replace(kinematic_enabled=True) + if rigid_props is not None + else sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True) + ) + spawn = art_cfg.spawn.replace(rigid_props=rigid_props) + return RigidObjectCfg( + prim_path=art_cfg.prim_path, + spawn=spawn, + init_state=RigidObjectCfg.InitialStateCfg( + pos=art_cfg.init_state.pos, + rot=art_cfg.init_state.rot, + lin_vel=art_cfg.init_state.lin_vel, + ang_vel=art_cfg.init_state.ang_vel, + ), + ) + + +def _monkey_patch_cloner_no_simplify() -> None: + """Skip the cloner's convex-hull mesh approximation so per-shape SDFs survive.""" + from isaaclab_newton.cloner import newton_replicate as _nr + + _orig = _nr.newton_physics_replicate + + def _no_simplify(*args, **kwargs): + kwargs.setdefault("simplify_meshes", False) + return _orig(*args, **kwargs) + + _nr.newton_physics_replicate = _no_simplify + import isaaclab_newton.cloner as _cloner_module # noqa: PLC0415 + + _cloner_module.newton_physics_replicate = _no_simplify + + +# --------------------------------------------------------------------------- +# Builder-time setup (MODEL_INIT callback body). +# --------------------------------------------------------------------------- + + +def _model_init_callback() -> None: + """Body of the MODEL_INIT callback. Operates on the live builder.""" + from isaaclab_newton.physics import NewtonManager + + builder = NewtonManager._builder + if builder is None: + return + + _set_joint_target_mode(builder) + _set_ctrl_source_joint_target(builder) + _filter_base_table_contacts(builder) + _tune_nut_bolt_contacts(builder) + _build_collision_sdfs(builder) + # NOTE: Per-DOF armature is set on the Franka via ``ImplicitActuatorCfg`` + # in :class:`FactoryEnvCfg.robot.actuators`, not via the builder. + # Newton's finalize step seeds ``model.joint_armature`` for the robot's + # DOFs from the actuator cfg and overwrites any per-DOF builder writes + # we make at those indices. The actuator-cfg path is the only reliable + # way to apply armature to a parsed-USD articulation today. + + +def _joint_label_indices(builder, name_substrs: list[str]) -> list[int]: + """Return joint indices whose label contains any of ``name_substrs``.""" + return [i for i, label in enumerate(builder.joint_label) if any(s in label for s in name_substrs)] + + +def _joint_indices_to_dof_indices(builder, joint_idxs: list[int]) -> list[int]: + """Translate joint indices into DOF indices via ``joint_qd_start``. + + Newton's per-DOF arrays (``joint_target_mode``, ``joint_target_ke``, …) + are indexed by *DOF*, not joint. With free-floating joints in the + scene (each contributes 6 qd entries but only 1 joint label), + ``joint_index != dof_index`` past env 0. ``joint_qd_start[j]`` gives + the qd-index where joint ``j``'s DOFs begin; ``joint_qd_start[j+1]`` + gives the end. We walk that range so a finger (1-DOF revolute) gives + 1 entry and a free joint (6-DOF) would give 6 — though we only call + this for finite-DOF revolute joints. + """ + qd_start = list(builder.joint_qd_start) + total_dofs = qd_start[-1] if qd_start else 0 + out: list[int] = [] + for j in joint_idxs: + dof_lo = int(qd_start[j]) + dof_hi = int(qd_start[j + 1]) if (j + 1) < len(qd_start) else int(total_dofs) + out.extend(range(dof_lo, dof_hi)) + return out + + +def _arm_dof_indices(builder) -> list[int]: + """DOF indices in ``builder.joint_target_mode`` for the arm joints.""" + js = _joint_label_indices(builder, _ARM_JOINT_NAMES) + return _joint_indices_to_dof_indices(builder, js) + + +def _finger_dof_indices(builder) -> list[int]: + """DOF indices in ``builder.joint_target_mode`` for the finger joints.""" + js = _joint_label_indices(builder, _FINGER_JOINT_NAMES) + return _joint_indices_to_dof_indices(builder, js) + + +def _robot_body_indices(builder) -> list[int]: + """Indices into ``builder.body_label`` that belong to the Franka.""" + return [i for i, label in enumerate(builder.body_label) if _ROBOT_BODY_PATH_SUBSTR in label] + + +def _set_joint_target_mode(builder) -> None: + """Force every robot DOF (arm + fingers) into POSITION mode. + + With ``joint_target_ke = 0`` on the arm, the position target has no + effect on the arm; but ``joint_target_kd > 0`` then gives mjwarp a + per-joint ``-kd * qd`` damping term that kills the redundant-7th DOF + mode the OSC would otherwise have to chase. + """ + for dof_idx in _arm_dof_indices(builder) + _finger_dof_indices(builder): + builder.joint_target_mode[dof_idx] = int(newton.JointTargetMode.POSITION) + + +def _set_ctrl_source_joint_target(builder) -> None: + """Pin every robot DOF's ``mujoco:ctrl_source`` to ``JOINT_TARGET``. + + With ``mujoco:ctrl_source = JOINT_TARGET``, mjwarp reads the actuator's + target from ``Control.joint_target_pos`` (which IsaacLab's + ``set_joint_position_target_index`` writes) instead of the unused + ``Control.mujoco.ctrl`` array. Required for the POSITION-mode arm + to actually consume our position targets. + """ + from newton.solvers import SolverMuJoCo + + custom = builder.custom_attributes.get("mujoco:ctrl_source") + if custom is None: + return + n_acts = len(builder.joint_target_mode) + if custom.values is None or len(custom.values) != n_acts: + custom.values = [int(SolverMuJoCo.CtrlSource.JOINT_TARGET)] * n_acts + else: + target_value = int(SolverMuJoCo.CtrlSource.JOINT_TARGET) + for dof_idx in _arm_dof_indices(builder) + _finger_dof_indices(builder): + custom.values[dof_idx] = target_value + + +def _filter_base_table_contacts(builder) -> None: + """Filter spurious robot-base ↔ table contacts. + + The Franka base (``panda_link0``, ``panda_link1``) sits on the table. + Without explicit collision-filter pairs, mjwarp generates a continuous + contact between the base collision mesh and the table top surface every + step, even when there's no physical interpenetration. Those tiny contact + forces propagate through the kinematic chain and show up as + multi-millimeter TCP drift during OSC hold. + """ + base_suffixes = ("/panda_link0", "/panda_link1") + table_substr = "/Table" + base_shape_idxs = [ + i + for i, body_idx in enumerate(builder.shape_body) + if 0 <= body_idx < len(builder.body_label) and builder.body_label[body_idx].endswith(base_suffixes) + ] + table_shape_idxs = [ + i + for i, body_idx in enumerate(builder.shape_body) + if 0 <= body_idx < len(builder.body_label) and table_substr in builder.body_label[body_idx] + ] + if not base_shape_idxs or not table_shape_idxs: + return + for base_i in base_shape_idxs: + for table_i in table_shape_idxs: + pair = (min(base_i, table_i), max(base_i, table_i)) + builder.shape_collision_filter_pairs.append(pair) + logger.info("Filtered %d base<->table collision pairs.", len(base_shape_idxs) * len(table_shape_idxs)) + + +def _tune_nut_bolt_contacts(builder) -> None: + """Stiffen contact-material gains on every nut/bolt collision shape. + + Newton's parser defaults are too soft for the threading task: the gripper + bounces off the nut. ``ke``/``kd`` lift normal stiffness/damping ~1-2 orders + of magnitude; ``mu`` stays above ``MJ_MINMU`` to silence the NaN-risk warning. + """ + if not all(hasattr(builder, attr) for attr in ("shape_label", "shape_material_mu", "shape_gap")): + return + for i in range(builder.shape_count): + label = str(builder.shape_label[i]).lower() + if "nut" in label: + builder.shape_material_mu[i] = 0.2 + builder.shape_material_ke[i] = 1.0e4 + builder.shape_material_kd[i] = 100.0 + builder.shape_gap[i] = 0.0 + elif "bolt" in label: + builder.shape_material_mu[i] = 0.5 + builder.shape_material_ke[i] = 1.0e4 + builder.shape_material_kd[i] = 100.0 + builder.shape_gap[i] = 0.0 + + +# --------------------------------------------------------------------------- +# SDF / hydroelastic collision setup: +# * fingers — 192-cube SDF + HYDROELASTIC, kh=1e11, condim=4. +# * nut/bolt — 256-cube SDF + HYDROELASTIC, kh=1e11 (rigid thread features). +# * other panda links — 64-cube SDF only, used for fast distance lookups. +# Builder-time only; the NewtonCfg (use_mujoco_contacts=False + +# sdf_hydroelastic_config) is what actually engages the hydroelastic forces. +# --------------------------------------------------------------------------- + +# SDF resolutions (cube edge). +_SDF_RES_FINGER = 192 +_SDF_RES_NUT_BOLT = 256 +_SDF_RES_PANDA = 64 +# Table is large + flat (1.2 × 0.6 × 0.04 m). 32³ → ~37 mm cells in xy, +# ~1.25 mm in z — plenty for a "rest on a flat surface" SDF without +# burning memory on a high-res grid. +_SDF_RES_TABLE = 32 +_SDF_BAND_FINGER = (-0.01, 0.01) +_SDF_BAND_NUT_BOLT = (-0.005, 0.005) +_SDF_BAND_PANDA = (-0.01, 0.01) +# Wider outside band (2 cm) so the bolt + nut see the table SDF from +# slightly above, with a narrower inside band — nothing should ever +# be deep inside the table. +_SDF_BAND_TABLE = (-0.005, 0.02) + +# Hydroelastic stiffness [Pa/m]. Rigid fingers + nut + bolt (1e11): at smaller +# values the finger SDF visibly punches through the nut SDF since the +# hydroelastic counter-force / penetration_volume can't push back the +# ~187 N from the finger PD close. +_KH_FINGER = 1e11 +_KH_NUT_BOLT = 1e11 + +# Finger-only friction extras. +_FINGER_MU_TORSIONAL = 0.1 +_FINGER_CONDIM = 4 + + +def _build_collision_sdfs(builder) -> None: + """Build SDFs on Factory's collision meshes for hydroelastic contacts. + + Categorises every collidable mesh shape by body label, builds an + SDF at the resolution appropriate for that category, and (for + fingers + nut + bolt) flips the ``HYDROELASTIC`` shape flag and + writes ``shape_material_kh`` + ``shape_material_mu_torsional``. + + Idempotent per mesh: each :class:`newton.Mesh` whose ``sdf`` is + already populated is skipped, so re-runs on a hot builder don't + re-bake the SDF grids. Multiple shapes can share the same mesh, + but mesh.build_sdf is only called once per unique mesh. + """ + finger_names = ("panda_leftfinger", "panda_rightfinger") + finger_body_idxs = {i for i, label in enumerate(builder.body_label) if any(n in label for n in finger_names)} + nut_body_idxs = {i for i, label in enumerate(builder.body_label) if "HeldAsset/factory_nut_loose" in label} + bolt_body_idxs = {i for i, label in enumerate(builder.body_label) if "FixedAsset/factory_bolt_loose" in label} + table_body_idxs = {i for i, label in enumerate(builder.body_label) if "/Table" in label} + panda_body_idxs = set(_robot_body_indices(builder)) - finger_body_idxs + + meshlike = (newton.GeoType.MESH, newton.GeoType.CONVEX_MESH) + counts = { + "finger": 0, + "nut": 0, + "bolt": 0, + "panda": 0, + "table": 0, + "skip_no_mesh": 0, + "skip_already_built": 0, + } + + condim_attr = builder.custom_attributes.get("mujoco:condim") + if condim_attr is not None and condim_attr.values is None: + condim_attr.values = {} + + for shape_idx, body_idx in enumerate(builder.shape_body): + if int(builder.shape_type[shape_idx]) not in (int(t) for t in meshlike): + continue + if not (int(builder.shape_flags[shape_idx]) & int(newton.ShapeFlags.COLLIDE_SHAPES)): + continue + + mesh = builder.shape_source[shape_idx] + if mesh is None: + counts["skip_no_mesh"] += 1 + continue + + if body_idx in finger_body_idxs: + category = "finger" + res, band = _SDF_RES_FINGER, _SDF_BAND_FINGER + elif body_idx in nut_body_idxs: + category = "nut" + res, band = _SDF_RES_NUT_BOLT, _SDF_BAND_NUT_BOLT + elif body_idx in bolt_body_idxs: + category = "bolt" + res, band = _SDF_RES_NUT_BOLT, _SDF_BAND_NUT_BOLT + elif body_idx in panda_body_idxs: + category = "panda" + res, band = _SDF_RES_PANDA, _SDF_BAND_PANDA + elif body_idx in table_body_idxs: + # Voxel SDF for visualisation under "Show Collision". Not + # flagged HYDROELASTIC — we want the bolt-on-table contact + # to stay on Newton's default rigid pipeline so it doesn't + # compete with the finger / nut / bolt hydroelastic mass + # balance. + category = "table" + res, band = _SDF_RES_TABLE, _SDF_BAND_TABLE + else: + continue + + if mesh.sdf is None: + # Bake non-unit shape_scale into the mesh vertices first — + # ``mesh.build_sdf`` doesn't honour shape_scale, so a mesh + # with shape_scale != 1 ends up with an SDF in the wrong + # coordinate system. Resetting shape_scale to (1,1,1) afterwards + # keeps shape vs. mesh in sync. + shape_scale = builder.shape_scale[shape_idx] + scale_arr = (float(shape_scale[0]), float(shape_scale[1]), float(shape_scale[2])) + if not ( + abs(scale_arr[0] - 1.0) < 1e-6 and abs(scale_arr[1] - 1.0) < 1e-6 and abs(scale_arr[2] - 1.0) < 1e-6 + ): + import numpy as _np # noqa: PLC0415 + + scaled_verts = mesh.vertices * _np.asarray(scale_arr, dtype=_np.float32) + mesh = mesh.copy(vertices=scaled_verts, recompute_inertia=True) + builder.shape_source[shape_idx] = mesh + builder.shape_scale[shape_idx] = (1.0, 1.0, 1.0) + mesh.build_sdf(max_resolution=res, narrow_band_range=band, margin=abs(band[1])) + counts[category] += 1 + else: + counts["skip_already_built"] += 1 + + if category in ("finger", "nut", "bolt"): + builder.shape_flags[shape_idx] |= int(newton.ShapeFlags.HYDROELASTIC) + builder.shape_material_kh[shape_idx] = _KH_FINGER if category == "finger" else _KH_NUT_BOLT + if category == "finger": + builder.shape_material_mu_torsional[shape_idx] = _FINGER_MU_TORSIONAL + if condim_attr is not None: + condim_attr.values[shape_idx] = _FINGER_CONDIM + + logger.info( + "Built SDFs: finger=%d nut=%d bolt=%d panda=%d table=%d (skipped: no_mesh=%d, already_built=%d).", + counts["finger"], + counts["nut"], + counts["bolt"], + counts["panda"], + counts["table"], + counts["skip_no_mesh"], + counts["skip_already_built"], + ) From 8a64081ea82b8a8acc8f460688aa75032fdabb6e Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Fri, 15 May 2026 19:35:10 +0200 Subject: [PATCH 23/35] factory: restore 'import warp as wp' on v04 (used by _compute_fingertip_velocity_from_newton_state) --- .../isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py | 1 + 1 file changed, 1 insertion(+) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py index 80d7b3282c2a..651b405db85d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py @@ -5,6 +5,7 @@ import numpy as np import torch +import warp as wp import carb From d7f76927970ddc3d559b1b6b6a62093a41ef21c3 Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Sat, 16 May 2026 08:42:31 +0200 Subject: [PATCH 24/35] factory_newton_setup: support SDF-only contact mode MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Selecting ``collision_cfg.sdf_hydroelastic_config=None`` on the Newton preset now drops Newton's hydroelastic pipeline and runs the SDF collision through pure penalty springs. Useful when the hydroelastic dependency is unavailable or for ablation studies. Tuning that changes per mode: - finger PD: hydroelastic relies on ``kh × penetration`` and runs with (1000, 10); SDF-only carries grip force through the PD spring and needs (2000, 40) to stop the nut slipping during threading. - nut/bolt material: hydroelastic uses ``ke=1e4`` / ``kd=100`` / ``gap=0``; SDF-only uses ``ke=1e7`` / ``kd=1e4`` / ``gap=5 mm`` so the penalty spring keeps the finger penetration inside the SDF narrow band. - finger shape flags: hydroelastic flips ``HYDROELASTIC`` + ``kh``; SDF-only writes ``ke`` / ``kd`` on the finger shapes (nut/bolt stay on Newton's default rigid pipeline). Mode is latched once in ``apply_cfg_overrides`` and read by the ``MODEL_INIT`` callback at builder time. --- .../direct/factory/factory_env_cfg.py | 13 ++- .../direct/factory/factory_newton_setup.py | 109 +++++++++++++----- 2 files changed, 88 insertions(+), 34 deletions(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py index d768883a5a0c..0c71995d2e19 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py @@ -90,14 +90,21 @@ class FactoryPhysicsCfg(PresetCfg): """Per-backend physics cfg for Factory tasks. PhysX preserves the original Factory tuning. Newton uses the MuJoCo-Warp - solver: + solver with Newton's SDF collision pipeline: * ``integrator='implicitfast'`` — better stability for contact-rich scenes. * ``cone='elliptic'``, ``impratio=10`` — friction-vs-normal coupling tuned for finger-on-nut/bolt threading. - * ``use_mujoco_contacts=False`` — routes hydroelastic contacts into the - MuJoCo solver so ``moment_matching`` / ``anchor_contact`` actually engage. + * ``use_mujoco_contacts=False`` — routes contacts through Newton's SDF + pipeline; combined with a non-None ``sdf_hydroelastic_config`` this + enables hydroelastic forces, otherwise the pipeline falls back to + penalty-spring contacts. * ``njmax`` / ``nconmax`` raised vs default to fit threading-pair contacts. + + Set ``collision_cfg.sdf_hydroelastic_config=None`` to run the SDF-only + penalty-spring mode (lower fidelity but no hydroelastic dependency). + :meth:`FactoryNewtonSetup.apply_cfg_overrides` reads this field and + retunes finger PD + per-shape ``ke`` / ``kd`` accordingly. """ physx = PhysxCfg( diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py index f8caaf5feae6..c7355305a69e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py @@ -16,7 +16,8 @@ ``MODEL_INIT`` callback that runs before model finalization. Overrides joint target mode + ctrl-source on the Franka, filters base ↔ table contacts, retunes the nut/bolt contact materials, and builds per-shape - hydroelastic SDFs. + SDFs. The SDFs feed either Newton's hydroelastic pipeline or a + penalty-spring fallback, selected by the active ``NewtonCfg``. * :func:`warm_up_kernels` — runs after ``_init_tensors``. Pre-JITs the Newton kernels the reset IK loop will hit. @@ -45,6 +46,11 @@ # belonging to the Franka under the IsaacLab USD layout. _ROBOT_BODY_PATH_SUBSTR = "/Robot/" +# Module-level latch set in :func:`apply_cfg_overrides` and read by the +# ``MODEL_INIT`` callback (no ``cfg`` reachable there). ``True`` when the +# Newton preset ships a non-None ``sdf_hydroelastic_config``. +_use_hydroelastic: bool = True + def register_model_init_callback() -> None: """Wire a Newton MODEL_INIT callback that mutates the builder.""" @@ -63,7 +69,8 @@ def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: Keeps the physics rate at 120 Hz with ``decimation = 8`` to keep the control rate at 15 Hz, disables the OSC null-space term, wraps joint-less Factory assets as :class:`RigidObjectCfg` (kinematic for static targets), - bumps arm armature, zeroes arm damping, softens finger PD, monkey-patches + bumps arm armature, zeroes arm damping, tunes the finger PD against the + active contact model (hydroelastic or SDF-only penalty), monkey-patches the cloner to skip convex-hull simplification (so nut/bolt thread + hex SDFs survive cloning), and scales the contact buffer with ``num_envs``. """ @@ -103,10 +110,21 @@ def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: arm_actuators["panda_arm1"].damping = 0.0 arm_actuators["panda_arm2"].damping = 0.0 - # Softer finger PD than PhysX default (7500, 173): with hydroelastic SDFs - # grip force comes from kh × penetration, not the PD spring. - arm_actuators["panda_hand"].stiffness = 1000.0 - arm_actuators["panda_hand"].damping = 10.0 + # Latch the contact model so the ``MODEL_INIT`` callback can branch. + global _use_hydroelastic + _use_hydroelastic = getattr(cfg.sim.physics.collision_cfg, "sdf_hydroelastic_config", None) is not None + + # Hydroelastic grip force comes from kh × penetration, so a soft PD + # spring (1000, 10) is enough. Under SDF-only the penalty spring is + # the only grip force, so the PD spring has to carry the load: stiffer + # (2000, 40) is the smallest pair that keeps the nut from slipping + # during threading without exciting finger oscillation. + if _use_hydroelastic: + arm_actuators["panda_hand"].stiffness = 1000.0 + arm_actuators["panda_hand"].damping = 10.0 + else: + arm_actuators["panda_hand"].stiffness = 2000.0 + arm_actuators["panda_hand"].damping = 40.0 _monkey_patch_cloner_no_simplify() @@ -317,38 +335,49 @@ def _filter_base_table_contacts(builder) -> None: def _tune_nut_bolt_contacts(builder) -> None: - """Stiffen contact-material gains on every nut/bolt collision shape. - - Newton's parser defaults are too soft for the threading task: the gripper - bounces off the nut. ``ke``/``kd`` lift normal stiffness/damping ~1-2 orders - of magnitude; ``mu`` stays above ``MJ_MINMU`` to silence the NaN-risk warning. + """Tune contact-material gains on every nut/bolt collision shape. + + Newton's parser defaults are too soft: the gripper bounces off the nut. + Both modes raise ``ke`` / ``kd`` and keep ``mu`` above ``MJ_MINMU``; + only the magnitudes differ. + + - hydroelastic: ``ke=1e4`` / ``kd=100`` / ``gap=0`` — the hydroelastic + pipeline carries the dominant normal force; the penalty spring stays + low so the two paths don't double-count. + - sdf-only: ``ke=1e7`` / ``kd=1e4`` / ``gap=5 mm`` — the penalty spring + is now the *only* normal force, so it has to keep finger penetration + inside the SDF narrow band; ``kd`` is sized to ~critical damping. """ if not all(hasattr(builder, attr) for attr in ("shape_label", "shape_material_mu", "shape_gap")): return + + if _use_hydroelastic: + ke, kd, gap = 1.0e4, 100.0, 0.0 + else: + ke, kd, gap = 1.0e7, 1.0e4, 0.005 + for i in range(builder.shape_count): label = str(builder.shape_label[i]).lower() if "nut" in label: builder.shape_material_mu[i] = 0.2 - builder.shape_material_ke[i] = 1.0e4 - builder.shape_material_kd[i] = 100.0 - builder.shape_gap[i] = 0.0 + builder.shape_material_ke[i] = ke + builder.shape_material_kd[i] = kd + builder.shape_gap[i] = gap elif "bolt" in label: builder.shape_material_mu[i] = 0.5 - builder.shape_material_ke[i] = 1.0e4 - builder.shape_material_kd[i] = 100.0 - builder.shape_gap[i] = 0.0 + builder.shape_material_ke[i] = ke + builder.shape_material_kd[i] = kd + builder.shape_gap[i] = gap # --------------------------------------------------------------------------- -# SDF / hydroelastic collision setup: -# * fingers — 192-cube SDF + HYDROELASTIC, kh=1e11, condim=4. -# * nut/bolt — 256-cube SDF + HYDROELASTIC, kh=1e11 (rigid thread features). -# * other panda links — 64-cube SDF only, used for fast distance lookups. -# Builder-time only; the NewtonCfg (use_mujoco_contacts=False + -# sdf_hydroelastic_config) is what actually engages the hydroelastic forces. +# SDF collision setup. Both modes build the same SDF grids; only the +# per-shape material flags differ: +# * hydroelastic: HYDROELASTIC flag + ``kh`` on finger/nut/bolt shapes. +# * sdf-only: ``ke`` / ``kd`` penalty springs on finger shapes. +# Finger friction extras (torsional + condim=4) apply in both modes. # --------------------------------------------------------------------------- -# SDF resolutions (cube edge). _SDF_RES_FINGER = 192 _SDF_RES_NUT_BOLT = 256 _SDF_RES_PANDA = 64 @@ -371,23 +400,37 @@ def _tune_nut_bolt_contacts(builder) -> None: _KH_FINGER = 1e11 _KH_NUT_BOLT = 1e11 +# SDF-only penalty spring on the fingers, used when ``_use_hydroelastic`` +# is False. ``ke`` is sized so the gripper close force keeps penetration +# well inside the 1 cm SDF band; ``kd`` is ~critically damped against +# the finger mass. +_KE_FINGER = 1.0e7 +_KD_FINGER = 1.0e4 + # Finger-only friction extras. _FINGER_MU_TORSIONAL = 0.1 _FINGER_CONDIM = 4 def _build_collision_sdfs(builder) -> None: - """Build SDFs on Factory's collision meshes for hydroelastic contacts. + """Build SDFs on Factory's collision meshes. Categorises every collidable mesh shape by body label, builds an - SDF at the resolution appropriate for that category, and (for - fingers + nut + bolt) flips the ``HYDROELASTIC`` shape flag and - writes ``shape_material_kh`` + ``shape_material_mu_torsional``. + SDF at the resolution appropriate for that category, and tags the + finger/nut/bolt shapes for the active contact model: + + - hydroelastic: ``HYDROELASTIC`` flag + ``shape_material_kh`` on + finger/nut/bolt. + - sdf-only: ``shape_material_ke`` / ``shape_material_kd`` penalty + springs on the fingers; nut + bolt stay rigid-pipeline. + + Finger torsional friction (``mu_torsional``) and ``condim=4`` apply + in both modes. Idempotent per mesh: each :class:`newton.Mesh` whose ``sdf`` is already populated is skipped, so re-runs on a hot builder don't re-bake the SDF grids. Multiple shapes can share the same mesh, - but mesh.build_sdf is only called once per unique mesh. + but ``mesh.build_sdf`` is only called once per unique mesh. """ finger_names = ("panda_leftfinger", "panda_rightfinger") finger_body_idxs = {i for i, label in enumerate(builder.body_label) if any(n in label for n in finger_names)} @@ -467,16 +510,20 @@ def _build_collision_sdfs(builder) -> None: else: counts["skip_already_built"] += 1 - if category in ("finger", "nut", "bolt"): + if _use_hydroelastic and category in ("finger", "nut", "bolt"): builder.shape_flags[shape_idx] |= int(newton.ShapeFlags.HYDROELASTIC) builder.shape_material_kh[shape_idx] = _KH_FINGER if category == "finger" else _KH_NUT_BOLT + elif not _use_hydroelastic and category == "finger": + builder.shape_material_ke[shape_idx] = _KE_FINGER + builder.shape_material_kd[shape_idx] = _KD_FINGER if category == "finger": builder.shape_material_mu_torsional[shape_idx] = _FINGER_MU_TORSIONAL if condim_attr is not None: condim_attr.values[shape_idx] = _FINGER_CONDIM logger.info( - "Built SDFs: finger=%d nut=%d bolt=%d panda=%d table=%d (skipped: no_mesh=%d, already_built=%d).", + "Built SDFs (%s): finger=%d nut=%d bolt=%d panda=%d table=%d (skipped: no_mesh=%d, already_built=%d).", + "hydroelastic" if _use_hydroelastic else "sdf-only", counts["finger"], counts["nut"], counts["bolt"], From e515eb6451b4dd1e661c42432bffdfd5bbd8fb50 Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Sat, 16 May 2026 10:15:56 +0200 Subject: [PATCH 25/35] factory_newton_setup: FACTORY_SDF_FINGER_KP/KD env-var override Adds a small ad-hoc knob to override the SDF-only finger PD without rebuilding the cfg surface. Reads ``FACTORY_SDF_FINGER_KP`` / ``FACTORY_SDF_FINGER_KD`` once in ``apply_cfg_overrides`` and applies them to the panda_hand actuator. Defaults match the values shipped in the SDF-only commit (kp=2000, kd=40). --- .../direct/factory/factory_newton_setup.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py index c7355305a69e..541ae1024023 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py @@ -28,6 +28,7 @@ from __future__ import annotations import logging +import os from typing import TYPE_CHECKING import newton @@ -123,8 +124,14 @@ def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: arm_actuators["panda_hand"].stiffness = 1000.0 arm_actuators["panda_hand"].damping = 10.0 else: - arm_actuators["panda_hand"].stiffness = 2000.0 - arm_actuators["panda_hand"].damping = 40.0 + # Ad-hoc sweep knob: ``FACTORY_SDF_FINGER_KP`` / ``FACTORY_SDF_FINGER_KD`` + # override the SDF-only finger PD without rebuilding the cfg surface. + # Defaults match the values picked in the SDF-only commit. + kp = float(os.environ.get("FACTORY_SDF_FINGER_KP", "2000")) + kd = float(os.environ.get("FACTORY_SDF_FINGER_KD", "40")) + arm_actuators["panda_hand"].stiffness = kp + arm_actuators["panda_hand"].damping = kd + logger.info("SDF-only finger PD: stiffness=%.1f damping=%.1f", kp, kd) _monkey_patch_cloner_no_simplify() From 6b460efc31a6fc0bf51c58dd140926c0ea997233 Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Sat, 16 May 2026 10:34:45 +0200 Subject: [PATCH 26/35] factory_newton_setup: bump SDF-only finger PD defaults to (8000, 160) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Picks the success_rate peak from the FACTORY_SDF_FINGER_KP/KD sweep on Factory_develop_baseline.pth (64 envs / 450 steps): ( 2000, 40) → 0.500 (old default) ( 3000, 60) → 0.547 ( 5000, 100) → 0.516 ( 6000, 120) → 0.641 ( 8000, 160) → 0.656 <- peak (10000, 200) → 0.594 (12000, 240) → 0.594 Higher stiffness above 8 k starts pushing the finger SDF into the nut SDF instead of gripping; lower stiffness lets the nut slip. Both axes peak at 8 k / 160. Still well below the hydroelastic 0.95 — SDF-only remains an ablation, not a competitive mode. --- .../direct/factory/factory_newton_setup.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py index 541ae1024023..9f11136b96a4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py @@ -117,18 +117,20 @@ def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: # Hydroelastic grip force comes from kh × penetration, so a soft PD # spring (1000, 10) is enough. Under SDF-only the penalty spring is - # the only grip force, so the PD spring has to carry the load: stiffer - # (2000, 40) is the smallest pair that keeps the nut from slipping - # during threading without exciting finger oscillation. + # the only grip force, so the PD spring has to carry the load — but + # too stiff and the finger SDF punches into the nut SDF instead of + # gripping. A 64-env / 450-step sweep on + # Factory_develop_baseline.pth picked (8000, 160) as the success + # peak (0.66 vs 0.50 at the prior 2 k default, 0.59 at 12 k). if _use_hydroelastic: arm_actuators["panda_hand"].stiffness = 1000.0 arm_actuators["panda_hand"].damping = 10.0 else: - # Ad-hoc sweep knob: ``FACTORY_SDF_FINGER_KP`` / ``FACTORY_SDF_FINGER_KD`` - # override the SDF-only finger PD without rebuilding the cfg surface. - # Defaults match the values picked in the SDF-only commit. - kp = float(os.environ.get("FACTORY_SDF_FINGER_KP", "2000")) - kd = float(os.environ.get("FACTORY_SDF_FINGER_KD", "40")) + # ``FACTORY_SDF_FINGER_KP`` / ``FACTORY_SDF_FINGER_KD`` env vars + # override the default for ad-hoc sweeps without rebuilding the + # cfg surface. + kp = float(os.environ.get("FACTORY_SDF_FINGER_KP", "8000")) + kd = float(os.environ.get("FACTORY_SDF_FINGER_KD", "160")) arm_actuators["panda_hand"].stiffness = kp arm_actuators["panda_hand"].damping = kd logger.info("SDF-only finger PD: stiffness=%.1f damping=%.1f", kp, kd) From a91dc82625e301f362808bb6c96bd3e2983346da Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Sat, 16 May 2026 16:32:08 +0200 Subject: [PATCH 27/35] factory_newton_setup: FACTORY_SOLVER_ITERATIONS / IMPRATIO env-var overrides MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Tunes MJWarp's ``solver_cfg.iterations`` and ``solver_cfg.impratio`` at runtime via env vars so we can sweep them without rebuilding the cfg surface — matches the ``FACTORY_SDF_FINGER_KP/KD`` knob already on this file. Defaults of 0 keep the cfg-pinned values; positive values override. Motivation: SDF-only penalty contacts may need much higher solver iterations (50-200) and impratio (100-1000) than the defaults to keep stiff contact pairs converged during gripper-on-nut threading. --- .../direct/factory/factory_newton_setup.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py index 9f11136b96a4..532a1fae3463 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py @@ -135,6 +135,21 @@ def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: arm_actuators["panda_hand"].damping = kd logger.info("SDF-only finger PD: stiffness=%.1f damping=%.1f", kp, kd) + # Ad-hoc solver knobs. ``FACTORY_SOLVER_ITERATIONS`` / ``FACTORY_SOLVER_IMPRATIO`` + # let us sweep MJWarp's main iteration count and friction-vs-normal + # coupling without rebuilding the cfg. Defaults below preserve current + # behavior (zero = leave the cfg value as-is). + solver_cfg = getattr(cfg.sim.physics, "solver_cfg", None) + if solver_cfg is not None: + iters_override = int(os.environ.get("FACTORY_SOLVER_ITERATIONS", "0")) + imp_override = float(os.environ.get("FACTORY_SOLVER_IMPRATIO", "0")) + if iters_override > 0 and hasattr(solver_cfg, "iterations"): + solver_cfg.iterations = iters_override + logger.info("MJWarp solver iterations override: %d", iters_override) + if imp_override > 0 and hasattr(solver_cfg, "impratio"): + solver_cfg.impratio = imp_override + logger.info("MJWarp solver impratio override: %.1f", imp_override) + _monkey_patch_cloner_no_simplify() # Scale the contact buffer with num_envs so the gripper close doesn't From 0ecdf0790c971b724e96a0d07de9cc062293720b Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Sat, 16 May 2026 17:15:25 +0200 Subject: [PATCH 28/35] factory_newton_setup: bump SDF-only finger contact ke/kd to (3e8, 3e5) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Empirical finding: keeping nut/bolt at Newton-example contact values (ke=1e7, kd=1e4) and bumping only the *finger* contact stiffness + damping by ~30× jumps SDF-only success on Factory_develop_baseline.pth from 0.625 to 0.891 (engage 0.656 -> 0.953). The default ``_KE_FINGER = 1e7`` was inherited from the Newton nut/bolt example which has no gripper, so the value was wrong for the finger pads under the gripper's saturated 40 N close force. Combined with stiffness sweep on finger (ke, kd): (1e7, 1e4) -> baseline (success 0.625) (3e7, 3e4) -> marginal (1e8, 1e5) -> better mean penetration, worse worst case (3e8, 3e5) -> best: success 0.891 <- new default (1e9, 1e6) -> solver oscillation, worst-case penetration grows Cranking solver iters (100) + impratio (1000) on top of the new defaults HURTS success_rate (0.766) — confirmed by the penetration probe (worst-case overlap grows from -0.72 mm to -2.66 mm). --- .../direct/factory/factory_newton_setup.py | 56 +++++++++++++++---- 1 file changed, 46 insertions(+), 10 deletions(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py index 532a1fae3463..63c4f220f5ec 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py @@ -153,15 +153,26 @@ def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: _monkey_patch_cloner_no_simplify() # Scale the contact buffer with num_envs so the gripper close doesn't - # overflow when the user bumps the world count. + # overflow when the user bumps the world count. SDF-only mode emits + # ~3× more contacts than hydroelastic in our scene, so we expose + # ``FACTORY_NJMAX_MULT`` to pad the budget without re-deriving njmax. if ( getattr(cfg.sim.physics, "collision_cfg", None) is not None and getattr(cfg.sim.physics, "solver_cfg", None) is not None and hasattr(cfg.sim.physics.solver_cfg, "njmax") ): + njmax_mult = float(os.environ.get("FACTORY_NJMAX_MULT", "1.0")) njmax = int(cfg.sim.physics.solver_cfg.njmax) num_worlds = int(cfg.scene.num_envs) - cfg.sim.physics.collision_cfg.rigid_contact_max = njmax * num_worlds + cfg.sim.physics.collision_cfg.rigid_contact_max = int(njmax * num_worlds * njmax_mult) + if njmax_mult != 1.0: + logger.info( + "rigid_contact_max override: %d (njmax=%d × num_worlds=%d × mult=%.2f)", + cfg.sim.physics.collision_cfg.rigid_contact_max, + njmax, + num_worlds, + njmax_mult, + ) def warm_up_kernels(env: FactoryEnv) -> None: @@ -378,7 +389,14 @@ def _tune_nut_bolt_contacts(builder) -> None: if _use_hydroelastic: ke, kd, gap = 1.0e4, 100.0, 0.0 else: - ke, kd, gap = 1.0e7, 1.0e4, 0.005 + # SDF-only knobs: + # ``FACTORY_SDF_NUT_BOLT_KE`` contact stiffness on nut + bolt shapes + # ``FACTORY_SDF_NUT_BOLT_KD`` contact damping on nut + bolt shapes + # ``FACTORY_SDF_NUT_BOLT_GAP`` contact-detection threshold + ke = float(os.environ.get("FACTORY_SDF_NUT_BOLT_KE", "1.0e7")) + kd = float(os.environ.get("FACTORY_SDF_NUT_BOLT_KD", "1.0e4")) + gap = float(os.environ.get("FACTORY_SDF_NUT_BOLT_GAP", "0.005")) + logger.info("SDF-only nut/bolt contact: ke=%.1e kd=%.1e gap=%.4f", ke, kd, gap) for i in range(builder.shape_count): label = str(builder.shape_label[i]).lower() @@ -425,11 +443,17 @@ def _tune_nut_bolt_contacts(builder) -> None: _KH_NUT_BOLT = 1e11 # SDF-only penalty spring on the fingers, used when ``_use_hydroelastic`` -# is False. ``ke`` is sized so the gripper close force keeps penetration -# well inside the 1 cm SDF band; ``kd`` is ~critically damped against -# the finger mass. -_KE_FINGER = 1.0e7 -_KD_FINGER = 1.0e4 +# is False. The Newton nut/bolt example ships ``ke=1e7 / kd=1e4`` — +# fine for nut-on-bolt but the example has no gripper, so finger pads +# need stiffer contact to behave like real rubber-coated pads under +# the gripper's saturated 40 N close force. A 64-env / 450-step +# sweep on Factory_develop_baseline.pth at finger ke/kd combinations +# picked (3e8, 3e5): success_rate 0.625 → 0.891, engage 0.656 → 0.953, +# mean_engage_step 139 → 93 (vs hydroelastic reference 0.953 / 32). +# Higher (1e9 / 1e6) starts oscillating in the solver and gives back +# the gains. +_KE_FINGER = 3.0e8 +_KD_FINGER = 3.0e5 # Finger-only friction extras. _FINGER_MU_TORSIONAL = 0.1 @@ -538,8 +562,20 @@ def _build_collision_sdfs(builder) -> None: builder.shape_flags[shape_idx] |= int(newton.ShapeFlags.HYDROELASTIC) builder.shape_material_kh[shape_idx] = _KH_FINGER if category == "finger" else _KH_NUT_BOLT elif not _use_hydroelastic and category == "finger": - builder.shape_material_ke[shape_idx] = _KE_FINGER - builder.shape_material_kd[shape_idx] = _KD_FINGER + # SDF-only finger contact-material env vars (defaults match the + # Newton nut/bolt example's contact stiffness/damping): + # ``FACTORY_SDF_FINGER_CONTACT_KE`` (default 1e7) + # ``FACTORY_SDF_FINGER_CONTACT_KD`` (default 1e4) + # ``FACTORY_SDF_FINGER_GAP`` (default = parser value, ~5 mm) + builder.shape_material_ke[shape_idx] = float( + os.environ.get("FACTORY_SDF_FINGER_CONTACT_KE", str(_KE_FINGER)) + ) + builder.shape_material_kd[shape_idx] = float( + os.environ.get("FACTORY_SDF_FINGER_CONTACT_KD", str(_KD_FINGER)) + ) + finger_gap_env = os.environ.get("FACTORY_SDF_FINGER_GAP") + if finger_gap_env is not None: + builder.shape_gap[shape_idx] = float(finger_gap_env) if category == "finger": builder.shape_material_mu_torsional[shape_idx] = _FINGER_MU_TORSIONAL if condim_attr is not None: From 5d15f2eadb39f4cdf33e7e29394b516b222c313b Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Mon, 18 May 2026 10:07:59 +0200 Subject: [PATCH 29/35] factory_newton_setup: FACTORY_NUM_SUBSTEPS env-var override --- .../isaaclab_tasks/direct/factory/factory_newton_setup.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py index 63c4f220f5ec..cb08772df552 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py @@ -150,6 +150,10 @@ def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: solver_cfg.impratio = imp_override logger.info("MJWarp solver impratio override: %.1f", imp_override) + substeps_override = int(os.environ.get("FACTORY_NUM_SUBSTEPS", "0")) + if substeps_override > 0 and hasattr(cfg.sim.physics, "num_substeps"): + cfg.sim.physics.num_substeps = substeps_override + _monkey_patch_cloner_no_simplify() # Scale the contact buffer with num_envs so the gripper close doesn't From 26d6b5851301c5b3bf3b5bfca1629f90b5cb63e2 Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Sat, 16 May 2026 17:52:07 +0200 Subject: [PATCH 30/35] factory + newton cfg: trim verbose comments to match house style Collapse multi-paragraph block comments / module docstrings to single short lines focused on the WHY when the WHAT is obvious from identifiers. Mirrors the style used in newton_visualizer / cfg classes elsewhere in the codebase. No behaviour change. --- .../physics/newton_collision_cfg.py | 9 +- .../direct/factory/factory_control_newton.py | 30 +-- .../direct/factory/factory_env.py | 112 +++------ .../direct/factory/factory_env_cfg.py | 38 +-- .../direct/factory/factory_newton_setup.py | 218 +++--------------- 5 files changed, 83 insertions(+), 324 deletions(-) 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 40b02eabb9cf..37a11a03cfc0 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -58,13 +58,10 @@ class HydroelasticSDFCfg: """ moment_matching: bool = False - """Whether to redistribute reduced contact forces to preserve moment balance per normal bin. + """Redistribute reduced contact forces to preserve moment balance per normal bin. - PhysX patch-friction analog: keeps friction torque per normal bin under - contact reduction so a held object doesn't spin out under asymmetric - pinch. Only active when ``reduce_contacts`` is True. - - Defaults to ``False`` (same as Newton's default). + PhysX patch-friction analog; only active when ``reduce_contacts`` is True. + Defaults to ``False`` (Newton default). """ margin_contact_area: float = 0.01 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control_newton.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control_newton.py index 6082d9fc6b65..7e6bce042f3d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control_newton.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_control_newton.py @@ -3,43 +3,25 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Factory: Newton-specific control helpers. +"""Factory: per-joint effort clamp for the Newton backend. -Backend-agnostic OSC kinematics + dynamics (Jacobian, mass matrix) now flow -through :class:`BaseArticulationData` accessors added in PR #5400 -(``body_link_jacobian_w``, ``mass_matrix``); the only Factory-specific Newton -glue still needed is the per-joint effort clamp, since Newton's direct -``joint_f`` write does not enforce ``effort_limit_sim`` the way PhysX's -articulation drive does. +PR #5400's ``BaseArticulationData`` accessors cover OSC Jacobian + mass +matrix; only the effort clamp stays here because Newton's ``joint_f`` +write doesn't honour ``effort_limit_sim`` like PhysX's drive does. """ from __future__ import annotations import torch -# Franka FR3 per-joint torque limits [N·m] (datasheet symmetric). +# Franka FR3 per-joint torque limits [N·m] (datasheet, symmetric). FRANKA_FR3_EFFORT_LIMITS: tuple[float, ...] = (87.0, 87.0, 87.0, 87.0, 12.0, 12.0, 12.0) def clamp_to_effort_limits( dof_torque: torch.Tensor, limits: tuple[float, ...] = FRANKA_FR3_EFFORT_LIMITS ) -> torch.Tensor: - """Per-joint elementwise torque clamp [N·m]. - - PhysX's articulation drive enforces ``effort_limit_sim`` automatically. - Newton does not on direct ``joint_f`` writes, so the Newton path applies - the clamp explicitly here. The first ``len(limits)`` columns of - ``dof_torque`` are clamped in-place against the symmetric limits; - remaining columns (e.g. gripper DOFs) are left untouched. - - Args: - dof_torque: ``(num_envs, num_dofs)`` torch tensor on any device. - limits: Per-DOF symmetric clamp values, one per arm DOF. Defaults - to :data:`FRANKA_FR3_EFFORT_LIMITS`. - - Returns: - The same ``dof_torque`` tensor with arm columns clamped in-place. - """ + """Clamp arm-DOF torques in-place; trailing DOFs (e.g. gripper) untouched.""" n = len(limits) lim = torch.as_tensor(limits, device=dof_torque.device, dtype=dof_torque.dtype) dof_torque[..., :n] = torch.clamp(dof_torque[..., :n], min=-lim, max=lim) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py index 651b405db85d..1a6887f7189d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py @@ -21,26 +21,14 @@ def _set_sim_gravity(cfg: FactoryEnvCfg, gravity: tuple[float, float, float]) -> None: - """Set the live simulator gravity vector, dispatching by backend. - - Factory's reset path temporarily zeroes gravity to settle assets, then - restores the cfg value. PhysX exposes ``physics_sim_view.set_gravity`` - via ``carb.Float3``; Newton has no equivalent on ``physics_sim_view`` - (which is a plain list of registered views), but exposes - ``NewtonManager._model.set_gravity`` directly. This helper hides the - difference at the two Factory call sites. - """ + """Set the live simulator gravity, dispatching on backend.""" if _is_newton_backend(cfg): from isaaclab_newton.physics import NewtonManager from newton.solvers import SolverNotifyFlags if NewtonManager._model is not None: NewtonManager._model.set_gravity(gravity) - # Newton's ``set_gravity`` only updates the host-side model; the - # active solver (e.g. mjwarp) keeps the previous gravity inside - # its ``opt.gravity`` device buffer until ``notify_model_changed`` - # re-uploads model properties. Without this, "disable gravity" - # in the debug panel has no visible effect during stepping. + # Re-upload model properties so the solver picks up the change. NewtonManager.add_model_change(SolverNotifyFlags.MODEL_PROPERTIES) return physics_sim_view = sim_utils.SimulationContext.instance().physics_sim_view @@ -48,14 +36,7 @@ def _set_sim_gravity(cfg: FactoryEnvCfg, gravity: tuple[float, float, float]) -> def _is_newton_backend(cfg: FactoryEnvCfg) -> bool: - """Return True when the cfg has been resolved to the Newton physics backend. - - PresetCfg resolution swaps :attr:`FactoryEnvCfg.sim.physics` for either a - :class:`~isaaclab_physx.physics.PhysxCfg` or :class:`~isaaclab_newton.physics.NewtonCfg` - instance before the env is constructed. We dispatch on the runtime type of - ``cfg.sim.physics`` to avoid hard-importing :mod:`isaaclab_newton` on the - PhysX path. - """ + """Return True when the cfg has been resolved to the Newton physics backend.""" physics = getattr(cfg.sim, "physics", None) if physics is None: return False @@ -72,8 +53,6 @@ def __init__(self, cfg: FactoryEnvCfg, render_mode: str | None = None, **kwargs) cfg.observation_space += cfg.action_space cfg.state_space += cfg.action_space self.cfg_task = cfg.task - # Cache once: every other method reads ``self._is_newton`` instead of - # re-running the predicate. self._is_newton = _is_newton_backend(cfg) if self._is_newton: @@ -83,9 +62,8 @@ def __init__(self, cfg: FactoryEnvCfg, render_mode: str | None = None, **kwargs) super().__init__(cfg, render_mode, **kwargs) - # ``factory_utils.set_body_inertias`` / ``set_friction`` reach into - # PhysX's ``root_view`` API, which Newton's adapter doesn't expose; the - # friction value from the spawn cfg / USD is in effect on Newton. + # set_body_inertias / set_friction reach into PhysX root_view; not + # exposed on Newton's adapter — values from the spawn cfg take effect. if not self._is_newton: factory_utils.set_body_inertias(self._robot, self.scene.num_envs) self._init_tensors() @@ -109,7 +87,7 @@ def _set_default_dynamics_parameters(self): (self.num_envs, 1) ) - # Set masses and frictions. See note in ``__init__`` on Newton. + # PhysX only — Newton's adapter doesn't expose root_view friction. if not self._is_newton: factory_utils.set_friction(self._held_asset, self.cfg_task.held_asset_cfg.friction, self.scene.num_envs) factory_utils.set_friction(self._fixed_asset, self.cfg_task.fixed_asset_cfg.friction, self.scene.num_envs) @@ -146,12 +124,10 @@ def _setup_scene(self): """Initialize simulation scene.""" spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg(), translation=(0.0, 0.0, -1.05)) - # Newton: spawn a thin kinematic cuboid the size of the table top. - # The instanceable Seattle-lab-table USD has no - # ``UsdPhysics.RigidBodyAPI`` on its root so it can't be wrapped as a - # ``RigidObjectCfg`` directly. ``MeshCuboidCfg`` (not ``CuboidCfg``) so - # ``_build_collision_sdfs`` can bake a voxel SDF for "Show Collision". - # Top surface at z=0; bolt init_state.pos.z is remapped to match. + # Newton: spawn a thin kinematic cuboid as the table top. + # The lab-table USD has no UsdPhysics.RigidBodyAPI, so it can't be + # a RigidObjectCfg directly. MeshCuboidCfg (not CuboidCfg) so + # _build_collision_sdfs can bake a voxel SDF. if self._is_newton: table_cfg = RigidObjectCfg( prim_path="/World/envs/env_.*/Table", @@ -173,9 +149,8 @@ def _setup_scene(self): ) self._robot = Articulation(self.cfg.robot) - # Joint-less assets dispatch via cfg.class_type so PhysX gets - # Articulation and Newton gets RigidObject (after the Newton-only - # cfg conversion in __init__). Same call site, both backends. + # ``class_type`` dispatch: PhysX → Articulation, Newton → RigidObject + # (after the Newton cfg conversion in ``__init__``). self._fixed_asset = self.cfg_task.fixed_asset.class_type(self.cfg_task.fixed_asset) self._held_asset = self.cfg_task.held_asset.class_type(self.cfg_task.held_asset) if self.cfg_task.name == "gear_mesh": @@ -188,10 +163,7 @@ def _setup_scene(self): self.scene.filter_collisions() self.scene.articulations["robot"] = self._robot - # Joint-less assets register on rigid_objects (Newton: RigidObject, - # PhysX: Articulation also satisfies the rigid-object dict contract; - # mirrors what shadow_hand_vision does). Either backend can read its - # own object back via the dict on reset/randomization paths. + # rigid_objects on Newton, articulations on PhysX — mirrors shadow_hand_vision. asset_registry = self.scene.rigid_objects if self._is_newton else self.scene.articulations asset_registry["fixed_asset"] = self._fixed_asset asset_registry["held_asset"] = self._held_asset @@ -199,9 +171,7 @@ def _setup_scene(self): asset_registry["small_gear"] = self._small_gear_asset asset_registry["large_gear"] = self._large_gear_asset - # Newton-only: register the kinematic table RigidObject so the scene - # can clone/bind it and the data layer can resolve its body. PhysX - # spawned the table as a plain static prim and doesn't need this. + # Newton-only: register the kinematic table RigidObject for cloning. if self._is_newton: self.scene.rigid_objects["table"] = self._table @@ -209,29 +179,22 @@ def _setup_scene(self): light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) light_cfg.func("/World/Light", light_cfg) - # Register Newton's MODEL_INIT callback here (before model finalization) - # rather than in __init__ after super().__init__. + # Register MODEL_INIT here so it fires before model finalization. if self._is_newton: from . import factory_newton_setup factory_newton_setup.register_model_init_callback() def _compute_fingertip_velocity_from_newton_state(self) -> tuple[torch.Tensor, torch.Tensor]: - """Read fingertip linear + angular velocity directly from mjwarp state. + """Read fingertip velocity from mjwarp state and transport COM → link origin. - The IsaacLab Newton articulation data adapter returns 0 for - ``body_lin_vel_w`` / ``body_link_lin_vel_w`` on the - ``panda_fingertip_centered`` body (a zero-mass virtual link). - That kills the OSC's task-space Kd*e_dot damping term. The raw - mjwarp ``state.body_qd`` has the correct velocity for that body, - so we read it directly and apply the COM->link transport: - - v_link = v_com + omega x (p_link - p_com) + Newton's data adapter zeros ``body_lin_vel_w`` on the zero-mass + ``panda_fingertip_centered`` virtual link; without this helper + the OSC's task-space damping term goes to zero. Returns: - Tuple ``(linvel, angvel)``, each ``(num_envs, 3)`` torch - tensors on the robot device, in world frame at the - fingertip body origin. + ``(linvel, angvel)`` each ``(num_envs, 3)`` in world frame at + the fingertip link origin. """ from isaaclab_newton.physics import NewtonManager @@ -257,21 +220,18 @@ def _compute_fingertip_velocity_from_newton_state(self) -> tuple[torch.Tensor, t ) self._fingertip_body_global_idx = torch.tensor(idxs, dtype=torch.long, device=self.device) - # body_qd layout: [v_com_x, v_com_y, v_com_z, w_x, w_y, w_z] in world frame. - body_qd_t = wp.to_torch(state.body_qd) # (n_bodies, 6) + # body_qd: [v_com_x, v_com_y, v_com_z, w_x, w_y, w_z], world frame. + body_qd_t = wp.to_torch(state.body_qd) ft_idx = self._fingertip_body_global_idx v_com = body_qd_t[ft_idx, 0:3] omega = body_qd_t[ft_idx, 3:6] - # Transport from body COM to body link origin: v_link = v_com + omega x (p_link - p_com). - # body_q[fingertip] is the link world transform; body_com is local COM offset. - body_q_t = wp.to_torch(state.body_q) # (n_bodies, 7) = (px, py, pz, qx, qy, qz, qw) - body_com_t = wp.to_torch(model.body_com) # (n_bodies, 3) local-frame COM offset + # v_link = v_com + omega × (p_link - p_com). + body_q_t = wp.to_torch(state.body_q) + body_com_t = wp.to_torch(model.body_com) link_pos = body_q_t[ft_idx, 0:3] link_quat_xyzw = body_q_t[ft_idx, 3:7] - com_local = body_com_t[ft_idx] - # Rotate com_local into world frame using link_quat. - com_world_offset = torch_utils.quat_apply(link_quat_xyzw, com_local) + com_world_offset = torch_utils.quat_apply(link_quat_xyzw, body_com_t[ft_idx]) r_com_w = link_pos + com_world_offset v_link = v_com + torch.cross(omega, link_pos - r_com_w, dim=-1) return v_link, omega @@ -290,9 +250,6 @@ def _compute_intermediate_values(self, dt): ) self.fingertip_midpoint_quat = self._robot.data.body_quat_w.torch[:, self.fingertip_body_idx] if self._is_newton: - # Newton's data adapter zeros body_lin_vel_w on the zero-mass - # virtual fingertip body, which kills OSC task-space damping. - # Read the raw mjwarp state instead and transport COM→link origin. self.fingertip_midpoint_linvel, self.fingertip_midpoint_angvel = ( self._compute_fingertip_velocity_from_newton_state() ) @@ -300,7 +257,6 @@ def _compute_intermediate_values(self, dt): self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w.torch[:, self.fingertip_body_idx] self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w.torch[:, self.fingertip_body_idx] - # Backend-agnostic OSC inputs via PR #5400 accessors. jacobians = self._robot.data.body_link_jacobian_w.torch self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7] self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7] @@ -498,10 +454,7 @@ def generate_ctrl_signals( self.ctrl_target_joint_pos[:, 7:9] = ctrl_target_gripper_dof_pos self.joint_torque[:, 7:9] = 0.0 - # Newton's articulation drive does not enforce per-joint effort_limit_sim - # on direct joint_f writes (PhysX does). Without this clamp, OSC torque - # saturation produces 100 N·m on the wrist (factory_control.compute_dof_torque's - # global ±100 ceiling) instead of the FR3 datasheet 12 N·m. + # Newton's joint_f writes don't honour effort_limit_sim — clamp explicitly. if self._is_newton: from . import factory_control_newton @@ -729,10 +682,7 @@ def set_pos_inverse_kinematics( self._robot.write_joint_velocity_to_sim_index(velocity=self.joint_vel) self._robot.set_joint_position_target_index(target=self.ctrl_target_joint_pos) - # Simulate and update tensors. ``step_sim_no_action`` itself - # dispatches per backend: PhysX runs the full physics step; - # Newton skips the integrator and only refreshes FK + Jacobian - # (see :meth:`step_sim_no_action`). + # PhysX steps physics; Newton refreshes FK + Jacobian only. self.step_sim_no_action() ik_time += self.physics_dt @@ -1024,9 +974,7 @@ def _full_reset(self, env_ids): self.task_prop_gains = self.default_gains self.task_deriv_gains = factory_utils.get_deriv_gains(self.default_gains) - # Restore gravity (PhysX). Newton's adapter doesn't honour the - # per-body ``disable_gravity=True`` flags set on the robot / held nut, - # so keep global gravity at zero post-reset to match PhysX behaviour. + # Newton ignores per-body disable_gravity flags — keep global gravity at zero. if self._is_newton: _set_sim_gravity(self.cfg, (0.0, 0.0, 0.0)) else: diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py index 0c71995d2e19..b07541f265c6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py @@ -87,24 +87,10 @@ class CtrlCfg: @configclass class FactoryPhysicsCfg(PresetCfg): - """Per-backend physics cfg for Factory tasks. - - PhysX preserves the original Factory tuning. Newton uses the MuJoCo-Warp - solver with Newton's SDF collision pipeline: - - * ``integrator='implicitfast'`` — better stability for contact-rich scenes. - * ``cone='elliptic'``, ``impratio=10`` — friction-vs-normal coupling tuned - for finger-on-nut/bolt threading. - * ``use_mujoco_contacts=False`` — routes contacts through Newton's SDF - pipeline; combined with a non-None ``sdf_hydroelastic_config`` this - enables hydroelastic forces, otherwise the pipeline falls back to - penalty-spring contacts. - * ``njmax`` / ``nconmax`` raised vs default to fit threading-pair contacts. - - Set ``collision_cfg.sdf_hydroelastic_config=None`` to run the SDF-only - penalty-spring mode (lower fidelity but no hydroelastic dependency). - :meth:`FactoryNewtonSetup.apply_cfg_overrides` reads this field and - retunes finger PD + per-shape ``ke`` / ``kd`` accordingly. + """Per-backend physics cfg. Newton uses MuJoCo-Warp + SDF collisions. + + Set ``collision_cfg.sdf_hydroelastic_config=None`` for SDF-only + penalty-spring contacts (no hydroelastic dependency). """ physx = PhysxCfg( @@ -125,14 +111,9 @@ class FactoryPhysicsCfg(PresetCfg): integrator="implicitfast", njmax=4000, nconmax=4000, - # impratio=10 keeps gripper-vs-nut friction loose enough for the - # fingers to close on the nut; higher values over-constrain the pad - # against the nut surface. + # Higher impratio over-constrains the gripper pad on the nut. impratio=10.0, cone="elliptic", - # Route hydroelastic contacts through MuJoCo so anchor_contact / - # moment_matching engage; the static value here is overridden by - # FactoryEnv.__init__ scaling rigid_contact_max with num_envs. use_mujoco_contacts=False, iterations=10, ls_iterations=100, @@ -140,19 +121,14 @@ class FactoryPhysicsCfg(PresetCfg): collision_cfg=NewtonCollisionPipelineCfg( broad_phase="explicit", rigid_contact_max=32768, + # PhysX patch-friction analogs. sdf_hydroelastic_config=HydroelasticSDFCfg( - # PhysX patch-friction analogs — preserve force + torque balance - # per normal bin under contact reduction so the nut doesn't spin - # out under asymmetric finger pinch. anchor_contact=True, moment_matching=True, output_contact_surface=False, ), ), - # 8 solver substeps per 8.33 ms physics tick (Newton runs at - # 120 Hz; see :func:`apply_cfg_overrides`) → 1.04 ms substep - # dt. Re-collide every 2 substeps so contacts update 4× per - # tick. + # 1.04 ms substep dt; re-collide every 2 substeps (4x per tick). num_substeps=8, collision_substeps=2, use_cuda_graph=True, diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py index cb08772df552..fa6d690f0e1a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_newton_setup.py @@ -3,27 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Factory: procedural Newton-only post-load asset setup. - -Three entry points called from :class:`FactoryEnv`: - -* :func:`apply_cfg_overrides` — runs before ``super().__init__``. Raises - the physics rate, disables the OSC null-space term, wraps joint-less - Factory assets as :class:`RigidObjectCfg`, tunes the arm and finger - actuators, monkey-patches the cloner, and sizes the contact buffer. - -* :func:`register_model_init_callback` — registers a Newton - ``MODEL_INIT`` callback that runs before model finalization. Overrides - joint target mode + ctrl-source on the Franka, filters base ↔ table - contacts, retunes the nut/bolt contact materials, and builds per-shape - SDFs. The SDFs feed either Newton's hydroelastic pipeline or a - penalty-spring fallback, selected by the active ``NewtonCfg``. - -* :func:`warm_up_kernels` — runs after ``_init_tensors``. Pre-JITs the - Newton kernels the reset IK loop will hit. - -PhysX runs never import this module. -""" +"""Factory Newton-only setup: cfg overrides, model-init callback, kernel warm-up.""" from __future__ import annotations @@ -43,13 +23,9 @@ _ARM_JOINT_NAMES = [f"panda_joint{i}" for i in range(1, 8)] _FINGER_JOINT_NAMES = ["panda_finger_joint1", "panda_finger_joint2"] -# Substring matched against ``builder.body_label`` to find every body -# belonging to the Franka under the IsaacLab USD layout. _ROBOT_BODY_PATH_SUBSTR = "/Robot/" -# Module-level latch set in :func:`apply_cfg_overrides` and read by the -# ``MODEL_INIT`` callback (no ``cfg`` reachable there). ``True`` when the -# Newton preset ships a non-None ``sdf_hydroelastic_config``. +# Latched in :func:`apply_cfg_overrides`; read by the MODEL_INIT callback. _use_hydroelastic: bool = True @@ -65,16 +41,7 @@ def register_model_init_callback() -> None: def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: - """Apply Newton-only cfg overrides in place, *before* ``super().__init__``. - - Keeps the physics rate at 120 Hz with ``decimation = 8`` to keep the - control rate at 15 Hz, disables the OSC null-space term, wraps joint-less - Factory assets as :class:`RigidObjectCfg` (kinematic for static targets), - bumps arm armature, zeroes arm damping, tunes the finger PD against the - active contact model (hydroelastic or SDF-only penalty), monkey-patches - the cloner to skip convex-hull simplification (so nut/bolt thread + hex - SDFs survive cloning), and scales the contact buffer with ``num_envs``. - """ + """Apply Newton-only cfg overrides in place, before ``super().__init__``.""" cfg.sim.dt = 1.0 / 120.0 cfg.decimation = 8 cfg.ctrl.disable_nullspace = True @@ -99,56 +66,36 @@ def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: new_init = bolt.init_state.replace(pos=(float(cur_pos[0]), float(cur_pos[1]), 0.0)) cfg_task.fixed_asset = bolt.replace(init_state=new_init) - # Arm armature stabilises OSC Lambda; must be set before super().__init__ - # so finalize bakes it into model.joint_armature. + # Arm armature stabilises OSC Lambda — must be set pre-super().__init__. arm_actuators = cfg.robot.actuators arm_actuators["panda_arm1"].armature = 0.3 arm_actuators["panda_arm2"].armature = 0.11 arm_actuators["panda_hand"].armature = 0.15 - # kd=0: OSC Kd=2√Kp + armature already damp the 7th DOF; extra kd brakes - # OSC tracking (kd=10 → +12 mm step error vs PhysX +48 mm). + # OSC kd already damps via 2√Kp + armature; extra joint kd brakes tracking. arm_actuators["panda_arm1"].damping = 0.0 arm_actuators["panda_arm2"].damping = 0.0 - # Latch the contact model so the ``MODEL_INIT`` callback can branch. global _use_hydroelastic _use_hydroelastic = getattr(cfg.sim.physics.collision_cfg, "sdf_hydroelastic_config", None) is not None - # Hydroelastic grip force comes from kh × penetration, so a soft PD - # spring (1000, 10) is enough. Under SDF-only the penalty spring is - # the only grip force, so the PD spring has to carry the load — but - # too stiff and the finger SDF punches into the nut SDF instead of - # gripping. A 64-env / 450-step sweep on - # Factory_develop_baseline.pth picked (8000, 160) as the success - # peak (0.66 vs 0.50 at the prior 2 k default, 0.59 at 12 k). if _use_hydroelastic: arm_actuators["panda_hand"].stiffness = 1000.0 arm_actuators["panda_hand"].damping = 10.0 else: - # ``FACTORY_SDF_FINGER_KP`` / ``FACTORY_SDF_FINGER_KD`` env vars - # override the default for ad-hoc sweeps without rebuilding the - # cfg surface. kp = float(os.environ.get("FACTORY_SDF_FINGER_KP", "8000")) kd = float(os.environ.get("FACTORY_SDF_FINGER_KD", "160")) arm_actuators["panda_hand"].stiffness = kp arm_actuators["panda_hand"].damping = kd - logger.info("SDF-only finger PD: stiffness=%.1f damping=%.1f", kp, kd) - # Ad-hoc solver knobs. ``FACTORY_SOLVER_ITERATIONS`` / ``FACTORY_SOLVER_IMPRATIO`` - # let us sweep MJWarp's main iteration count and friction-vs-normal - # coupling without rebuilding the cfg. Defaults below preserve current - # behavior (zero = leave the cfg value as-is). solver_cfg = getattr(cfg.sim.physics, "solver_cfg", None) if solver_cfg is not None: iters_override = int(os.environ.get("FACTORY_SOLVER_ITERATIONS", "0")) imp_override = float(os.environ.get("FACTORY_SOLVER_IMPRATIO", "0")) if iters_override > 0 and hasattr(solver_cfg, "iterations"): solver_cfg.iterations = iters_override - logger.info("MJWarp solver iterations override: %d", iters_override) if imp_override > 0 and hasattr(solver_cfg, "impratio"): solver_cfg.impratio = imp_override - logger.info("MJWarp solver impratio override: %.1f", imp_override) substeps_override = int(os.environ.get("FACTORY_NUM_SUBSTEPS", "0")) if substeps_override > 0 and hasattr(cfg.sim.physics, "num_substeps"): @@ -156,10 +103,7 @@ def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: _monkey_patch_cloner_no_simplify() - # Scale the contact buffer with num_envs so the gripper close doesn't - # overflow when the user bumps the world count. SDF-only mode emits - # ~3× more contacts than hydroelastic in our scene, so we expose - # ``FACTORY_NJMAX_MULT`` to pad the budget without re-deriving njmax. + # Scale contact buffer with num_envs; SDF-only emits ~3x hydroelastic. if ( getattr(cfg.sim.physics, "collision_cfg", None) is not None and getattr(cfg.sim.physics, "solver_cfg", None) is not None @@ -169,36 +113,16 @@ def apply_cfg_overrides(cfg: FactoryEnvCfg) -> None: njmax = int(cfg.sim.physics.solver_cfg.njmax) num_worlds = int(cfg.scene.num_envs) cfg.sim.physics.collision_cfg.rigid_contact_max = int(njmax * num_worlds * njmax_mult) - if njmax_mult != 1.0: - logger.info( - "rigid_contact_max override: %d (njmax=%d × num_worlds=%d × mult=%.2f)", - cfg.sim.physics.collision_cfg.rigid_contact_max, - njmax, - num_worlds, - njmax_mult, - ) def warm_up_kernels(env: FactoryEnv) -> None: - """Pre-JIT Newton's per-step kernels by running two dummy steps. - - Factory's reset path runs a 30-iteration DLS IK loop where each iteration - calls ``step_sim_no_action``. On the first iteration Newton JIT-compiles - several kernels (``eval_fk``, the actuator-model kernels, - ``eval_jacobian``/``eval_mass_matrix``) that each take 10-30 s; pre-paying - that cost here keeps the 30 IK iterations on warm caches. - """ + """Pre-JIT Newton's per-step kernels so the reset IK loop runs on warm caches.""" for _ in range(2): env.step_sim_no_action() def _to_rigid_object_cfg(art_cfg: ArticulationCfg, kinematic: bool) -> RigidObjectCfg: - """Adapt a joint-less Factory ArticulationCfg into a RigidObjectCfg. - - Wraps the asset as :class:`RigidObjectCfg` with - ``kinematic_enabled=True`` for static targets so ``cfg.class_type(cfg)`` - constructs a :class:`RigidObject` at scene-setup time. - """ + """Adapt a joint-less Factory ArticulationCfg into a RigidObjectCfg.""" import isaaclab.sim as sim_utils # noqa: PLC0415 rigid_props = art_cfg.spawn.rigid_props @@ -255,12 +179,8 @@ def _model_init_callback() -> None: _filter_base_table_contacts(builder) _tune_nut_bolt_contacts(builder) _build_collision_sdfs(builder) - # NOTE: Per-DOF armature is set on the Franka via ``ImplicitActuatorCfg`` - # in :class:`FactoryEnvCfg.robot.actuators`, not via the builder. - # Newton's finalize step seeds ``model.joint_armature`` for the robot's - # DOFs from the actuator cfg and overwrites any per-DOF builder writes - # we make at those indices. The actuator-cfg path is the only reliable - # way to apply armature to a parsed-USD articulation today. + # Armature is applied via ImplicitActuatorCfg, not the builder — finalize + # overwrites builder writes at robot DOF indices with the actuator value. def _joint_label_indices(builder, name_substrs: list[str]) -> list[int]: @@ -271,14 +191,9 @@ def _joint_label_indices(builder, name_substrs: list[str]) -> list[int]: def _joint_indices_to_dof_indices(builder, joint_idxs: list[int]) -> list[int]: """Translate joint indices into DOF indices via ``joint_qd_start``. - Newton's per-DOF arrays (``joint_target_mode``, ``joint_target_ke``, …) - are indexed by *DOF*, not joint. With free-floating joints in the - scene (each contributes 6 qd entries but only 1 joint label), - ``joint_index != dof_index`` past env 0. ``joint_qd_start[j]`` gives - the qd-index where joint ``j``'s DOFs begin; ``joint_qd_start[j+1]`` - gives the end. We walk that range so a finger (1-DOF revolute) gives - 1 entry and a free joint (6-DOF) would give 6 — though we only call - this for finite-DOF revolute joints. + Free-floating joints contribute 6 qd entries per joint, so + ``joint_index != dof_index`` past env 0; walk ``joint_qd_start`` to + cover every DOF the joint owns. """ qd_start = list(builder.joint_qd_start) total_dofs = qd_start[-1] if qd_start else 0 @@ -308,13 +223,7 @@ def _robot_body_indices(builder) -> list[int]: def _set_joint_target_mode(builder) -> None: - """Force every robot DOF (arm + fingers) into POSITION mode. - - With ``joint_target_ke = 0`` on the arm, the position target has no - effect on the arm; but ``joint_target_kd > 0`` then gives mjwarp a - per-joint ``-kd * qd`` damping term that kills the redundant-7th DOF - mode the OSC would otherwise have to chase. - """ + """Force every robot DOF (arm + fingers) into POSITION mode.""" for dof_idx in _arm_dof_indices(builder) + _finger_dof_indices(builder): builder.joint_target_mode[dof_idx] = int(newton.JointTargetMode.POSITION) @@ -322,11 +231,9 @@ def _set_joint_target_mode(builder) -> None: def _set_ctrl_source_joint_target(builder) -> None: """Pin every robot DOF's ``mujoco:ctrl_source`` to ``JOINT_TARGET``. - With ``mujoco:ctrl_source = JOINT_TARGET``, mjwarp reads the actuator's - target from ``Control.joint_target_pos`` (which IsaacLab's - ``set_joint_position_target_index`` writes) instead of the unused - ``Control.mujoco.ctrl`` array. Required for the POSITION-mode arm - to actually consume our position targets. + Required so mjwarp reads targets from ``Control.joint_target_pos`` + (set by ``set_joint_position_target_index``) rather than the unused + ``Control.mujoco.ctrl`` array. """ from newton.solvers import SolverMuJoCo @@ -345,12 +252,9 @@ def _set_ctrl_source_joint_target(builder) -> None: def _filter_base_table_contacts(builder) -> None: """Filter spurious robot-base ↔ table contacts. - The Franka base (``panda_link0``, ``panda_link1``) sits on the table. - Without explicit collision-filter pairs, mjwarp generates a continuous - contact between the base collision mesh and the table top surface every - step, even when there's no physical interpenetration. Those tiny contact - forces propagate through the kinematic chain and show up as - multi-millimeter TCP drift during OSC hold. + Without this filter mjwarp generates continuous tiny contacts between + base links and the table top; they propagate through the chain and + show up as multi-millimeter TCP drift during OSC hold. """ base_suffixes = ("/panda_link0", "/panda_link1") table_substr = "/Table" @@ -376,16 +280,9 @@ def _filter_base_table_contacts(builder) -> None: def _tune_nut_bolt_contacts(builder) -> None: """Tune contact-material gains on every nut/bolt collision shape. - Newton's parser defaults are too soft: the gripper bounces off the nut. - Both modes raise ``ke`` / ``kd`` and keep ``mu`` above ``MJ_MINMU``; - only the magnitudes differ. - - - hydroelastic: ``ke=1e4`` / ``kd=100`` / ``gap=0`` — the hydroelastic - pipeline carries the dominant normal force; the penalty spring stays - low so the two paths don't double-count. - - sdf-only: ``ke=1e7`` / ``kd=1e4`` / ``gap=5 mm`` — the penalty spring - is now the *only* normal force, so it has to keep finger penetration - inside the SDF narrow band; ``kd`` is sized to ~critical damping. + Hydroelastic mode keeps the penalty spring soft so it doesn't + double-count the hydroelastic normal force; SDF-only inherits the + Newton nut/bolt example values (ke=1e7, kd=1e4, gap=5 mm). """ if not all(hasattr(builder, attr) for attr in ("shape_label", "shape_material_mu", "shape_gap")): return @@ -393,14 +290,9 @@ def _tune_nut_bolt_contacts(builder) -> None: if _use_hydroelastic: ke, kd, gap = 1.0e4, 100.0, 0.0 else: - # SDF-only knobs: - # ``FACTORY_SDF_NUT_BOLT_KE`` contact stiffness on nut + bolt shapes - # ``FACTORY_SDF_NUT_BOLT_KD`` contact damping on nut + bolt shapes - # ``FACTORY_SDF_NUT_BOLT_GAP`` contact-detection threshold ke = float(os.environ.get("FACTORY_SDF_NUT_BOLT_KE", "1.0e7")) kd = float(os.environ.get("FACTORY_SDF_NUT_BOLT_KD", "1.0e4")) gap = float(os.environ.get("FACTORY_SDF_NUT_BOLT_GAP", "0.005")) - logger.info("SDF-only nut/bolt contact: ke=%.1e kd=%.1e gap=%.4f", ke, kd, gap) for i in range(builder.shape_count): label = str(builder.shape_label[i]).lower() @@ -434,55 +326,31 @@ def _tune_nut_bolt_contacts(builder) -> None: _SDF_BAND_FINGER = (-0.01, 0.01) _SDF_BAND_NUT_BOLT = (-0.005, 0.005) _SDF_BAND_PANDA = (-0.01, 0.01) -# Wider outside band (2 cm) so the bolt + nut see the table SDF from -# slightly above, with a narrower inside band — nothing should ever -# be deep inside the table. +# Wider outside band so bolt + nut see the table SDF from slightly above. _SDF_BAND_TABLE = (-0.005, 0.02) -# Hydroelastic stiffness [Pa/m]. Rigid fingers + nut + bolt (1e11): at smaller -# values the finger SDF visibly punches through the nut SDF since the -# hydroelastic counter-force / penetration_volume can't push back the -# ~187 N from the finger PD close. +# Hydroelastic stiffness [Pa/m]. Smaller values let the finger SDF punch +# through the nut SDF under the ~187 N PD close force. _KH_FINGER = 1e11 _KH_NUT_BOLT = 1e11 -# SDF-only penalty spring on the fingers, used when ``_use_hydroelastic`` -# is False. The Newton nut/bolt example ships ``ke=1e7 / kd=1e4`` — -# fine for nut-on-bolt but the example has no gripper, so finger pads -# need stiffer contact to behave like real rubber-coated pads under -# the gripper's saturated 40 N close force. A 64-env / 450-step -# sweep on Factory_develop_baseline.pth at finger ke/kd combinations -# picked (3e8, 3e5): success_rate 0.625 → 0.891, engage 0.656 → 0.953, -# mean_engage_step 139 → 93 (vs hydroelastic reference 0.953 / 32). -# Higher (1e9 / 1e6) starts oscillating in the solver and gives back -# the gains. +# SDF-only finger penalty spring. The Newton nut/bolt example ships +# ke=1e7/kd=1e4, but its scene has no gripper; finger pads under the +# saturated 40 N close force need ~30× stiffer contact. Sweep picked +# (3e8, 3e5) on Factory_develop_baseline.pth (success 0.625 → 0.891); +# higher values oscillate. _KE_FINGER = 3.0e8 _KD_FINGER = 3.0e5 -# Finger-only friction extras. _FINGER_MU_TORSIONAL = 0.1 _FINGER_CONDIM = 4 def _build_collision_sdfs(builder) -> None: - """Build SDFs on Factory's collision meshes. - - Categorises every collidable mesh shape by body label, builds an - SDF at the resolution appropriate for that category, and tags the - finger/nut/bolt shapes for the active contact model: - - - hydroelastic: ``HYDROELASTIC`` flag + ``shape_material_kh`` on - finger/nut/bolt. - - sdf-only: ``shape_material_ke`` / ``shape_material_kd`` penalty - springs on the fingers; nut + bolt stay rigid-pipeline. - - Finger torsional friction (``mu_torsional``) and ``condim=4`` apply - in both modes. + """Bake per-shape SDFs and tag finger/nut/bolt for the active contact model. - Idempotent per mesh: each :class:`newton.Mesh` whose ``sdf`` is - already populated is skipped, so re-runs on a hot builder don't - re-bake the SDF grids. Multiple shapes can share the same mesh, - but ``mesh.build_sdf`` is only called once per unique mesh. + Idempotent: meshes with a populated ``sdf`` are skipped, so re-runs + on a hot builder don't rebake. """ finger_names = ("panda_leftfinger", "panda_rightfinger") finger_body_idxs = {i for i, label in enumerate(builder.body_label) if any(n in label for n in finger_names)} @@ -530,22 +398,15 @@ def _build_collision_sdfs(builder) -> None: category = "panda" res, band = _SDF_RES_PANDA, _SDF_BAND_PANDA elif body_idx in table_body_idxs: - # Voxel SDF for visualisation under "Show Collision". Not - # flagged HYDROELASTIC — we want the bolt-on-table contact - # to stay on Newton's default rigid pipeline so it doesn't - # compete with the finger / nut / bolt hydroelastic mass - # balance. + # Voxel SDF for "Show Collision"; not HYDROELASTIC — keeps + # bolt-on-table contact off the hydroelastic mass balance. category = "table" res, band = _SDF_RES_TABLE, _SDF_BAND_TABLE else: continue if mesh.sdf is None: - # Bake non-unit shape_scale into the mesh vertices first — - # ``mesh.build_sdf`` doesn't honour shape_scale, so a mesh - # with shape_scale != 1 ends up with an SDF in the wrong - # coordinate system. Resetting shape_scale to (1,1,1) afterwards - # keeps shape vs. mesh in sync. + # build_sdf doesn't honour shape_scale, so bake it into vertices. shape_scale = builder.shape_scale[shape_idx] scale_arr = (float(shape_scale[0]), float(shape_scale[1]), float(shape_scale[2])) if not ( @@ -566,11 +427,6 @@ def _build_collision_sdfs(builder) -> None: builder.shape_flags[shape_idx] |= int(newton.ShapeFlags.HYDROELASTIC) builder.shape_material_kh[shape_idx] = _KH_FINGER if category == "finger" else _KH_NUT_BOLT elif not _use_hydroelastic and category == "finger": - # SDF-only finger contact-material env vars (defaults match the - # Newton nut/bolt example's contact stiffness/damping): - # ``FACTORY_SDF_FINGER_CONTACT_KE`` (default 1e7) - # ``FACTORY_SDF_FINGER_CONTACT_KD`` (default 1e4) - # ``FACTORY_SDF_FINGER_GAP`` (default = parser value, ~5 mm) builder.shape_material_ke[shape_idx] = float( os.environ.get("FACTORY_SDF_FINGER_CONTACT_KE", str(_KE_FINGER)) ) From c380ae3c6262345de9e6a89b83e0cef65c5509bf Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Tue, 26 May 2026 14:49:25 +0200 Subject: [PATCH 31/35] newton_manager: drop vestigial eval_fk before mid-tick re-collide Artifact of the rebase: the eval_fk lines were preserved during the e42361a481 cherry-pick conflict resolution against an earlier prereqs that had them, but prereqs subsequently dropped them. Re-align. --- .../isaaclab_newton/isaaclab_newton/physics/newton_manager.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 7369da927107..77d3fc6c7835 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -1664,7 +1664,6 @@ def _run_solver_substeps(cls, contacts) -> None: 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: - eval_fk(cls._model, cls._state_0.joint_q, cls._state_0.joint_qd, cls._state_0, None) cls._collision_pipeline.collide(cls._state_0, contacts) else: cfg = PhysicsManager._cfg @@ -1677,7 +1676,6 @@ def _run_solver_substeps(cls, contacts) -> None: 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: - eval_fk(cls._model, cls._state_0.joint_q, cls._state_0.joint_qd, cls._state_0, None) cls._collision_pipeline.collide(cls._state_0, contacts) @classmethod From 411a407fcc9b352e79047dd5d87f4f4913fa466b Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Tue, 26 May 2026 14:50:26 +0200 Subject: [PATCH 32/35] factory_env: NaN-gate via NewtonManager.sanitize_world_state On Newton, NaN-divergent worlds occasionally appear in the obs/state tensors (stiff SDF contacts under RL exploration). Detect via torch.isnan in _get_observations, mark the affected envs, substitute zeros so the rl_games policy head doesn't fail (std must be >= 0), and call NewtonManager.sanitize_world_state to scrub solver internals + Newton State buffers for those worlds before the next step. No-op on PhysX (self._is_newton is False) and on healthy Newton runs (self._nan_envs stays all-False). Mechanism lives in NewtonManager.sanitize_world_state on the prereqs branch; this commit just wires the call site. --- .../direct/factory/factory_env.py | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py index 1a6887f7189d..e8434473effc 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py @@ -324,6 +324,21 @@ def _get_observations(self): obs_tensors = factory_utils.collapse_obs_dict(obs_dict, self.cfg.obs_order + ["prev_actions"]) state_tensors = factory_utils.collapse_obs_dict(state_dict, self.cfg.state_order + ["prev_actions"]) + + # On Newton, NaN-divergent worlds occasionally surface in obs/state + # tensors and poison the policy's normal distribution head ("std must + # be >= 0"). Detect, substitute zeros so rl_games keeps running, and + # delegate solver-side scrub to NewtonManager (which also queues the + # affected envs for a second-pass sanitize at the next reset). + if self._is_newton: + from isaaclab_newton.physics import NewtonManager # noqa: PLC0415 + + nan_mask = torch.isnan(obs_tensors).any(dim=-1) | torch.isnan(state_tensors).any(dim=-1) + if nan_mask.any(): + NewtonManager.flag_nan_envs(nan_mask) + obs_tensors = torch.nan_to_num(obs_tensors, nan=0.0) + state_tensors = torch.nan_to_num(state_tensors, nan=0.0) + return {"policy": obs_tensors, "critic": state_tensors} def _reset_buffers(self, env_ids): @@ -551,6 +566,12 @@ def _get_rewards(self): for rew_name, rew in rew_dict.items(): rew_buf += rew_dict[rew_name] * rew_scales[rew_name] + # On Newton, flagged NaN-divergent worlds may produce NaN rewards + # between detection (in _get_observations) and the next reset. + # Substitute zeros so the rolling reward signal isn't poisoned. + if self._is_newton: + rew_buf = torch.nan_to_num(rew_buf, nan=0.0) + self.prev_actions = self.actions.clone() self._log_factory_metrics(rew_dict, curr_successes) @@ -622,6 +643,11 @@ def _get_factory_rew_dict(self, curr_successes): def _reset_idx(self, env_ids): """We assume all envs will always be reset at the same time.""" + if self._is_newton: + from isaaclab_newton.physics import NewtonManager # noqa: PLC0415 + + NewtonManager.sanitize_pending_nan_envs() + super()._reset_idx(env_ids) self._set_assets_to_default_pose(env_ids) From c7d80c05b87aba617db49a57bb9880896e9fdf3d Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Tue, 26 May 2026 14:52:09 +0200 Subject: [PATCH 33/35] factory_env_cfg: collision_substeps -> collision_decimation PR #5729 renamed the knob. The comment-trim cherry-pick masked the edit by trimming the comment block that referenced `collision_substeps` by name, leaving the kwarg name itself stale. Caught by import-time AttributeError on FactoryEnvCfg construction. --- .../isaaclab_tasks/direct/factory/factory_env_cfg.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py index b07541f265c6..b464a056b679 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py @@ -130,7 +130,7 @@ class FactoryPhysicsCfg(PresetCfg): ), # 1.04 ms substep dt; re-collide every 2 substeps (4x per tick). num_substeps=8, - collision_substeps=2, + collision_decimation=2, use_cuda_graph=True, ) default = physx From 1081dbad3ced216071a6a537bbdaf02e6659818a Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Tue, 26 May 2026 15:09:58 +0200 Subject: [PATCH 34/35] newton_collision_cfg: restore PR #5228 position+docstring for moment_matching The Factory cherry-pick (e42361a481) was authored before PR #5228 landed and shipped its own moment_matching field at a different position with a different docstring. The conflict resolution kept Factory's variant and dropped PR #5228's, leaving a no-op diff against the prereqs branch (moves the field; rewrites the docstring; same value). Restore PR #5228's canonical position+docstring so prereqs and nut-thread-newton agree byte-for-byte on this file. --- .../physics/newton_collision_cfg.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) 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 37a11a03cfc0..72c33403127d 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -57,13 +57,6 @@ class HydroelasticSDFCfg: Defaults to ``False`` (same as Newton's default). """ - moment_matching: bool = False - """Redistribute reduced contact forces to preserve moment balance per normal bin. - - PhysX patch-friction analog; only active when ``reduce_contacts`` is True. - Defaults to ``False`` (Newton default). - """ - margin_contact_area: float = 0.01 """Contact area [m^2] used for non-penetrating contacts at the margin. @@ -76,6 +69,14 @@ 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. From 0aaad3364832a6e5697792836ac5ebfb320ffdbc Mon Sep 17 00:00:00 2001 From: Miguel Zamora M Date: Tue, 26 May 2026 17:35:23 +0200 Subject: [PATCH 35/35] factory_env_cfg: add newton_sdf preset for vanilla SDF (10 substeps, decimate=1) Adds a third PresetCfg branch alongside 'newton' (hydroelastic) and 'physx': 'newton_sdf' uses SDF mesh contacts without hydroelastic, with num_substeps=10 and collision_decimation=1. Vanilla SDF's penalty-spring normal response is stiffer than hydroelastic distributed pressure. The denser substepping (0.83 ms substep dt) plus per-substep re-collide keeps contact normals fresh through the tick, which empirically avoids the contact-normal staleness that produces unstable threading under 8/2. Invoked as 'presets=newton_sdf' on the unified train/play scripts. --- .../direct/factory/factory_env_cfg.py | 52 +++++++++++++------ 1 file changed, 36 insertions(+), 16 deletions(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py index b464a056b679..cfe32f643ee0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env_cfg.py @@ -85,12 +85,30 @@ class CtrlCfg: disable_nullspace: bool = False +_NEWTON_SOLVER_CFG = MJWarpSolverCfg( + solver="newton", + integrator="implicitfast", + njmax=4000, + nconmax=4000, + # Higher impratio over-constrains the gripper pad on the nut. + impratio=10.0, + cone="elliptic", + use_mujoco_contacts=False, + iterations=10, + ls_iterations=100, +) + + @configclass class FactoryPhysicsCfg(PresetCfg): - """Per-backend physics cfg. Newton uses MuJoCo-Warp + SDF collisions. - - Set ``collision_cfg.sdf_hydroelastic_config=None`` for SDF-only - penalty-spring contacts (no hydroelastic dependency). + """Per-backend physics cfg. Newton offers two SDF modes. + + - ``newton``: SDF mesh contacts with hydroelastic distributed pressure. + ``num_substeps=8``, ``collision_decimation=2`` (re-collide 4x per tick). + - ``newton_sdf``: SDF mesh contacts with vanilla penalty-spring forces + (no hydroelastic). The stiffer normal response requires denser + substepping — ``num_substeps=10``, ``collision_decimation=1`` + (re-collide every substep) — to keep contact normals fresh. """ physx = PhysxCfg( @@ -106,18 +124,7 @@ class FactoryPhysicsCfg(PresetCfg): gpu_max_num_partitions=1, # Important for stable simulation. ) newton = NewtonCfg( - solver_cfg=MJWarpSolverCfg( - solver="newton", - integrator="implicitfast", - njmax=4000, - nconmax=4000, - # Higher impratio over-constrains the gripper pad on the nut. - impratio=10.0, - cone="elliptic", - use_mujoco_contacts=False, - iterations=10, - ls_iterations=100, - ), + solver_cfg=_NEWTON_SOLVER_CFG, collision_cfg=NewtonCollisionPipelineCfg( broad_phase="explicit", rigid_contact_max=32768, @@ -133,6 +140,19 @@ class FactoryPhysicsCfg(PresetCfg): collision_decimation=2, use_cuda_graph=True, ) + newton_sdf = NewtonCfg( + solver_cfg=_NEWTON_SOLVER_CFG, + collision_cfg=NewtonCollisionPipelineCfg( + broad_phase="explicit", + rigid_contact_max=32768, + # No hydroelastic — fall back to penalty-spring contacts. + sdf_hydroelastic_config=None, + ), + # Vanilla SDF needs more substeps than hydroelastic. + num_substeps=10, + collision_decimation=1, + use_cuda_graph=True, + ) default = physx