diff --git a/source/isaaclab_newton/config/extension.toml b/source/isaaclab_newton/config/extension.toml index 5fbc20c99341..fe59ab3dfbb2 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.12" +version = "0.5.13" # 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 371ce6f91e2a..72715c38aaca 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,25 @@ Changelog --------- +0.5.13 (2026-04-13) +~~~~~~~~~~~~~~~~~~~ + +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.12 (2026-04-13) ~~~~~~~~~~~~~~~~~~~ @@ -22,7 +41,6 @@ Added registration, wildcard body matching, and zero-copy transform views. - 0.5.10 (2026-04-05) ~~~~~~~~~~~~~~~~~~~ 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..c8b0db0b3f4a --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -0,0 +1,186 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for Newton collision pipeline.""" + +from __future__ import annotations + +from typing import Any, Literal + +from isaaclab.utils import configclass + + +@configclass +class HydroelasticSDFCfg: + """Configuration for SDF-based hydroelastic collision handling. + + Hydroelastic contacts generate distributed contact areas instead of point contacts, + providing more realistic force distribution for manipulation and compliant surfaces. + + For more details, see the `Newton Collisions Guide`_. + + .. _Newton Collisions Guide: https://newton-physics.github.io/newton/latest/concepts/collisions.html#hydroelastic-contacts + """ + + reduce_contacts: bool = True + """Whether to reduce contacts to a smaller representative set per shape pair. + + When False, all generated contacts are passed through without reduction. + + Defaults to ``True`` (same as Newton's default). + """ + + buffer_fraction: float = 1.0 + """Fraction of worst-case hydroelastic buffer allocations. Range: (0, 1]. + + Lower values reduce memory usage but may cause overflows in dense scenes. + Overflows are bounds-safe and emit warnings; increase this value when warnings appear. + + Defaults to ``1.0`` (same as Newton's default). + """ + + normal_matching: bool = True + """Whether to rotate reduced contact normals to align with aggregate force direction. + + Only active when ``reduce_contacts`` is True. + + Defaults to ``True`` (same as Newton's default). + """ + + anchor_contact: bool = False + """Whether to add an anchor contact at the center of pressure for each normal bin. + + The anchor contact helps preserve moment balance. Only active when ``reduce_contacts`` is True. + + Defaults to ``False`` (same as Newton's default). + """ + + margin_contact_area: float = 0.01 + """Contact area [m^2] used for non-penetrating contacts at the margin. + + Defaults to ``0.01`` (same as Newton's default). + """ + + output_contact_surface: bool = False + """Whether to output hydroelastic contact surface vertices for visualization. + + Defaults to ``False`` (same as Newton's default). + """ + + +@configclass +class NewtonCollisionPipelineCfg: + """Configuration for Newton collision pipeline. + + Full-featured collision pipeline with GJK/MPR narrow phase and pluggable broad phase. + When this config is set on :attr:`NewtonCfg.collision_cfg`: + + - **MJWarpSolverCfg**: Newton's collision pipeline replaces MuJoCo's internal contact solver. + - **Other solvers** (XPBD, Featherstone, etc.): Configures the collision pipeline parameters + (these solvers always use Newton's collision pipeline). + + Key features: + + - GJK/MPR algorithms for convex-convex collision detection + - Multiple broad phase options: NXN (all-pairs), SAP (sweep-and-prune), EXPLICIT (precomputed pairs) + - Mesh-mesh collision via SDF with contact reduction + - Optional hydroelastic contact model for compliant surfaces + + For more details, see the `Newton Collisions Guide`_ and `CollisionPipeline API`_. + + .. _Newton Collisions Guide: https://newton-physics.github.io/newton/latest/concepts/collisions.html + .. _CollisionPipeline API: https://newton-physics.github.io/newton/api/_generated/newton.CollisionPipeline.html + """ + + broad_phase: Literal["explicit", "nxn", "sap"] = "explicit" + """Broad phase algorithm for collision detection. + + Options: + + - ``"explicit"``: Use precomputed shape pairs from ``model.shape_contact_pairs``. + - ``"nxn"``: All-pairs brute force. Simple but O(n^2) complexity. + - ``"sap"``: Sweep-and-prune. Good for scenes with many dynamic objects. + + Defaults to ``"explicit"`` (same as Newton's default when ``broad_phase=None``). + """ + + reduce_contacts: bool = True + """Whether to reduce contacts for mesh-mesh collisions. + + When True, uses shared memory contact reduction to select representative contacts. + Improves performance and stability for meshes with many vertices. + + Defaults to ``True`` (same as Newton's default). + """ + + rigid_contact_max: int | None = None + """Maximum number of rigid contacts to allocate. + + Resolution order: + + 1. If provided, use this value. + 2. Else if ``model.rigid_contact_max > 0``, use the model value. + 3. Else estimate automatically from model shape and pair metadata. + + Defaults to ``None`` (auto-estimate, same as Newton's default). + """ + + max_triangle_pairs: int = 1_000_000 + """Maximum number of triangle pairs allocated by narrow phase for mesh and heightfield collisions. + + Increase this when scenes with large/complex meshes or heightfields report + triangle-pair overflow warnings. + + Defaults to ``1_000_000`` (same as Newton's default). + """ + + soft_contact_max: int | None = None + """Maximum number of soft contacts to allocate. + + If None, computed as ``shape_count * particle_count``. + + Defaults to ``None`` (auto-compute, same as Newton's default). + """ + + soft_contact_margin: float = 0.01 + """Margin [m] for soft contact generation. + + Defaults to ``0.01`` (same as Newton's default). + """ + + requires_grad: bool | None = None + """Whether to enable gradient computation for collision. + + If ``None``, uses ``model.requires_grad``. + + Defaults to ``None`` (same as Newton's default). + """ + + sdf_hydroelastic_config: HydroelasticSDFCfg | None = None + """Configuration for SDF-based hydroelastic collision handling. + + If ``None``, hydroelastic contacts are disabled. + If set, enables hydroelastic contacts with the specified parameters. + + Defaults to ``None`` (hydroelastic disabled, same as Newton's default). + """ + + def to_pipeline_args(self) -> dict[str, Any]: + """Build keyword arguments for :class:`newton.CollisionPipeline`. + + Converts this configuration into the dict expected by + ``CollisionPipeline.__init__``, handling nested config conversion + (e.g. :class:`HydroelasticSDFCfg` → ``HydroelasticSDF.Config``). + + Returns: + Keyword arguments suitable for ``CollisionPipeline(model, **args)``. + """ + from newton.geometry import HydroelasticSDF + + cfg_dict = self.to_dict() + hydro_cfg = cfg_dict.pop("sdf_hydroelastic_config", None) + if hydro_cfg is not None: + cfg_dict["sdf_hydroelastic_config"] = HydroelasticSDF.Config(**hydro_cfg) + return cfg_dict diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index b8832cda9bd2..f08c8a506c79 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -40,6 +40,8 @@ if TYPE_CHECKING: from isaaclab.sim.simulation_context import SimulationContext + from .newton_collision_cfg import NewtonCollisionPipelineCfg + logger = logging.getLogger(__name__) # Tagged union for entries in _cl_site_index_map. @@ -97,6 +99,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 _newton_frame_transform_sensors: list = [] # List of SensorFrameTransform _report_contacts: bool = False @@ -385,6 +388,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._newton_frame_transform_sensors = [] cls._report_contacts = False @@ -725,7 +729,11 @@ def _initialize_contacts(cls) -> None: if cls._needs_collision_pipeline: # Newton collision pipeline: create pipeline and generate contacts if cls._collision_pipeline is None: - cls._collision_pipeline = CollisionPipeline(cls._model, broad_phase="explicit") + if cls._collision_cfg is not None: + cls._collision_pipeline = CollisionPipeline(cls._model, **cls._collision_cfg.to_pipeline_args()) + else: + cls._collision_pipeline = CollisionPipeline(cls._model, broad_phase="explicit") + if cls._contacts is None: cls._contacts = cls._collision_pipeline.contacts() @@ -785,19 +793,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`). + """