From 957188a6cd16ef197d368233f0e1dc69bd23fa3e Mon Sep 17 00:00:00 2001 From: ruziniuuuuu Date: Mon, 27 Jul 2026 18:51:15 +0800 Subject: [PATCH 1/2] Add stable VBD rigid coupling controls --- CONTRIBUTORS.md | 1 + .../changelog.d/ruzini-vbd-coupling.minor.rst | 11 ++ .../isaaclab_contrib/coupling/coupler.py | 77 ++++++++++- .../deformable/newton_manager_cfg.py | 24 ++++ .../deformable/vbd_manager.py | 63 ++++++++- .../test/coupling/test_coupler.py | 82 ++++++++++- .../test_deformable_builder_hooks.py | 128 ++++++++++++++++++ .../ruzini-contact-matching.minor.rst | 5 + .../physics/newton_collision_cfg.py | 9 ++ .../test_newton_manager_abstraction.py | 15 ++ 10 files changed, 407 insertions(+), 8 deletions(-) create mode 100644 source/isaaclab_contrib/changelog.d/ruzini-vbd-coupling.minor.rst create mode 100644 source/isaaclab_newton/changelog.d/ruzini-contact-matching.minor.rst diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 8de19bd2b8f1..ee95b7a0b002 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -178,6 +178,7 @@ Guidelines for modifications: * Rosario Scalise * Ruben D'Sa * Ruben Grandia +* ruziniuuuuu * Ryan Gresia * Ryley McCarroll * Sahara Yuta diff --git a/source/isaaclab_contrib/changelog.d/ruzini-vbd-coupling.minor.rst b/source/isaaclab_contrib/changelog.d/ruzini-vbd-coupling.minor.rst new file mode 100644 index 000000000000..052cb7e3dae2 --- /dev/null +++ b/source/isaaclab_contrib/changelog.d/ruzini-vbd-coupling.minor.rst @@ -0,0 +1,11 @@ +Added +^^^^^ + +* Added rigid-contact stability controls to + :class:`~isaaclab_contrib.deformable.VBDSolverCfg`. + +Fixed +^^^^^ + +* Fixed replicated VBD builder coloring and synchronized generalized + coordinates for non-cable articulations owned by VBD coupling entries. diff --git a/source/isaaclab_contrib/isaaclab_contrib/coupling/coupler.py b/source/isaaclab_contrib/isaaclab_contrib/coupling/coupler.py index 7d39367e5b31..4b20c0f91f0e 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/coupling/coupler.py +++ b/source/isaaclab_contrib/isaaclab_contrib/coupling/coupler.py @@ -10,6 +10,9 @@ from dataclasses import dataclass from functools import partial +import newton +import numpy as np +import warp as wp from isaaclab_newton.physics import ( KaminoSolverCfg, MJWarpSolverCfg, @@ -19,12 +22,21 @@ ) from isaaclab_newton.physics.mpm_manager import NewtonMPMManager from isaaclab_newton.physics.newton_manager import NewtonManager -from newton import CollisionPipeline, Model, ModelBuilder, ShapeFlags +from newton import ( + CollisionPipeline, + Contacts, + Control, + Model, + ModelBuilder, + ShapeFlags, + State, +) from newton.solvers.experimental.coupled import SolverCoupled, SolverCoupledADMM, SolverCoupledProxy from isaaclab.physics import PhysicsManager from isaaclab.utils.string import resolve_matching_names +from ..deformable.newton_manager_cfg import VBDSolverCfg from ..deformable.vbd_manager import NewtonVBDManager from .coupler_cfg import ( CouplerAdmmCfg, @@ -79,6 +91,15 @@ def _build_solver(cls, model: Model, solver_cfg: CouplerCfg) -> None: cls._validate_config(solver_cfg) resolved_entries = [cls._resolve_entry(model, entry) for entry in solver_cfg.entries] + vbd_entries = [entry for entry in resolved_entries if isinstance(entry.config.solver_cfg, VBDSolverCfg)] + if vbd_entries: + coupled_ik_mask = cls._make_coupled_ik_articulation_mask( + model, + vbd_entries, + ) + cls._ik_articulation_mask = coupled_ik_mask if bool(np.any(coupled_ik_mask.numpy())) else None + else: + cls._ik_articulation_mask = None proxies: list[CouplerProxyMappingCfg] = [] active_proxy_destinations: set[str] = set() if isinstance(solver_cfg, CouplerProxyCfg): @@ -113,6 +134,57 @@ def _build_solver(cls, model: Model, solver_cfg: CouplerCfg) -> None: NewtonManager._supports_contact_sensors = False NewtonManager._needs_collision_pipeline = needs_collision_pipeline + @classmethod + def _step_solver( + cls, + state_in: State, + state_out: State, + control: Control, + contacts: Contacts | None, + substep_dt: float, + ) -> None: + """Advance all entries and publish VBD-owned generalized coordinates.""" + cls._solver.step(state_in, state_out, control, contacts, substep_dt) + if cls._ik_articulation_mask is not None: + newton.eval_ik( + cls._model, + state_out, + state_out.joint_q, + state_out.joint_qd, + mask=cls._ik_articulation_mask, + ) + + @classmethod + def _make_coupled_ik_articulation_mask( + cls, + model: Model, + entries: list[_ResolvedEntry], + ) -> wp.array: + """Select non-cable articulations owned by nested VBD managers.""" + vbd_owned_joints: set[int] = set() + for entry in entries: + if isinstance(entry.config.solver_cfg, VBDSolverCfg): + vbd_owned_joints.update(entry.joints) + + mask = NewtonVBDManager._make_ik_articulation_mask(model).numpy() + articulation_starts = np.asarray( + model.articulation_start.numpy(), + dtype=np.int32, + ) + articulation_ends = np.asarray( + model.articulation_end.numpy(), + dtype=np.int32, + ) + for articulation_index, (start, end) in enumerate( + zip( + articulation_starts[: model.articulation_count], + articulation_ends, + strict=True, + ) + ): + mask[articulation_index] &= any(joint in vbd_owned_joints for joint in range(int(start), int(end))) + return wp.array(mask, dtype=wp.bool, device=model.device) + @classmethod def _validate_config(cls, solver_cfg: CouplerCfg) -> None: """Validate adapter-specific nested-manager constraints before construction.""" @@ -197,8 +269,9 @@ def _register_builder_attributes(cls, builder: ModelBuilder) -> None: @classmethod def _prepare_builder_for_finalize(cls, builder: ModelBuilder) -> None: """Normalize kinematic colliders when a coupled entry uses implicit MPM.""" - super()._prepare_builder_for_finalize(builder) solver_cfg = getattr(PhysicsManager._cfg, "solver_cfg", None) + if any(isinstance(entry.solver_cfg, VBDSolverCfg) for entry in getattr(solver_cfg, "entries", ())): + NewtonVBDManager._prepare_builder_for_finalize(builder) if any(isinstance(entry.solver_cfg, MPMSolverCfg) for entry in getattr(solver_cfg, "entries", ())): NewtonMPMManager._prepare_builder_for_finalize(builder) diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py index 3a69f11884de..d9ad289aacb4 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py @@ -117,6 +117,30 @@ class VBDSolverCfg(NewtonModelSolverCfg): rigid_contact_k_start: float = 1.0e2 """Initial stiffness seed for all rigid body contacts [N/m].""" + rigid_contact_hard: bool = True + """Whether rigid contacts use the augmented-Lagrangian hard-contact model. + + Disable this for compliant penalty contacts, such as moving kinematic + proxies grasping a light rigid object. + """ + + rigid_contact_history: bool = False + """Whether rigid contacts warm-start penalties, duals, and stick anchors.""" + + rigid_avbd_contact_alpha: float | None = None + """Body-contact C0 stabilization strength in ``[0, 1]``. + + ``None`` uses SolverVBD's shared ``rigid_avbd_alpha`` default. Lower values + correct more current penetration and strengthen contacts when history is + disabled or the iteration count is modest. + """ + + rigid_avbd_beta: float = 0.0 + """Penalty ramp rate per rigid-body AVBD iteration; zero disables ramping.""" + + rigid_body_contact_buffer_size: int = 64 + """Maximum rigid contacts stored per body during one VBD solve.""" + @configclass class CoupledMJWarpVBDSolverCfg(NewtonModelSolverCfg): diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py index 02569d4906a2..fd2b1764e5c0 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py @@ -10,9 +10,11 @@ import logging from typing import TYPE_CHECKING +import newton +import numpy as np import warp as wp from isaaclab_newton.physics.newton_manager import NewtonManager -from newton import Model +from newton import Model, ModelBuilder from newton._src.usd.schemas import SchemaResolverNewton, SchemaResolverPhysx from newton.solvers import SolverVBD @@ -56,6 +58,8 @@ class NewtonVBDManager(NewtonManager): Always uses Newton's :class:`CollisionPipeline` for contact handling. """ + _ik_articulation_mask: wp.array | None = None + @classmethod def initialize(cls, sim_context: SimulationContext) -> None: """Initialize the manager with simulation context. @@ -78,6 +82,7 @@ def initialize(cls, sim_context: SimulationContext) -> None: @classmethod def _solver_specific_clear(cls): """Clear VBD-specific state.""" + cls._ik_articulation_mask = None clear_deformable_builder_hooks() @classmethod @@ -228,12 +233,13 @@ def instantiate_builder_from_stage(cls): } NewtonManager._num_envs = len(env_paths) - # Coloring is required by the VBD solver for particles and VBD-integrated bodies. - # Safe without particles: color() skips particle coloring when particle_count == 0. - builder.color() - cls.set_builder(builder) + @classmethod + def _prepare_builder_for_finalize(cls, builder: ModelBuilder) -> None: + """Build the vertex and rigid-body coloring required by VBD.""" + builder.color() + @classmethod def _create_solver(cls, model: Model, solver_cfg: VBDSolverCfg) -> SolverVBD: """Construct the configured VBD solver.""" @@ -249,6 +255,53 @@ def _build_solver(cls, model: Model, solver_cfg: VBDSolverCfg) -> None: NewtonManager._solver = cls._create_solver(model, solver_cfg) NewtonManager._use_single_state = False NewtonManager._needs_collision_pipeline = True + articulation_mask = cls._make_ik_articulation_mask(model) + cls._ik_articulation_mask = articulation_mask if bool(np.any(articulation_mask.numpy())) else None + + @staticmethod + def _make_ik_articulation_mask(model: Model) -> wp.array: + """Select articulations whose generalized coordinates support eval_ik.""" + joint_types = np.asarray(model.joint_type.numpy(), dtype=np.int32) + articulation_starts = np.asarray(model.articulation_start.numpy(), dtype=np.int32) + articulation_ends = np.asarray(model.articulation_end.numpy(), dtype=np.int32) + mask = np.ones((model.articulation_count,), dtype=bool) + for articulation_index, (start, end) in enumerate( + zip( + articulation_starts[: model.articulation_count], + articulation_ends, + strict=True, + ) + ): + articulation_joint_types = joint_types[int(start) : int(end)] + has_cable = np.any(articulation_joint_types == int(newton.JointType.CABLE)) + has_generalized_joint = np.any( + np.isin( + articulation_joint_types, + ( + int(newton.JointType.REVOLUTE), + int(newton.JointType.PRISMATIC), + int(newton.JointType.BALL), + int(newton.JointType.FREE), + int(newton.JointType.DISTANCE), + int(newton.JointType.D6), + ), + ) + ) + mask[articulation_index] = has_generalized_joint and not has_cable + return wp.array(mask, dtype=wp.bool, device=model.device) + + @classmethod + def _step_solver(cls, state_in, state_out, control, contacts, substep_dt: float) -> None: + """Advance VBD and synchronize generalized articulation coordinates.""" + cls._solver.step(state_in, state_out, control, contacts, substep_dt) + if cls._ik_articulation_mask is not None: + newton.eval_ik( + cls._model, + state_out, + state_out.joint_q, + state_out.joint_qd, + mask=cls._ik_articulation_mask, + ) @classmethod def _simulate_physics_only(cls) -> None: diff --git a/source/isaaclab_contrib/test/coupling/test_coupler.py b/source/isaaclab_contrib/test/coupling/test_coupler.py index cf4a3a10873f..108c33fe4b03 100644 --- a/source/isaaclab_contrib/test/coupling/test_coupler.py +++ b/source/isaaclab_contrib/test/coupling/test_coupler.py @@ -28,7 +28,7 @@ XPBDSolverCfg, ) from isaaclab_newton.physics.newton_manager import NewtonManager -from newton import ShapeFlags +from newton import JointType, ShapeFlags from newton.solvers.experimental.coupled import SolverCoupledADMM, SolverCoupledProxy from isaaclab_contrib.coupling import ( @@ -69,6 +69,86 @@ def test_public_coupling_exports_are_importable(): assert getattr(coupling, name) is not None +def test_coupler_step_reconstructs_selected_vbd_joint_state(monkeypatch): + """Coupled VBD bodies publish coherent generalized root coordinates.""" + calls = [] + model = object() + articulation_mask = object() + state_out = SimpleNamespace(joint_q=object(), joint_qd=object()) + + class _Solver: + def step(self, *args): + calls.append(("step", args)) + + monkeypatch.setattr(NewtonManager, "_solver", _Solver()) + monkeypatch.setattr(NewtonManager, "_model", model) + monkeypatch.setattr( + NewtonCouplerManager, + "_ik_articulation_mask", + articulation_mask, + raising=False, + ) + monkeypatch.setattr( + "newton.eval_ik", + lambda *args, **kwargs: calls.append(("eval_ik", args, kwargs)), + ) + + state_in = object() + control = object() + contacts = object() + NewtonCouplerManager._step_solver( + state_in, + state_out, + control, + contacts, + 0.001, + ) + + assert [call[0] for call in calls] == ["step", "eval_ik"] + assert calls[1][1] == (model, state_out, state_out.joint_q, state_out.joint_qd) + assert calls[1][2] == {"mask": articulation_mask} + + +def test_coupler_selects_only_vbd_owned_non_cable_articulations(): + """MJWarp state and native cable coordinates stay outside VBD IK sync.""" + + model = SimpleNamespace( + joint_type=_FakeArray( + np.asarray( + (JointType.REVOLUTE, JointType.CABLE, JointType.FREE), + dtype=np.int32, + ) + ), + articulation_start=_FakeArray(np.asarray((0, 1, 2, 3), dtype=np.int32)), + articulation_end=_FakeArray(np.asarray((1, 2, 3), dtype=np.int32)), + articulation_count=3, + device="cpu", + ) + entries = [ + NewtonCouplerManager._ResolvedEntry( + config=CouplerEntryCfg(name="robot", solver_cfg=MJWarpSolverCfg()), + bodies=[], + particles=[], + joints=[0], + shapes=[], + ), + NewtonCouplerManager._ResolvedEntry( + config=CouplerEntryCfg(name="cable", solver_cfg=VBDSolverCfg()), + bodies=[], + particles=[], + joints=[1, 2], + shapes=[], + ), + ] + + mask = NewtonCouplerManager._make_coupled_ik_articulation_mask( + model, + entries, + ) + + assert mask.numpy().tolist() == [False, False, True] + + @dataclass class _FakeModel: """Fields consulted by the coupler's pure configuration helpers.""" diff --git a/source/isaaclab_contrib/test/deformable/test_deformable_builder_hooks.py b/source/isaaclab_contrib/test/deformable/test_deformable_builder_hooks.py index fa886612ca1e..630af40c2256 100644 --- a/source/isaaclab_contrib/test/deformable/test_deformable_builder_hooks.py +++ b/source/isaaclab_contrib/test/deformable/test_deformable_builder_hooks.py @@ -7,11 +7,13 @@ import sys from types import SimpleNamespace +import numpy as np import pytest import warp as wp from isaaclab_newton.cloner.replicate import NewtonReplicateContext from isaaclab_newton.physics import NewtonManager from isaaclab_newton.sim.spawners.materials import NewtonDeformableMaterialCfg +from newton import JointType from isaaclab_contrib.deformable import DeformableObject, VBDSolverCfg from isaaclab_contrib.deformable.deformable_object import ( @@ -19,6 +21,7 @@ add_deformable_entry_to_builder, setup_registered_deformable_fabric_sync, ) +from isaaclab_contrib.deformable.vbd_manager import NewtonVBDManager class _FakeBuilder: @@ -31,6 +34,14 @@ def add_cloth_mesh(self, **kwargs) -> None: self.particle_count += len(kwargs["vertices"]) +class _ColoringBuilder: + def __init__(self): + self.color_calls = 0 + + def color(self) -> None: + self.color_calls += 1 + + class _FakePath: def __init__(self, path: str): self.pathString = path @@ -145,6 +156,123 @@ def test_newton_physics_context_is_replicate_context(): assert PHYSICS_CONTEXT is NewtonReplicateContext +def test_vbd_manager_colors_replicated_builder_before_finalize(): + """VBD prepares builders created by the physics replication path.""" + builder = _ColoringBuilder() + + NewtonVBDManager._prepare_builder_for_finalize(builder) + + assert builder.color_calls == 1 + + +def test_vbd_cfg_exposes_rigid_contact_stability_controls(): + """Coupled rigid scenes can configure the public SolverVBD stability knobs.""" + cfg = VBDSolverCfg( + rigid_contact_hard=False, + rigid_contact_history=True, + rigid_avbd_contact_alpha=0.0, + rigid_avbd_beta=100.0, + rigid_body_contact_buffer_size=256, + ) + + assert cfg.rigid_contact_hard is False + assert cfg.rigid_contact_history is True + assert cfg.rigid_avbd_contact_alpha == pytest.approx(0.0) + assert cfg.rigid_avbd_beta == pytest.approx(100.0) + assert cfg.rigid_body_contact_buffer_size == 256 + + +def test_vbd_manager_reconstructs_joint_state_after_maximal_solver_step(monkeypatch): + """VBD keeps generalized articulation state synchronized with body poses.""" + calls = [] + state_in = SimpleNamespace() + state_out = SimpleNamespace(joint_q=object(), joint_qd=object()) + model = object() + control = object() + contacts = object() + articulation_mask = object() + + class _Solver: + def step(self, *args): + calls.append(("step", args)) + + monkeypatch.setattr(NewtonManager, "_solver", _Solver()) + monkeypatch.setattr(NewtonManager, "_model", model) + monkeypatch.setattr( + NewtonVBDManager, + "_ik_articulation_mask", + articulation_mask, + raising=False, + ) + monkeypatch.setattr( + "newton.eval_ik", + lambda *args, **kwargs: calls.append(("eval_ik", args, kwargs)), + ) + + NewtonVBDManager._step_solver( + state_in, + state_out, + control, + contacts, + 0.001, + ) + + assert [call[0] for call in calls] == ["step", "eval_ik"] + assert calls[1][1] == (model, state_out, state_out.joint_q, state_out.joint_qd) + assert calls[1][2] == {"mask": articulation_mask} + + +def test_vbd_manager_skips_joint_reconstruction_without_supported_articulation( + monkeypatch, +): + """Pure deformable or cable models do not run generalized IK.""" + calls = [] + + class _Solver: + def step(self, *args): + calls.append(("step", args)) + + monkeypatch.setattr(NewtonManager, "_solver", _Solver()) + monkeypatch.setattr(NewtonVBDManager, "_ik_articulation_mask", None) + monkeypatch.setattr( + "newton.eval_ik", + lambda *args, **kwargs: calls.append(("eval_ik", args, kwargs)), + ) + + NewtonVBDManager._step_solver( + SimpleNamespace(), + SimpleNamespace(joint_q=object(), joint_qd=object()), + object(), + object(), + 0.001, + ) + + assert [call[0] for call in calls] == ["step"] + + +def test_vbd_joint_state_sync_excludes_native_cable_articulations(): + """Generic IK must not rewrite cable constraint coordinates.""" + + class _Array: + def __init__(self, values): + self._values = np.asarray(values) + + def numpy(self): + return self._values + + model = SimpleNamespace( + joint_type=_Array((JointType.REVOLUTE, JointType.CABLE, JointType.FREE)), + articulation_start=_Array((0, 1, 2, 3)), + articulation_end=_Array((1, 2, 3)), + articulation_count=3, + device="cpu", + ) + + mask = NewtonVBDManager._make_ik_articulation_mask(model) + + assert mask.numpy().tolist() == [True, False, True] + + def test_fabric_particle_sync_skips_missing_fabric_prim(monkeypatch): """Test that missing Fabric prims are skipped before attributes are authored.""" entry = _make_surface_entry() diff --git a/source/isaaclab_newton/changelog.d/ruzini-contact-matching.minor.rst b/source/isaaclab_newton/changelog.d/ruzini-contact-matching.minor.rst new file mode 100644 index 000000000000..fe176990f070 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/ruzini-contact-matching.minor.rst @@ -0,0 +1,5 @@ +Added +^^^^^ + +* Added frame-to-frame contact matching controls to + :class:`~isaaclab_newton.physics.NewtonCollisionPipelineCfg`. diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py index 9e75d3153308..0754b8f583dc 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -115,6 +115,15 @@ class NewtonCollisionPipelineCfg: Defaults to ``True`` (same as Newton's default). """ + contact_matching: Literal["disabled", "latest", "sticky"] = "disabled" + """Frame-to-frame rigid-contact matching mode.""" + + contact_matching_pos_threshold: float = 0.0005 + """Maximum midpoint distance for matching rigid contacts [m].""" + + contact_matching_normal_dot_threshold: float = 0.995 + """Minimum normal dot product for matching rigid contacts.""" + rigid_contact_max: int | None = None """Maximum number of rigid contacts to allocate. 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 551b0ba883ad..d1791319be9e 100644 --- a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py +++ b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py @@ -742,6 +742,21 @@ def test_mjwarp_internal_contacts_with_collision_cfg_raises(): sim.reset() +def test_collision_cfg_forwards_contact_matching_controls(): + """Collision history users can request Newton's native matching buffers.""" + cfg = NewtonCollisionPipelineCfg( + contact_matching="latest", + contact_matching_pos_threshold=0.002, + contact_matching_normal_dot_threshold=0.98, + ) + + pipeline_args = cfg.to_pipeline_args() + + assert pipeline_args["contact_matching"] == "latest" + assert pipeline_args["contact_matching_pos_threshold"] == pytest.approx(0.002) + assert pipeline_args["contact_matching_normal_dot_threshold"] == pytest.approx(0.98) + + @pytest.mark.parametrize( "num_substeps, collision_decimation, expected_mid_loop_collides", [ From 486438ce7936765b42f217622a82727fee9bb44d Mon Sep 17 00:00:00 2001 From: ruziniuuuuu Date: Fri, 7 Aug 2026 16:19:48 +0800 Subject: [PATCH 2/2] Narrow VBD control scope Keep only SolverVBD configuration fields and verify their constructor forwarding and defaults. Move articulation synchronization and collision-pipeline matching to separate follow-up work. --- .../changelog.d/ruzini-vbd-coupling.minor.rst | 6 - .../isaaclab_contrib/coupling/coupler.py | 77 +--------- .../deformable/newton_manager_cfg.py | 14 +- .../deformable/vbd_manager.py | 63 +------- .../test/coupling/test_coupler.py | 82 +--------- .../test_deformable_builder_hooks.py | 145 +++--------------- .../ruzini-contact-matching.minor.rst | 5 - .../physics/newton_collision_cfg.py | 9 -- .../test_newton_manager_abstraction.py | 15 -- 9 files changed, 40 insertions(+), 376 deletions(-) delete mode 100644 source/isaaclab_newton/changelog.d/ruzini-contact-matching.minor.rst diff --git a/source/isaaclab_contrib/changelog.d/ruzini-vbd-coupling.minor.rst b/source/isaaclab_contrib/changelog.d/ruzini-vbd-coupling.minor.rst index 052cb7e3dae2..d9f67f818fb8 100644 --- a/source/isaaclab_contrib/changelog.d/ruzini-vbd-coupling.minor.rst +++ b/source/isaaclab_contrib/changelog.d/ruzini-vbd-coupling.minor.rst @@ -3,9 +3,3 @@ Added * Added rigid-contact stability controls to :class:`~isaaclab_contrib.deformable.VBDSolverCfg`. - -Fixed -^^^^^ - -* Fixed replicated VBD builder coloring and synchronized generalized - coordinates for non-cable articulations owned by VBD coupling entries. diff --git a/source/isaaclab_contrib/isaaclab_contrib/coupling/coupler.py b/source/isaaclab_contrib/isaaclab_contrib/coupling/coupler.py index 4b20c0f91f0e..7d39367e5b31 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/coupling/coupler.py +++ b/source/isaaclab_contrib/isaaclab_contrib/coupling/coupler.py @@ -10,9 +10,6 @@ from dataclasses import dataclass from functools import partial -import newton -import numpy as np -import warp as wp from isaaclab_newton.physics import ( KaminoSolverCfg, MJWarpSolverCfg, @@ -22,21 +19,12 @@ ) from isaaclab_newton.physics.mpm_manager import NewtonMPMManager from isaaclab_newton.physics.newton_manager import NewtonManager -from newton import ( - CollisionPipeline, - Contacts, - Control, - Model, - ModelBuilder, - ShapeFlags, - State, -) +from newton import CollisionPipeline, Model, ModelBuilder, ShapeFlags from newton.solvers.experimental.coupled import SolverCoupled, SolverCoupledADMM, SolverCoupledProxy from isaaclab.physics import PhysicsManager from isaaclab.utils.string import resolve_matching_names -from ..deformable.newton_manager_cfg import VBDSolverCfg from ..deformable.vbd_manager import NewtonVBDManager from .coupler_cfg import ( CouplerAdmmCfg, @@ -91,15 +79,6 @@ def _build_solver(cls, model: Model, solver_cfg: CouplerCfg) -> None: cls._validate_config(solver_cfg) resolved_entries = [cls._resolve_entry(model, entry) for entry in solver_cfg.entries] - vbd_entries = [entry for entry in resolved_entries if isinstance(entry.config.solver_cfg, VBDSolverCfg)] - if vbd_entries: - coupled_ik_mask = cls._make_coupled_ik_articulation_mask( - model, - vbd_entries, - ) - cls._ik_articulation_mask = coupled_ik_mask if bool(np.any(coupled_ik_mask.numpy())) else None - else: - cls._ik_articulation_mask = None proxies: list[CouplerProxyMappingCfg] = [] active_proxy_destinations: set[str] = set() if isinstance(solver_cfg, CouplerProxyCfg): @@ -134,57 +113,6 @@ def _build_solver(cls, model: Model, solver_cfg: CouplerCfg) -> None: NewtonManager._supports_contact_sensors = False NewtonManager._needs_collision_pipeline = needs_collision_pipeline - @classmethod - def _step_solver( - cls, - state_in: State, - state_out: State, - control: Control, - contacts: Contacts | None, - substep_dt: float, - ) -> None: - """Advance all entries and publish VBD-owned generalized coordinates.""" - cls._solver.step(state_in, state_out, control, contacts, substep_dt) - if cls._ik_articulation_mask is not None: - newton.eval_ik( - cls._model, - state_out, - state_out.joint_q, - state_out.joint_qd, - mask=cls._ik_articulation_mask, - ) - - @classmethod - def _make_coupled_ik_articulation_mask( - cls, - model: Model, - entries: list[_ResolvedEntry], - ) -> wp.array: - """Select non-cable articulations owned by nested VBD managers.""" - vbd_owned_joints: set[int] = set() - for entry in entries: - if isinstance(entry.config.solver_cfg, VBDSolverCfg): - vbd_owned_joints.update(entry.joints) - - mask = NewtonVBDManager._make_ik_articulation_mask(model).numpy() - articulation_starts = np.asarray( - model.articulation_start.numpy(), - dtype=np.int32, - ) - articulation_ends = np.asarray( - model.articulation_end.numpy(), - dtype=np.int32, - ) - for articulation_index, (start, end) in enumerate( - zip( - articulation_starts[: model.articulation_count], - articulation_ends, - strict=True, - ) - ): - mask[articulation_index] &= any(joint in vbd_owned_joints for joint in range(int(start), int(end))) - return wp.array(mask, dtype=wp.bool, device=model.device) - @classmethod def _validate_config(cls, solver_cfg: CouplerCfg) -> None: """Validate adapter-specific nested-manager constraints before construction.""" @@ -269,9 +197,8 @@ def _register_builder_attributes(cls, builder: ModelBuilder) -> None: @classmethod def _prepare_builder_for_finalize(cls, builder: ModelBuilder) -> None: """Normalize kinematic colliders when a coupled entry uses implicit MPM.""" + super()._prepare_builder_for_finalize(builder) solver_cfg = getattr(PhysicsManager._cfg, "solver_cfg", None) - if any(isinstance(entry.solver_cfg, VBDSolverCfg) for entry in getattr(solver_cfg, "entries", ())): - NewtonVBDManager._prepare_builder_for_finalize(builder) if any(isinstance(entry.solver_cfg, MPMSolverCfg) for entry in getattr(solver_cfg, "entries", ())): NewtonMPMManager._prepare_builder_for_finalize(builder) diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py index d9ad289aacb4..ca2c4dbe2784 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py @@ -115,7 +115,11 @@ class VBDSolverCfg(NewtonModelSolverCfg): """ rigid_contact_k_start: float = 1.0e2 - """Initial stiffness seed for all rigid body contacts [N/m].""" + """Initial stiffness seed for all rigid body contacts [N/m]. + + This only takes effect when :attr:`rigid_avbd_beta` is greater than zero; + otherwise the penalty stiffness stays fixed at the contact stiffness. + """ rigid_contact_hard: bool = True """Whether rigid contacts use the augmented-Lagrangian hard-contact model. @@ -136,7 +140,13 @@ class VBDSolverCfg(NewtonModelSolverCfg): """ rigid_avbd_beta: float = 0.0 - """Penalty ramp rate per rigid-body AVBD iteration; zero disables ramping.""" + """Penalty ramp rate per rigid-body AVBD iteration; zero disables ramping. + + :attr:`rigid_contact_k_start` only takes effect when this is greater than + zero; at zero the penalty stiffness is fixed at the contact stiffness. + Set to e.g. ``1e5`` to enable ramping. Shared by linear [m] and angular + [rad] constraints, which have different units. + """ rigid_body_contact_buffer_size: int = 64 """Maximum rigid contacts stored per body during one VBD solve.""" diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py index fd2b1764e5c0..02569d4906a2 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py @@ -10,11 +10,9 @@ import logging from typing import TYPE_CHECKING -import newton -import numpy as np import warp as wp from isaaclab_newton.physics.newton_manager import NewtonManager -from newton import Model, ModelBuilder +from newton import Model from newton._src.usd.schemas import SchemaResolverNewton, SchemaResolverPhysx from newton.solvers import SolverVBD @@ -58,8 +56,6 @@ class NewtonVBDManager(NewtonManager): Always uses Newton's :class:`CollisionPipeline` for contact handling. """ - _ik_articulation_mask: wp.array | None = None - @classmethod def initialize(cls, sim_context: SimulationContext) -> None: """Initialize the manager with simulation context. @@ -82,7 +78,6 @@ def initialize(cls, sim_context: SimulationContext) -> None: @classmethod def _solver_specific_clear(cls): """Clear VBD-specific state.""" - cls._ik_articulation_mask = None clear_deformable_builder_hooks() @classmethod @@ -233,13 +228,12 @@ def instantiate_builder_from_stage(cls): } NewtonManager._num_envs = len(env_paths) - cls.set_builder(builder) - - @classmethod - def _prepare_builder_for_finalize(cls, builder: ModelBuilder) -> None: - """Build the vertex and rigid-body coloring required by VBD.""" + # Coloring is required by the VBD solver for particles and VBD-integrated bodies. + # Safe without particles: color() skips particle coloring when particle_count == 0. builder.color() + cls.set_builder(builder) + @classmethod def _create_solver(cls, model: Model, solver_cfg: VBDSolverCfg) -> SolverVBD: """Construct the configured VBD solver.""" @@ -255,53 +249,6 @@ def _build_solver(cls, model: Model, solver_cfg: VBDSolverCfg) -> None: NewtonManager._solver = cls._create_solver(model, solver_cfg) NewtonManager._use_single_state = False NewtonManager._needs_collision_pipeline = True - articulation_mask = cls._make_ik_articulation_mask(model) - cls._ik_articulation_mask = articulation_mask if bool(np.any(articulation_mask.numpy())) else None - - @staticmethod - def _make_ik_articulation_mask(model: Model) -> wp.array: - """Select articulations whose generalized coordinates support eval_ik.""" - joint_types = np.asarray(model.joint_type.numpy(), dtype=np.int32) - articulation_starts = np.asarray(model.articulation_start.numpy(), dtype=np.int32) - articulation_ends = np.asarray(model.articulation_end.numpy(), dtype=np.int32) - mask = np.ones((model.articulation_count,), dtype=bool) - for articulation_index, (start, end) in enumerate( - zip( - articulation_starts[: model.articulation_count], - articulation_ends, - strict=True, - ) - ): - articulation_joint_types = joint_types[int(start) : int(end)] - has_cable = np.any(articulation_joint_types == int(newton.JointType.CABLE)) - has_generalized_joint = np.any( - np.isin( - articulation_joint_types, - ( - int(newton.JointType.REVOLUTE), - int(newton.JointType.PRISMATIC), - int(newton.JointType.BALL), - int(newton.JointType.FREE), - int(newton.JointType.DISTANCE), - int(newton.JointType.D6), - ), - ) - ) - mask[articulation_index] = has_generalized_joint and not has_cable - return wp.array(mask, dtype=wp.bool, device=model.device) - - @classmethod - def _step_solver(cls, state_in, state_out, control, contacts, substep_dt: float) -> None: - """Advance VBD and synchronize generalized articulation coordinates.""" - cls._solver.step(state_in, state_out, control, contacts, substep_dt) - if cls._ik_articulation_mask is not None: - newton.eval_ik( - cls._model, - state_out, - state_out.joint_q, - state_out.joint_qd, - mask=cls._ik_articulation_mask, - ) @classmethod def _simulate_physics_only(cls) -> None: diff --git a/source/isaaclab_contrib/test/coupling/test_coupler.py b/source/isaaclab_contrib/test/coupling/test_coupler.py index 108c33fe4b03..cf4a3a10873f 100644 --- a/source/isaaclab_contrib/test/coupling/test_coupler.py +++ b/source/isaaclab_contrib/test/coupling/test_coupler.py @@ -28,7 +28,7 @@ XPBDSolverCfg, ) from isaaclab_newton.physics.newton_manager import NewtonManager -from newton import JointType, ShapeFlags +from newton import ShapeFlags from newton.solvers.experimental.coupled import SolverCoupledADMM, SolverCoupledProxy from isaaclab_contrib.coupling import ( @@ -69,86 +69,6 @@ def test_public_coupling_exports_are_importable(): assert getattr(coupling, name) is not None -def test_coupler_step_reconstructs_selected_vbd_joint_state(monkeypatch): - """Coupled VBD bodies publish coherent generalized root coordinates.""" - calls = [] - model = object() - articulation_mask = object() - state_out = SimpleNamespace(joint_q=object(), joint_qd=object()) - - class _Solver: - def step(self, *args): - calls.append(("step", args)) - - monkeypatch.setattr(NewtonManager, "_solver", _Solver()) - monkeypatch.setattr(NewtonManager, "_model", model) - monkeypatch.setattr( - NewtonCouplerManager, - "_ik_articulation_mask", - articulation_mask, - raising=False, - ) - monkeypatch.setattr( - "newton.eval_ik", - lambda *args, **kwargs: calls.append(("eval_ik", args, kwargs)), - ) - - state_in = object() - control = object() - contacts = object() - NewtonCouplerManager._step_solver( - state_in, - state_out, - control, - contacts, - 0.001, - ) - - assert [call[0] for call in calls] == ["step", "eval_ik"] - assert calls[1][1] == (model, state_out, state_out.joint_q, state_out.joint_qd) - assert calls[1][2] == {"mask": articulation_mask} - - -def test_coupler_selects_only_vbd_owned_non_cable_articulations(): - """MJWarp state and native cable coordinates stay outside VBD IK sync.""" - - model = SimpleNamespace( - joint_type=_FakeArray( - np.asarray( - (JointType.REVOLUTE, JointType.CABLE, JointType.FREE), - dtype=np.int32, - ) - ), - articulation_start=_FakeArray(np.asarray((0, 1, 2, 3), dtype=np.int32)), - articulation_end=_FakeArray(np.asarray((1, 2, 3), dtype=np.int32)), - articulation_count=3, - device="cpu", - ) - entries = [ - NewtonCouplerManager._ResolvedEntry( - config=CouplerEntryCfg(name="robot", solver_cfg=MJWarpSolverCfg()), - bodies=[], - particles=[], - joints=[0], - shapes=[], - ), - NewtonCouplerManager._ResolvedEntry( - config=CouplerEntryCfg(name="cable", solver_cfg=VBDSolverCfg()), - bodies=[], - particles=[], - joints=[1, 2], - shapes=[], - ), - ] - - mask = NewtonCouplerManager._make_coupled_ik_articulation_mask( - model, - entries, - ) - - assert mask.numpy().tolist() == [False, False, True] - - @dataclass class _FakeModel: """Fields consulted by the coupler's pure configuration helpers.""" diff --git a/source/isaaclab_contrib/test/deformable/test_deformable_builder_hooks.py b/source/isaaclab_contrib/test/deformable/test_deformable_builder_hooks.py index 630af40c2256..c73271e277f0 100644 --- a/source/isaaclab_contrib/test/deformable/test_deformable_builder_hooks.py +++ b/source/isaaclab_contrib/test/deformable/test_deformable_builder_hooks.py @@ -3,17 +3,17 @@ # # SPDX-License-Identifier: BSD-3-Clause +import inspect import math import sys from types import SimpleNamespace -import numpy as np import pytest import warp as wp from isaaclab_newton.cloner.replicate import NewtonReplicateContext from isaaclab_newton.physics import NewtonManager from isaaclab_newton.sim.spawners.materials import NewtonDeformableMaterialCfg -from newton import JointType +from newton.solvers import SolverVBD from isaaclab_contrib.deformable import DeformableObject, VBDSolverCfg from isaaclab_contrib.deformable.deformable_object import ( @@ -34,14 +34,6 @@ def add_cloth_mesh(self, **kwargs) -> None: self.particle_count += len(kwargs["vertices"]) -class _ColoringBuilder: - def __init__(self): - self.color_calls = 0 - - def color(self) -> None: - self.color_calls += 1 - - class _FakePath: def __init__(self, path: str): self.pathString = path @@ -156,121 +148,24 @@ def test_newton_physics_context_is_replicate_context(): assert PHYSICS_CONTEXT is NewtonReplicateContext -def test_vbd_manager_colors_replicated_builder_before_finalize(): - """VBD prepares builders created by the physics replication path.""" - builder = _ColoringBuilder() - - NewtonVBDManager._prepare_builder_for_finalize(builder) - - assert builder.color_calls == 1 - - -def test_vbd_cfg_exposes_rigid_contact_stability_controls(): - """Coupled rigid scenes can configure the public SolverVBD stability knobs.""" - cfg = VBDSolverCfg( - rigid_contact_hard=False, - rigid_contact_history=True, - rigid_avbd_contact_alpha=0.0, - rigid_avbd_beta=100.0, - rigid_body_contact_buffer_size=256, - ) - - assert cfg.rigid_contact_hard is False - assert cfg.rigid_contact_history is True - assert cfg.rigid_avbd_contact_alpha == pytest.approx(0.0) - assert cfg.rigid_avbd_beta == pytest.approx(100.0) - assert cfg.rigid_body_contact_buffer_size == 256 - - -def test_vbd_manager_reconstructs_joint_state_after_maximal_solver_step(monkeypatch): - """VBD keeps generalized articulation state synchronized with body poses.""" - calls = [] - state_in = SimpleNamespace() - state_out = SimpleNamespace(joint_q=object(), joint_qd=object()) - model = object() - control = object() - contacts = object() - articulation_mask = object() - - class _Solver: - def step(self, *args): - calls.append(("step", args)) - - monkeypatch.setattr(NewtonManager, "_solver", _Solver()) - monkeypatch.setattr(NewtonManager, "_model", model) - monkeypatch.setattr( - NewtonVBDManager, - "_ik_articulation_mask", - articulation_mask, - raising=False, - ) - monkeypatch.setattr( - "newton.eval_ik", - lambda *args, **kwargs: calls.append(("eval_ik", args, kwargs)), - ) - - NewtonVBDManager._step_solver( - state_in, - state_out, - control, - contacts, - 0.001, - ) - - assert [call[0] for call in calls] == ["step", "eval_ik"] - assert calls[1][1] == (model, state_out, state_out.joint_q, state_out.joint_qd) - assert calls[1][2] == {"mask": articulation_mask} - - -def test_vbd_manager_skips_joint_reconstruction_without_supported_articulation( - monkeypatch, -): - """Pure deformable or cable models do not run generalized IK.""" - calls = [] - - class _Solver: - def step(self, *args): - calls.append(("step", args)) - - monkeypatch.setattr(NewtonManager, "_solver", _Solver()) - monkeypatch.setattr(NewtonVBDManager, "_ik_articulation_mask", None) - monkeypatch.setattr( - "newton.eval_ik", - lambda *args, **kwargs: calls.append(("eval_ik", args, kwargs)), - ) - - NewtonVBDManager._step_solver( - SimpleNamespace(), - SimpleNamespace(joint_q=object(), joint_qd=object()), - object(), - object(), - 0.001, - ) - - assert [call[0] for call in calls] == ["step"] - - -def test_vbd_joint_state_sync_excludes_native_cable_articulations(): - """Generic IK must not rewrite cable constraint coordinates.""" - - class _Array: - def __init__(self, values): - self._values = np.asarray(values) - - def numpy(self): - return self._values - - model = SimpleNamespace( - joint_type=_Array((JointType.REVOLUTE, JointType.CABLE, JointType.FREE)), - articulation_start=_Array((0, 1, 2, 3)), - articulation_end=_Array((1, 2, 3)), - articulation_count=3, - device="cpu", - ) - - mask = NewtonVBDManager._make_ik_articulation_mask(model) - - assert mask.numpy().tolist() == [True, False, True] +def test_vbd_cfg_forwards_rigid_contact_stability_controls(): + """Forward VBD rigid-contact controls with Newton-compatible defaults.""" + configured_values = { + "rigid_contact_hard": False, + "rigid_contact_history": True, + "rigid_avbd_contact_alpha": 0.0, + "rigid_avbd_beta": 100.0, + "rigid_body_contact_buffer_size": 256, + } + cfg = VBDSolverCfg(**configured_values) + + solver_kwargs = NewtonVBDManager._filter_solver_kwargs(SolverVBD, cfg) + + assert {name: solver_kwargs[name] for name in configured_values} == configured_values + solver_parameters = inspect.signature(SolverVBD.__init__).parameters + default_cfg = VBDSolverCfg() + for name in configured_values: + assert getattr(default_cfg, name) == solver_parameters[name].default def test_fabric_particle_sync_skips_missing_fabric_prim(monkeypatch): diff --git a/source/isaaclab_newton/changelog.d/ruzini-contact-matching.minor.rst b/source/isaaclab_newton/changelog.d/ruzini-contact-matching.minor.rst deleted file mode 100644 index fe176990f070..000000000000 --- a/source/isaaclab_newton/changelog.d/ruzini-contact-matching.minor.rst +++ /dev/null @@ -1,5 +0,0 @@ -Added -^^^^^ - -* Added frame-to-frame contact matching controls to - :class:`~isaaclab_newton.physics.NewtonCollisionPipelineCfg`. 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 0754b8f583dc..9e75d3153308 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -115,15 +115,6 @@ class NewtonCollisionPipelineCfg: Defaults to ``True`` (same as Newton's default). """ - contact_matching: Literal["disabled", "latest", "sticky"] = "disabled" - """Frame-to-frame rigid-contact matching mode.""" - - contact_matching_pos_threshold: float = 0.0005 - """Maximum midpoint distance for matching rigid contacts [m].""" - - contact_matching_normal_dot_threshold: float = 0.995 - """Minimum normal dot product for matching rigid contacts.""" - rigid_contact_max: int | None = None """Maximum number of rigid contacts to allocate. 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 d1791319be9e..551b0ba883ad 100644 --- a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py +++ b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py @@ -742,21 +742,6 @@ def test_mjwarp_internal_contacts_with_collision_cfg_raises(): sim.reset() -def test_collision_cfg_forwards_contact_matching_controls(): - """Collision history users can request Newton's native matching buffers.""" - cfg = NewtonCollisionPipelineCfg( - contact_matching="latest", - contact_matching_pos_threshold=0.002, - contact_matching_normal_dot_threshold=0.98, - ) - - pipeline_args = cfg.to_pipeline_args() - - assert pipeline_args["contact_matching"] == "latest" - assert pipeline_args["contact_matching_pos_threshold"] == pytest.approx(0.002) - assert pipeline_args["contact_matching_normal_dot_threshold"] == pytest.approx(0.98) - - @pytest.mark.parametrize( "num_substeps, collision_decimation, expected_mid_loop_collides", [