From 9f39d06f4db1e5ad891478471e40bdd76608feee Mon Sep 17 00:00:00 2001 From: jichuanh Date: Wed, 8 Jul 2026 08:10:23 +0000 Subject: [PATCH 01/14] Enable Newton gravity compensation via inverse dynamics Newton's inverse-dynamics feature (newton-physics/newton#2753, closing the request in newton-physics/newton#2497) provides the primitive that gravity_compensation_forces was waiting for on the Newton backend. - Implement the accessor with eval_inverse_dynamics(GRAVITY_FORCE) and a flat-buffer gather into the per-view contract layout of (N, num_joints + num_base_dofs); buffers are pre-allocated so reads stay CUDA-graph-capture safe. - Bump the pinned Newton commit to the feature's merge commit; the transitive MuJoCo stack moves from the 3.8 to the 3.10 series. - Replace the strict-xfail gap test with positive coverage: shape contracts (fixed/floating base), a Jacobian-transpose gravity identity (incl. non-identity root pose), FK-staleness refresh, and the static-equilibrium accuracy test mirrored from the PhysX suite. - Drop stale unsupported-on-Newton guidance from task-space actions. --- pyproject.toml | 4 +- .../jichuanh-newton-gravity-compensation.rst | 7 + .../mdp/actions/pink_task_space_actions.py | 10 +- .../envs/mdp/actions/task_space_actions.py | 5 +- ...uanh-newton-gravity-compensation.minor.rst | 15 + .../assets/articulation/articulation_data.py | 80 ++++-- .../assets/articulation/kernels.py | 30 ++ .../test/assets/test_articulation.py | 269 +++++++++++++++--- .../jichuanh-newton-gravity-compensation.skip | 0 .../test/assets/test_articulation.py | 8 +- 10 files changed, 348 insertions(+), 80 deletions(-) create mode 100644 source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst create mode 100644 source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst create mode 100644 source/isaaclab_physx/changelog.d/jichuanh-newton-gravity-compensation.skip diff --git a/pyproject.toml b/pyproject.toml index 92cca8dfdf02..f2f53fed238d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -159,7 +159,7 @@ torchaudio = "2.11.0" ovphysx = "0.5.2+head.f62c22207c" ovrtx = ">=0.3.0,<0.4.0" # Newton git commit + matching warp-lang prerelease (upgraded together). -newton = "c7ae7c7648cd0717df39e5c94b95d5a02c997320" +newton = "b24dc255392126fe42a609ea6c4e4c8dca8009cc" warp = "1.15.0.dev20260626" [tool.ruff] @@ -326,7 +326,7 @@ prerelease = "allow" # the CUDA indexes via [tool.uv.sources]. Values mirror [tool.isaaclab.versions] above. override-dependencies = [ "numpy>=2", - "newton[sim] @ git+https://github.com/newton-physics/newton.git@c7ae7c7648cd0717df39e5c94b95d5a02c997320", + "newton[sim] @ git+https://github.com/newton-physics/newton.git@b24dc255392126fe42a609ea6c4e4c8dca8009cc", # Force the Newton-matched schemas over isaacsim's ==0.2.0 pin. "newton-usd-schemas>=0.3.1", "torch==2.11.0", diff --git a/source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst b/source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst new file mode 100644 index 000000000000..77d185bb134d --- /dev/null +++ b/source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst @@ -0,0 +1,7 @@ +Changed +^^^^^^^ + +* Changed task-space action docstrings that advised keeping gravity compensation + disabled on the Newton backend — :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` + is now implemented there, so ``gravity_compensation`` / + ``enable_gravity_compensation`` may be enabled on both backends. diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py index 1612c6621c71..3a5e67ffc426 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py @@ -322,12 +322,10 @@ def apply_actions(self) -> None: def _apply_gravity_compensation(self) -> None: """Apply gravity compensation to arm joints if not disabled in props. - Reads :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces`, - which raises :class:`NotImplementedError` on the Newton backend (no upstream - primitive). That is intentional — if a user opts into gravity compensation on - Newton via ``enable_gravity_compensation=True``, they should see a loud failure - rather than silently receive zeros. To use Pink IK on Newton, keep - ``enable_gravity_compensation=False``. + Reads :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` + and applies it as a joint-effort target on the controlled arm joints, on top + of the joint-position targets from IK. Supported on both the PhysX and the + Newton backend. """ if not self._asset.cfg.spawn.rigid_props.disable_gravity: # ``gravity_compensation_forces`` shape is ``(N, num_joints + num_base_dofs)``. diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py index ac1a713d234d..51437849afc9 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -650,9 +650,8 @@ def _compute_dynamic_quantities(self): / :attr:`~isaaclab.controllers.OperationalSpaceControllerCfg.nullspace_control` and :attr:`~isaaclab.controllers.OperationalSpaceControllerCfg.gravity_compensation` - respectively. This avoids an unconditional engine call on backends - that don't expose the corresponding primitive (Newton has no - gravity-compensation API). + respectively. This avoids unconditional engine calls when the controller + does not consume the corresponding quantity. Note: For floating-base robots the Jacobian / mass-matrix / gravity-compensation DoF axis prepends 6 floating-base columns. We use ``self._jacobi_joint_idx`` diff --git a/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst b/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst new file mode 100644 index 000000000000..3e73b5455507 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst @@ -0,0 +1,15 @@ +Added +^^^^^ + +* Added the Newton implementation of + :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces`, backed by + Newton's inverse-dynamics API (``eval_inverse_dynamics``). The accessor previously + raised ``NotImplementedError`` on the Newton backend; gravity compensation in + task-space controllers (operational-space control, Pink IK) now works on Newton. + +Changed +^^^^^^^ + +* Changed the pinned Newton commit to pick up the upstream inverse-dynamics feature + (joint-space mass matrix, gravity and Coriolis compensation forces). The transitive + MuJoCo dependencies move from the 3.8 to the 3.10 series accordingly. diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py index be4c5c92e181..15bafd13d8cb 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -1002,22 +1002,41 @@ def mass_matrix(self) -> ProxyArray: def gravity_compensation_forces(self) -> ProxyArray: """See :attr:`isaaclab.assets.BaseArticulationData.gravity_compensation_forces`. - Newton implementation: raises :class:`NotImplementedError` — Newton's - ``ArticulationView`` exposes only ``eval_fk`` / ``eval_jacobian`` / - ``eval_mass_matrix``. Use PhysX, or set the controller's - ``gravity_compensation=False`` until upstream Newton adds the primitive. - Tracking upstream: `newton#2497 `_, - `newton#2529 `_, - `newton#2625 `_. - """ - raise NotImplementedError( - "Newton has no gravity-compensation primitive. Use PhysX, or set the controller's" - " ``gravity_compensation=False`` until upstream Newton adds an" - " ``eval_gravity_compensation`` API. Tracking upstream:" - " https://github.com/newton-physics/newton/issues/2497," - " https://github.com/newton-physics/newton/issues/2529," - " https://github.com/newton-physics/newton/issues/2625." + Newton implementation: ``eval_inverse_dynamics`` with ``EvalType.GRAVITY_FORCE`` + (writes the model-wide flat DoF buffer) then a gather kernel extracts this + view's DoF segment. + """ + if self._inverse_dynamics_buf is None: + raise NotImplementedError( + "gravity_compensation_forces requires Newton's inverse-dynamics API:" + f" {self._inverse_dynamics_unavailable_reason}" + ) + # Local import: keeps isaaclab_newton importable against Newton versions that + # predate the inverse-dynamics API (the guard above raises before reaching it). + from newton import InverseDynamics # noqa: PLC0415 + + # eval_inverse_dynamics reads ``state.body_q``; refresh FK if stale. + # Matches the convention in ``body_link_pose_w`` — Python-guarded lazy refresh. + self._ensure_fk_fresh() + # eval_inverse_dynamics writes every articulation in the view's model-wide flat + # buffer (zeros outside the view); the gather kernel extracts this view's DoF + # segments. All buffers are pre-allocated for CUDA-graph capture safety. + self._root_view.eval_inverse_dynamics( + SimulationManager.get_state_0(), + InverseDynamics.EvalType.GRAVITY_FORCE, + self._inverse_dynamics_buf, + ) + wp.launch( + articulation_kernels.gather_dof_force_rows, + dim=self._gravity_compensation_forces_buf.shape, + inputs=[ + self._inverse_dynamics_buf.gravity_force, + self._jacobian_view_dof_starts, + ], + outputs=[self._gravity_compensation_forces_buf], + device=self.device, ) + return self._gravity_compensation_forces_ta """ Joint state properties. @@ -1794,9 +1813,11 @@ def _create_buffers(self) -> None: def _create_jacobian_buffers(self, model) -> None: """Allocate the scratch + view-sized buffers used by task-space accessors. - Newton's :meth:`eval_jacobian` / :meth:`eval_mass_matrix` write into model-sized - scratch buffers spanning every articulation in the model; the gather kernels in - :attr:`body_com_jacobian_w` / :attr:`mass_matrix` extract this view's rows. The + Newton's :meth:`eval_jacobian` / :meth:`eval_mass_matrix` / + :meth:`eval_inverse_dynamics` write into model-sized scratch buffers spanning + every articulation in the model; the gather kernels in + :attr:`body_com_jacobian_w` / :attr:`mass_matrix` / + :attr:`gravity_compensation_forces` extract this view's rows. The output buffers are sized using THIS articulation's body / DoF counts (not the model-wide ``max_*``) so heterogeneous scenes do not leak zero-padded rows / cols into the returned tensor. The DoF axis includes ``num_base_dofs`` floating-base @@ -1859,6 +1880,28 @@ def _create_jacobian_buffers(self, model) -> None: device=self.device, ) + # -- ``gravity_compensation_forces``: model-wide inverse-dynamics container + # (eval_inverse_dynamics output buffers + private RNEA scratch) and per-view + # output (gather output). ``None`` when the installed Newton predates the + # inverse-dynamics API (AttributeError) or the model contains joints it does + # not support, e.g. CABLE (ValueError); the property raises with the reason. + try: + self._inverse_dynamics_buf = model.inverse_dynamics() + self._inverse_dynamics_unavailable_reason = "" + except (AttributeError, ValueError) as err: + self._inverse_dynamics_buf = None + self._inverse_dynamics_unavailable_reason = str(err) + # Flat joint-space (``Model.joint_qd`` layout) DoF start of each view + # articulation, for gathering flat per-DoF outputs. Host-computed once: + # dof_start = joint_qd_start[first joint of the articulation]. + art_first_joint = model.articulation_start.numpy()[self._jacobian_view_art_ids.numpy()] + self._jacobian_view_dof_starts = wp.array( + model.joint_qd_start.numpy()[art_first_joint], dtype=wp.int32, device=self.device + ) + self._gravity_compensation_forces_buf = wp.zeros( + (self._num_instances, self._num_joints + num_base_dofs), dtype=wp.float32, device=self.device + ) + def _pin_proxy_arrays(self) -> None: """Create or rebind all pinned ProxyArray wrappers. @@ -1937,6 +1980,7 @@ def _pin_proxy_arrays(self) -> None: self._body_com_jacobian_w_ta = ProxyArray(self._body_com_jacobian_w_buf) self._body_link_jacobian_w_ta = ProxyArray(self._body_link_jacobian_w_buf) self._mass_matrix_ta = ProxyArray(self._mass_matrix_buf) + self._gravity_compensation_forces_ta = ProxyArray(self._gravity_compensation_forces_buf) # -- deprecated state properties (lazy); type annotations declared once here self._root_state_w_ta: ProxyArray | None = None diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py index a928bd524761..eb07b42e69d3 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py @@ -692,6 +692,36 @@ def gather_mass_matrix_rows( dst[i, r, c] = src[art_ids[i], r, c] +@wp.kernel +def gather_dof_force_rows( + src: wp.array(dtype=wp.float32), + view_dof_starts: wp.array(dtype=wp.int32), + dst: wp.array2d(dtype=wp.float32), +): + """Copy per-view articulation DoF forces from a flat model-sized buffer into a view-sized buffer. + + Flat-layout analogue of :func:`gather_mass_matrix_rows` for per-DoF outputs + written in :attr:`newton.Model.joint_qd` layout (e.g. the gravity + compensation force from ``newton.eval_inverse_dynamics``). The DoF axis is + preserved in full (including the leading 6 free-root entries for + floating-base articulations), matching the cross-library industry + convention used by PhysX, Pinocchio, Drake, MuJoCo, RBDL, OCS2, and + iDynTree. + + The gather is in-place on a pre-allocated ``dst`` buffer, so the kernel + launch is safe under CUDA graph capture. + + Args: + src: Input flat DoF buffer. Shape is (model.joint_dof_count,). + view_dof_starts: Flat DoF start index of each view articulation. Shape + is (num_instances,). + dst: Output DoF-force buffer for this view. Shape is + (num_instances, num_joints + num_base_dofs). + """ + i, d = wp.tid() + dst[i, d] = src[view_dof_starts[i] + d] + + @wp.kernel def shift_jacobian_com_to_origin( body_link_pose: wp.array2d(dtype=wp.transformf), diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 287c88762c59..76116d3109ec 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -2684,51 +2684,6 @@ def test_randomize_rigid_body_collider_offsets(sim, num_articulations, device, a torch.testing.assert_close(updated_gap, new_gap) -@pytest.mark.parametrize("num_articulations", [1]) -@pytest.mark.parametrize("device", ["cuda:0"]) -@pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.isaacsim_ci -@pytest.mark.xfail( - strict=True, - raises=NotImplementedError, - reason=( - "Newton's ArticulationView exposes eval_fk / eval_jacobian /" - " eval_mass_matrix only — no inverse-dynamics primitive yet." - " Upstream Newton is actively working on this through the inverse-" - " dynamics feature request (https://github.com/newton-physics/newton/issues/2497)" - " and its sub-task for Coriolis + gravity compensation" - " (https://github.com/newton-physics/newton/issues/2529). A known" - " correctness bug for floating-base + non-identity root pose is" - " tracked separately at" - " https://github.com/newton-physics/newton/issues/2625, and" - " informs why we deliberately do NOT roll our own J^T·m·g shim in" - " this PR — Newton's eventual primitive is going through RNEA via" - " MuJoCo Warp and may differ at corner cases we wouldn't catch." - " Once the wrapper at" - " isaaclab_newton.assets.ArticulationData.gravity_compensation_forces" - " switches from a NotImplementedError stub to a real implementation" - " (likely calling the new Newton primitive), this XFAIL will turn" - " into XPASS and fail under strict=True. The maintainer should" - " then: (1) drop this xfail or invert it into a positive value" - " assertion against PhysX (the cross-backend accuracy diff), and" - " (2) remove the OSC config-time guidance about setting" - " gravity_compensation=False on Newton." - ), -) -def test_get_gravity_compensation_forces_not_implemented_on_newton(sim, num_articulations, device, articulation_type): - """Pin the known Newton gravity-compensation gap. - - See the ``xfail`` marker for full rationale. The body simply invokes the - wrapper and lets the strict-xfail marker handle the expected failure. - """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) - articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) - sim.reset() - assert articulation.is_initialized - - _ = articulation.data.gravity_compensation_forces - - ## # Shape-contract regression tests for the new BaseArticulation accessors. # These pin the public shape contract so future regressions (e.g., reverting @@ -2798,6 +2753,30 @@ def test_get_mass_matrix_shape_and_nonsingular_fixed_base(sim, num_articulations assert (diag > 1e-6).all(), f"mass matrix has non-positive diagonal entries: min={diag.min()}" +@pytest.mark.parametrize("num_articulations", [1, 4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.isaacsim_ci +def test_get_gravity_compensation_forces_shape_fixed_base(sim, num_articulations, device, articulation_type): + """Fixed-base ``gravity_compensation_forces`` shape ``(N, num_joints)``. + + No floating-base entries on the DoF axis, and per-articulation output + sizing — a wrong view gather (model-wide ``max_dofs`` or a bad flat DoF + offset) would surface here in heterogeneous scenes. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + assert articulation.is_fixed_base, "panda fixture must be fixed-base for this test" + + g = articulation.data.gravity_compensation_forces.torch + + expected_shape = (num_articulations, articulation.num_joints) + assert g.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(g.shape)}" + assert g.dtype == torch.float32 + + @pytest.mark.parametrize("num_articulations", [1, 4]) @pytest.mark.parametrize("device", ["cuda:0"]) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -2858,6 +2837,32 @@ def test_get_mass_matrix_shape_floating_base(sim, num_articulations, device, add assert M.dtype == torch.float32 +@pytest.mark.parametrize("num_articulations", [1, 4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.parametrize("articulation_type", ["anymal"]) +@pytest.mark.isaacsim_ci +def test_get_gravity_compensation_forces_shape_floating_base( + sim, num_articulations, device, add_ground_plane, articulation_type +): + """Floating-base ``gravity_compensation_forces`` shape ``(N, num_joints + 6)``. + + Includes the 6 floating-base entries on the DoF axis, matching the + cross-library industry convention. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + assert not articulation.is_fixed_base, "anymal fixture must be floating-base for this test" + + g = articulation.data.gravity_compensation_forces.torch + + expected_shape = (num_articulations, articulation.num_joints + articulation.num_base_dofs) + assert g.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(g.shape)}" + assert g.dtype == torch.float32 + + @pytest.mark.parametrize("device", ["cuda:0"]) @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("articulation_type", ["anymal"]) @@ -3075,6 +3080,64 @@ def test_get_mass_matrix_symmetry_pd(sim, num_articulations, device, articulatio torch.linalg.cholesky(M + 1e-6 * eye) +@pytest.mark.parametrize("num_articulations", [4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) +@pytest.mark.isaacsim_ci +def test_get_gravity_compensation_forces_matches_jacobian_gravity( + sim, num_articulations, device, add_ground_plane, articulation_type +): + """``g(q)`` must equal ``-sum_b J_com_b^T (m_b * g_w)`` at the current configuration. + + Newton computes the gravity compensation force through an RNEA pass + (``eval_inverse_dynamics``); the static identity above derives the same + quantity independently from the (already contract-validated) COM-referenced + Jacobian and the per-body masses, pinning the sign convention, the DoF + ordering (including the 6 floating-base entries), and the flat-buffer view + gather in one assertion. Non-default joint positions and — for + floating-base — a rotated, lifted root pose guard the corner fixed + upstream in newton#2625 (wrong gravity compensation under non-identity + root pose). + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + + # Non-trivial configuration via manual writes (no sim step, so the assert + # compares both quantities at exactly this state): random joint offsets, + # and for floating-base a non-identity root pose. + torch.manual_seed(0) + q = articulation.data.default_joint_pos.torch + 0.3 * torch.randn( + num_articulations, articulation.num_joints, device=device + ) + articulation.write_joint_position_to_sim_index(position=q) + if not articulation.is_fixed_base: + root_pose = articulation.data.default_root_pose.torch.clone() + root_pose[:, 2] += 1.0 + # (x, y, z, w) quaternion — 30 deg roll about x. + root_pose[:, 3:] = torch.tensor([0.2588, 0.0, 0.0, 0.9659], device=device) + articulation.write_root_pose_to_sim_index(root_pose=root_pose) + + g_meas = articulation.data.gravity_compensation_forces.torch + + # Independent derivation: generalized gravity load tau_g = sum_b J_lin_b^T (m_b g_w); + # the compensation force is its negation. The COM-referenced Jacobian is exactly the + # right lever arm for a point gravity force acting at each body's COM. + J_com = articulation.data.body_com_jacobian_w.torch # (N, B_jac, 6, D) + masses = articulation.data.body_mass.torch # (N, num_bodies) + if articulation.is_fixed_base: + # jacobi_body_idx == body_idx - 1 for fixed-base (fixed-root row excluded). + masses = masses[:, 1:] + gravity_w = wp.to_torch(SimulationManager.get_model().gravity) # (num_worlds, 3) + assert gravity_w.shape[0] == num_articulations, "fixture must place one articulation per world" + f_gravity = masses.unsqueeze(-1) * gravity_w.unsqueeze(1) # (N, B_jac, 3) + g_expected = -torch.einsum("nbij,nbi->nj", J_com[:, :, 0:3, :], f_gravity) + + torch.testing.assert_close(g_meas, g_expected, atol=1e-2, rtol=1e-3) + + @pytest.mark.parametrize("num_articulations", [1]) @pytest.mark.parametrize("device", ["cuda:0"]) @pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) @@ -3160,6 +3223,122 @@ def test_mass_matrix_refreshes_after_manual_joint_write( ) +@pytest.mark.parametrize("num_articulations", [1]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.isaacsim_ci +def test_gravity_compensation_refreshes_after_manual_joint_write(sim, num_articulations, device, articulation_type): + """After ``write_joint_position_to_sim_index`` (no sim step), the gravity + compensation read must reflect the new joint state. + + ``g(q)`` depends on ``q`` through the RNEA pass in ``eval_inverse_dynamics``, + which reads ``state.body_q``. Same FK-staleness pattern as the Jacobian and + the mass matrix. Gravity stays enabled (the default) — with gravity off, + ``g(q)`` is identically zero and the assert would be vacuous. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + sim.step() + articulation.update(sim.cfg.dt) + + g_0 = articulation.data.gravity_compensation_forces.torch.clone() + q_target = articulation.data.joint_pos.torch.clone() + 0.5 + env_ids = wp.array([0], dtype=wp.int32, device=device) + articulation.write_joint_position_to_sim_index(position=q_target, env_ids=env_ids) + g_1 = articulation.data.gravity_compensation_forces.torch.clone() + + assert not torch.allclose(g_0, g_1, atol=1e-3), ( + "gravity_compensation_forces did not change after manual joint write — " + "FK trigger likely missing before eval_inverse_dynamics." + ) + + +@pytest.mark.parametrize("num_articulations", [1]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.isaacsim_ci +def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulations, device, articulation_type): + """Newton accuracy: ``τ_gc`` must hold the manipulator in static equilibrium. + + Newton-side variant of the PhysX test of the same name (backend parity). + The contract is the EOM identity ``M(q) q̈ + C(q,q̇) q̇ + g(q) = τ_input``. + Setting ``τ_input = g(q)`` at ``q̇ = 0`` gives ``q̈ = 0`` — the arm should + not move. This pins + :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` + in isolation: sign errors, frame errors, and DoF-ordering errors all + surface as joint drift, while a controller-level test would have those + bugs averaged out by PD damping. + """ + base_cfg = generate_articulation_cfg(articulation_type=articulation_type) + # Replace default Franka actuators with a passthrough implicit actuator + # (stiffness = 0, damping = 0). With both gains zero the effort target + # we set IS the joint torque applied — no PD spring-damper masks the + # gravity-comp signal. Default Franka cfg has stiffness=80 / damping=4 + # which would absorb gravity through PD bias and hide accessor bugs. + cfg = base_cfg.replace( + actuators={ + "all": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness=0.0, + damping=0.0, + ), + }, + ) + # FRANKA_PANDA_CFG has rigid_props.disable_gravity=False already, but be + # defensive — gravity must be ON for τ_gc to have anything to cancel. + cfg = cfg.replace( + spawn=cfg.spawn.replace( + rigid_props=cfg.spawn.rigid_props.replace(disable_gravity=False), + ), + ) + + articulation, _ = generate_articulation(cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + + # Force a clean static state: default joint positions, zero velocities. + # ``sim.reset`` may leave residual ``q_dot`` from solver settling under + # gravity, so we pin it explicitly here. + default_q = articulation.data.default_joint_pos.torch.clone() + default_qd = torch.zeros_like(default_q) + articulation.write_joint_position_to_sim_index(position=default_q) + articulation.write_joint_velocity_to_sim_index(velocity=default_qd) + articulation.update(sim.cfg.dt) + + # Default joint pose from FRANKA_PANDA_CFG bends the elbow + # (joint2=-0.569, joint4=-2.81, joint6=3.04) so several links carry a + # gravity load — τ_gc is non-trivial in this configuration. A natural- + # hang pose (all zeros) would produce near-zero τ_gc and make this + # test uninformative. + init_q = articulation.data.joint_pos.torch.clone() + + # Step 100 times applying only τ_gc as joint efforts. + for _ in range(100): + # ``gravity_compensation_forces`` shape is ``(N, num_joints + num_base_dofs)`` + # — leading ``num_base_dofs`` floating-base entries (0 on fixed-base) followed + # by the actuated-joint entries. Slice past the floating-base entries so the + # remaining tensor aligns with ``set_joint_effort_target_index`` (actuated only). + tau_gc = articulation.data.gravity_compensation_forces.torch[:, articulation.num_base_dofs :] + articulation.set_joint_effort_target_index(target=tau_gc) + articulation.write_data_to_sim() + sim.step() + articulation.update(sim.cfg.dt) + + final_q = articulation.data.joint_pos.torch + drift = (final_q - init_q).abs().max() + # Tight bound: 5e-3 rad ≈ 0.3°. Numerical integration over 100 steps will + # accumulate some floor (sub-millirad on Franka), but a sign or frame bug + # in τ_gc produces drift of at least a degree per step on bent-elbow + # poses. This bound separates "correct" from "broken" cleanly. + assert drift < 5e-3, ( + f"max joint drift {drift:.5f} rad after 100 gravity-comp-only steps —" + " τ_gc did not hold static equilibrium. Check sign, DoF ordering, and" + " whether gravity_compensation_forces returns g(q) (positive) or" + " its negation." + ) + + @pytest.mark.parametrize("device", ["cuda:0"]) @pytest.mark.parametrize("articulation_type", ["panda"]) @pytest.mark.parametrize("gravity_enabled", [False]) diff --git a/source/isaaclab_physx/changelog.d/jichuanh-newton-gravity-compensation.skip b/source/isaaclab_physx/changelog.d/jichuanh-newton-gravity-compensation.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index ac4755962a95..34544d630746 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -2503,12 +2503,8 @@ def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulatio surface as joint drift, while a controller-level test would have those bugs averaged out by PD damping. - Newton-side equivalent is deliberately omitted in this PR (see the - ``xfail`` test pinning the upstream gap). Newton's inverse-dynamics - primitive is in flight at upstream issues #2497 / #2529 and has a known - floating-base bug (#2625) that we'd have to test around. Ship a Newton - accuracy variant of this test alongside the Newton implementation when - upstream lands. + Newton-side variant of the same name lives in + ``isaaclab_newton/test/assets/test_articulation.py`` (backend parity). """ base_cfg = generate_articulation_cfg(articulation_type=articulation_type) # Replace default Franka actuators with a passthrough implicit actuator From ee097bda05496f327df4d4a759bc6cbe62f2f432 Mon Sep 17 00:00:00 2001 From: jichuanh Date: Wed, 8 Jul 2026 08:57:04 +0000 Subject: [PATCH 02/14] Defer Newton pin bump, gate new tests on API availability The bumped Newton commit requires mujoco-warp 3.10 (bumped upstream in newton-physics/newton#2980, which the inverse-dynamics merge sits on), while isaacsim-core 6.0.0.1 exact-pins mujoco-warp==3.8.0.3. The isaaclab[isaacsim] wheel-install path therefore cannot co-resolve any Newton commit that provides the inverse-dynamics API, as caught by the install CI on this PR. Keep the develop Newton pin and land the accessor forward-compatible instead: articulation init already degrades gracefully on older Newton builds, and the new gravity-compensation tests now skip when the installed Newton lacks the API. They activate automatically with the next Newton pin bump once Isaac Sim moves to the MuJoCo 3.10 series. --- pyproject.toml | 4 ++-- .../jichuanh-newton-gravity-compensation.rst | 4 ++-- .../mdp/actions/pink_task_space_actions.py | 5 +++-- ...uanh-newton-gravity-compensation.minor.rst | 13 ++++--------- .../test/assets/test_articulation.py | 19 +++++++++++++++++++ 5 files changed, 30 insertions(+), 15 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index f2f53fed238d..92cca8dfdf02 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -159,7 +159,7 @@ torchaudio = "2.11.0" ovphysx = "0.5.2+head.f62c22207c" ovrtx = ">=0.3.0,<0.4.0" # Newton git commit + matching warp-lang prerelease (upgraded together). -newton = "b24dc255392126fe42a609ea6c4e4c8dca8009cc" +newton = "c7ae7c7648cd0717df39e5c94b95d5a02c997320" warp = "1.15.0.dev20260626" [tool.ruff] @@ -326,7 +326,7 @@ prerelease = "allow" # the CUDA indexes via [tool.uv.sources]. Values mirror [tool.isaaclab.versions] above. override-dependencies = [ "numpy>=2", - "newton[sim] @ git+https://github.com/newton-physics/newton.git@b24dc255392126fe42a609ea6c4e4c8dca8009cc", + "newton[sim] @ git+https://github.com/newton-physics/newton.git@c7ae7c7648cd0717df39e5c94b95d5a02c997320", # Force the Newton-matched schemas over isaacsim's ==0.2.0 pin. "newton-usd-schemas>=0.3.1", "torch==2.11.0", diff --git a/source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst b/source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst index 77d185bb134d..71f036b56cd5 100644 --- a/source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst +++ b/source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst @@ -3,5 +3,5 @@ Changed * Changed task-space action docstrings that advised keeping gravity compensation disabled on the Newton backend — :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` - is now implemented there, so ``gravity_compensation`` / - ``enable_gravity_compensation`` may be enabled on both backends. + is implemented on Newton builds that provide the inverse-dynamics API (newton > 1.3), + so ``gravity_compensation`` / ``enable_gravity_compensation`` may be enabled there. diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py index 3a5e67ffc426..dee209a24fed 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py @@ -324,8 +324,9 @@ def _apply_gravity_compensation(self) -> None: Reads :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` and applies it as a joint-effort target on the controlled arm joints, on top - of the joint-position targets from IK. Supported on both the PhysX and the - Newton backend. + of the joint-position targets from IK. Available on PhysX and on Newton builds + that provide the inverse-dynamics API (newton > 1.3); older Newton builds raise + :class:`NotImplementedError` on the read. """ if not self._asset.cfg.spawn.rigid_props.disable_gravity: # ``gravity_compensation_forces`` shape is ``(N, num_joints + num_base_dofs)``. diff --git a/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst b/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst index 3e73b5455507..4b359eb9915b 100644 --- a/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst +++ b/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst @@ -4,12 +4,7 @@ Added * Added the Newton implementation of :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces`, backed by Newton's inverse-dynamics API (``eval_inverse_dynamics``). The accessor previously - raised ``NotImplementedError`` on the Newton backend; gravity compensation in - task-space controllers (operational-space control, Pink IK) now works on Newton. - -Changed -^^^^^^^ - -* Changed the pinned Newton commit to pick up the upstream inverse-dynamics feature - (joint-space mass matrix, gravity and Coriolis compensation forces). The transitive - MuJoCo dependencies move from the 3.8 to the 3.10 series accordingly. + raised ``NotImplementedError`` unconditionally; it now computes the compensation + forces on Newton builds that provide the API (newton > 1.3) and keeps raising a + descriptive ``NotImplementedError`` on older builds, including the currently pinned + Newton commit. diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 76116d3109ec..a0f92a97a06c 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -2684,6 +2684,20 @@ def test_randomize_rigid_body_collider_offsets(sim, num_articulations, device, a torch.testing.assert_close(updated_gap, new_gap) +def _skip_if_newton_lacks_inverse_dynamics() -> None: + """Skip the current test when the installed Newton predates the inverse-dynamics API. + + ``gravity_compensation_forces`` needs ``newton.eval_inverse_dynamics`` (merged + upstream after the 1.3 release). The pinned Newton commit cannot move past it until + Isaac Sim's exact ``mujoco-warp==3.8.0.3`` pin co-resolves with Newton's MuJoCo 3.10 + requirement, so these tests activate automatically with the next Newton pin bump. + """ + import newton + + if not hasattr(newton, "InverseDynamics"): + pytest.skip("installed Newton has no inverse-dynamics API (needs newton > 1.3)") + + ## # Shape-contract regression tests for the new BaseArticulation accessors. # These pin the public shape contract so future regressions (e.g., reverting @@ -2764,6 +2778,7 @@ def test_get_gravity_compensation_forces_shape_fixed_base(sim, num_articulations sizing — a wrong view gather (model-wide ``max_dofs`` or a bad flat DoF offset) would surface here in heterogeneous scenes. """ + _skip_if_newton_lacks_inverse_dynamics() articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) sim.reset() @@ -2850,6 +2865,7 @@ def test_get_gravity_compensation_forces_shape_floating_base( Includes the 6 floating-base entries on the DoF axis, matching the cross-library industry convention. """ + _skip_if_newton_lacks_inverse_dynamics() articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) sim.reset() @@ -3100,6 +3116,7 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( upstream in newton#2625 (wrong gravity compensation under non-identity root pose). """ + _skip_if_newton_lacks_inverse_dynamics() articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) sim.reset() @@ -3236,6 +3253,7 @@ def test_gravity_compensation_refreshes_after_manual_joint_write(sim, num_articu the mass matrix. Gravity stays enabled (the default) — with gravity off, ``g(q)`` is identically zero and the assert would be vacuous. """ + _skip_if_newton_lacks_inverse_dynamics() articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) sim.reset() @@ -3270,6 +3288,7 @@ def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulatio surface as joint drift, while a controller-level test would have those bugs averaged out by PD damping. """ + _skip_if_newton_lacks_inverse_dynamics() base_cfg = generate_articulation_cfg(articulation_type=articulation_type) # Replace default Franka actuators with a passthrough implicit actuator # (stiffness = 0, damping = 0). With both gains zero the effort target From bdf18319e6ac543e46eca5ab204dc67649a66cb5 Mon Sep 17 00:00:00 2001 From: jichuanh Date: Wed, 8 Jul 2026 23:07:05 +0000 Subject: [PATCH 03/14] Bump Newton pin to include inverse dynamics, drop API guards MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Follow the centralized dependency scheme (#6009): the pin moves in [tool.isaaclab.versions] with the mirrored [tool.uv] override. The pinned commit b24dc255 is the merge commit of the upstream inverse-dynamics feature (newton-physics/newton#2753). Newton's MuJoCo requirement moved to the 3.10 series upstream (newton-physics/newton#2980) while isaacsim-core 6.0.0.1 exact-pins mujoco-warp==3.8.0.3, so mujoco/mujoco-warp join override-dependencies — Isaac Lab owns the Newton stack, the same rationale as the existing newton and torch overrides. The previously failing isaaclab[all,isaacsim] wheel resolution now succeeds from the repo root with the overrides applied. With the pin guaranteeing the API, drop the availability guard in the articulation data and the skip gating in the tests; the accessor and tests are unconditional like the sibling Jacobian and mass-matrix accessors. --- pyproject.toml | 10 +++++++-- .../jichuanh-newton-gravity-compensation.rst | 4 ++-- .../mdp/actions/pink_task_space_actions.py | 5 ++--- ...uanh-newton-gravity-compensation.minor.rst | 14 +++++++++---- .../assets/articulation/articulation_data.py | 21 +++---------------- .../test/assets/test_articulation.py | 19 ----------------- 6 files changed, 25 insertions(+), 48 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 92cca8dfdf02..ff614bdfa512 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -159,7 +159,7 @@ torchaudio = "2.11.0" ovphysx = "0.5.2+head.f62c22207c" ovrtx = ">=0.3.0,<0.4.0" # Newton git commit + matching warp-lang prerelease (upgraded together). -newton = "c7ae7c7648cd0717df39e5c94b95d5a02c997320" +newton = "b24dc255392126fe42a609ea6c4e4c8dca8009cc" warp = "1.15.0.dev20260626" [tool.ruff] @@ -326,9 +326,15 @@ prerelease = "allow" # the CUDA indexes via [tool.uv.sources]. Values mirror [tool.isaaclab.versions] above. override-dependencies = [ "numpy>=2", - "newton[sim] @ git+https://github.com/newton-physics/newton.git@c7ae7c7648cd0717df39e5c94b95d5a02c997320", + "newton[sim] @ git+https://github.com/newton-physics/newton.git@b24dc255392126fe42a609ea6c4e4c8dca8009cc", # Force the Newton-matched schemas over isaacsim's ==0.2.0 pin. "newton-usd-schemas>=0.3.1", + # The pinned Newton requires the MuJoCo 3.10 series (newton#2980); + # isaacsim-core==6.0.0.1 still exact-pins mujoco-warp==3.8.0.3. Isaac Lab owns + # the Newton stack, so force 3.10 over isaacsim's pins (same rationale as newton + # and torch above). + "mujoco-warp~=3.10.0,>=3.10.0.1", + "mujoco~=3.10.0", "torch==2.11.0", "torchvision==0.26.0", "torchaudio==2.11.0", diff --git a/source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst b/source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst index 71f036b56cd5..dee74bf27bd9 100644 --- a/source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst +++ b/source/isaaclab/changelog.d/jichuanh-newton-gravity-compensation.rst @@ -3,5 +3,5 @@ Changed * Changed task-space action docstrings that advised keeping gravity compensation disabled on the Newton backend — :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` - is implemented on Newton builds that provide the inverse-dynamics API (newton > 1.3), - so ``gravity_compensation`` / ``enable_gravity_compensation`` may be enabled there. + is now implemented on Newton, so ``gravity_compensation`` / + ``enable_gravity_compensation`` may be enabled on both backends. diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py index dee209a24fed..3a5e67ffc426 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py @@ -324,9 +324,8 @@ def _apply_gravity_compensation(self) -> None: Reads :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` and applies it as a joint-effort target on the controlled arm joints, on top - of the joint-position targets from IK. Available on PhysX and on Newton builds - that provide the inverse-dynamics API (newton > 1.3); older Newton builds raise - :class:`NotImplementedError` on the read. + of the joint-position targets from IK. Supported on both the PhysX and the + Newton backend. """ if not self._asset.cfg.spawn.rigid_props.disable_gravity: # ``gravity_compensation_forces`` shape is ``(N, num_joints + num_base_dofs)``. diff --git a/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst b/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst index 4b359eb9915b..4b251c446508 100644 --- a/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst +++ b/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst @@ -4,7 +4,13 @@ Added * Added the Newton implementation of :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces`, backed by Newton's inverse-dynamics API (``eval_inverse_dynamics``). The accessor previously - raised ``NotImplementedError`` unconditionally; it now computes the compensation - forces on Newton builds that provide the API (newton > 1.3) and keeps raising a - descriptive ``NotImplementedError`` on older builds, including the currently pinned - Newton commit. + raised ``NotImplementedError`` on the Newton backend; gravity compensation in + task-space controllers (operational-space control, Pink IK) now works on Newton. + +Changed +^^^^^^^ + +* Changed the pinned Newton commit to ``b24dc255392126fe42a609ea6c4e4c8dca8009cc``, + which includes the inverse-dynamics feature (joint-space mass matrix, gravity and + Coriolis compensation forces). The MuJoCo dependencies move from the 3.8 to the + 3.10 series with it, overriding Isaac Sim's ``mujoco-warp`` pin. diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py index 15bafd13d8cb..6ffd6e05c01f 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -11,6 +11,7 @@ import numpy as np import warp as wp +from newton import InverseDynamics from isaaclab.assets.articulation.base_articulation_data import BaseArticulationData from isaaclab.utils.buffers import TimestampedBufferWarp as TimestampedBuffer @@ -1006,15 +1007,6 @@ def gravity_compensation_forces(self) -> ProxyArray: (writes the model-wide flat DoF buffer) then a gather kernel extracts this view's DoF segment. """ - if self._inverse_dynamics_buf is None: - raise NotImplementedError( - "gravity_compensation_forces requires Newton's inverse-dynamics API:" - f" {self._inverse_dynamics_unavailable_reason}" - ) - # Local import: keeps isaaclab_newton importable against Newton versions that - # predate the inverse-dynamics API (the guard above raises before reaching it). - from newton import InverseDynamics # noqa: PLC0415 - # eval_inverse_dynamics reads ``state.body_q``; refresh FK if stale. # Matches the convention in ``body_link_pose_w`` — Python-guarded lazy refresh. self._ensure_fk_fresh() @@ -1882,15 +1874,8 @@ def _create_jacobian_buffers(self, model) -> None: # -- ``gravity_compensation_forces``: model-wide inverse-dynamics container # (eval_inverse_dynamics output buffers + private RNEA scratch) and per-view - # output (gather output). ``None`` when the installed Newton predates the - # inverse-dynamics API (AttributeError) or the model contains joints it does - # not support, e.g. CABLE (ValueError); the property raises with the reason. - try: - self._inverse_dynamics_buf = model.inverse_dynamics() - self._inverse_dynamics_unavailable_reason = "" - except (AttributeError, ValueError) as err: - self._inverse_dynamics_buf = None - self._inverse_dynamics_unavailable_reason = str(err) + # output (gather output) + self._inverse_dynamics_buf = model.inverse_dynamics() # Flat joint-space (``Model.joint_qd`` layout) DoF start of each view # articulation, for gathering flat per-DoF outputs. Host-computed once: # dof_start = joint_qd_start[first joint of the articulation]. diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index a0f92a97a06c..76116d3109ec 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -2684,20 +2684,6 @@ def test_randomize_rigid_body_collider_offsets(sim, num_articulations, device, a torch.testing.assert_close(updated_gap, new_gap) -def _skip_if_newton_lacks_inverse_dynamics() -> None: - """Skip the current test when the installed Newton predates the inverse-dynamics API. - - ``gravity_compensation_forces`` needs ``newton.eval_inverse_dynamics`` (merged - upstream after the 1.3 release). The pinned Newton commit cannot move past it until - Isaac Sim's exact ``mujoco-warp==3.8.0.3`` pin co-resolves with Newton's MuJoCo 3.10 - requirement, so these tests activate automatically with the next Newton pin bump. - """ - import newton - - if not hasattr(newton, "InverseDynamics"): - pytest.skip("installed Newton has no inverse-dynamics API (needs newton > 1.3)") - - ## # Shape-contract regression tests for the new BaseArticulation accessors. # These pin the public shape contract so future regressions (e.g., reverting @@ -2778,7 +2764,6 @@ def test_get_gravity_compensation_forces_shape_fixed_base(sim, num_articulations sizing — a wrong view gather (model-wide ``max_dofs`` or a bad flat DoF offset) would surface here in heterogeneous scenes. """ - _skip_if_newton_lacks_inverse_dynamics() articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) sim.reset() @@ -2865,7 +2850,6 @@ def test_get_gravity_compensation_forces_shape_floating_base( Includes the 6 floating-base entries on the DoF axis, matching the cross-library industry convention. """ - _skip_if_newton_lacks_inverse_dynamics() articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) sim.reset() @@ -3116,7 +3100,6 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( upstream in newton#2625 (wrong gravity compensation under non-identity root pose). """ - _skip_if_newton_lacks_inverse_dynamics() articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) sim.reset() @@ -3253,7 +3236,6 @@ def test_gravity_compensation_refreshes_after_manual_joint_write(sim, num_articu the mass matrix. Gravity stays enabled (the default) — with gravity off, ``g(q)`` is identically zero and the assert would be vacuous. """ - _skip_if_newton_lacks_inverse_dynamics() articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) sim.reset() @@ -3288,7 +3270,6 @@ def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulatio surface as joint drift, while a controller-level test would have those bugs averaged out by PD damping. """ - _skip_if_newton_lacks_inverse_dynamics() base_cfg = generate_articulation_cfg(articulation_type=articulation_type) # Replace default Franka actuators with a passthrough implicit actuator # (stiffness = 0, damping = 0). With both gains zero the effort target From 752cc91bd5587583a7011b7d82cd31a9c132ed5c Mon Sep 17 00:00:00 2001 From: jichuanh Date: Fri, 10 Jul 2026 21:16:51 +0000 Subject: [PATCH 04/14] Add OSC gravity compensation precision test on Newton Add a two-phase end-effector hold test: with gravity acting and compensation off, the OSC hold must sag past a 1.2 cm floor; flipping gravity_compensation mid-episode (read per compute call) must recover the hold to under 5 mm and by at least 10x. This validates the new Newton gravity_compensation_forces accessor end-to-end through the controller loop, complementing the force-level Jacobian cross-check. Measured on the pinned stack: 23.7 mm uncompensated sag vs 1.65 mm compensated, deterministic across runs. The floor assertion keeps the setup discriminating if the task stiffness is ever raised. The Franka home-pose helper gains a disable_gravity parameter since FRANKA_PANDA_HIGH_PD_CFG ships with per-body gravity disabled, and the stale 'Newton lacks a gravity-comp primitive' docstring in the sibling OSC sentinel now points at the new test. --- .../test/assets/test_articulation.py | 100 +++++++++++++++++- 1 file changed, 95 insertions(+), 5 deletions(-) diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 76116d3109ec..8f1910075315 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -360,7 +360,7 @@ def generate_articulation( # --------------------------------------------------------------------------- -def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False): +def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False, disable_gravity: bool = True): """Build a Franka articulation at its configured home pose. Constructs :data:`FRANKA_PANDA_HIGH_PD_CFG`, optionally zeroes the @@ -376,6 +376,9 @@ def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False): actuator stiffness and damping to zero. Used by the OSC test so OSC's joint-effort output is not opposed by the implicit-PD's residual ``kp·(target − q)``. + disable_gravity: Per-body gravity flag written to the spawn config. + :data:`FRANKA_PANDA_HIGH_PD_CFG` ships with gravity disabled; + pass False for tests where the arm must feel scene gravity. Returns: Tuple of ``(robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids)``. @@ -386,6 +389,7 @@ def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False): cfg.actuators["panda_shoulder"].damping = 0.0 cfg.actuators["panda_forearm"].stiffness = 0.0 cfg.actuators["panda_forearm"].damping = 0.0 + cfg.spawn.rigid_props.disable_gravity = disable_gravity sim_utils.create_prim("/World/Env_0", "Xform", translation=(0.0, 0.0, 0.0)) robot = Articulation(cfg) sim.reset() @@ -3432,10 +3436,10 @@ def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_en and asserts a loose regression bound rather than a tight correctness oracle. - Newton lacks a gravity-comp primitive (see ``xfail`` test below; - upstream Newton issues #2497, #2529, #2625), so OSC runs with - ``gravity_compensation=False`` and the test isolates from gravity by - disabling scene gravity. ``inertial_dynamics_decoupling=True`` + OSC runs with ``gravity_compensation=False`` and scene gravity disabled + so the sentinel isolates the J/M bridge; the gravity-compensation path is + covered by :func:`test_franka_osc_gravity_compensation_precision`. + ``inertial_dynamics_decoupling=True`` exercises ``mass_matrix`` and the Newton COM-referenced J → M_b → J product. The actuator PD is zeroed at cfg time so OSC's joint-effort output is not opposed by ``kp·(target − q)``. @@ -3505,5 +3509,91 @@ def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_en assert rot_mean < 5e-2, f"OSC rot_mean {rot_mean:.5f} > 0.05 rad — bridge regression?" +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.parametrize("gravity_enabled", [True]) +@pytest.mark.isaacsim_ci +def test_franka_osc_gravity_compensation_precision(sim, device, articulation_type, gravity_enabled): + """Two-phase EE hold: gravity sag without compensation, tight hold with it. + + Same OSC pose-hold loop as :func:`test_franka_osc_tracking_accuracy`, but + with scene and per-body gravity ON and the target pinned to the initial EE + pose, so any steady-state error is pure gravity sag. Phase 1 runs with + ``gravity_compensation=False`` and must sag past a floor; phase 2 flips + ``osc.cfg.gravity_compensation`` — read per :meth:`compute` call, so the + flag is the only variable across phases (the gravity tensor is fetched and + passed in both) — and must recover to the sibling sentinel's 5 mm bound. + + The floor assertion keeps the test discriminating: if the task stiffness + is ever raised high enough to mask gravity, phase 1 stops clearing the + floor and the test fails loudly instead of silently passing on a + non-discriminating setup. The gravity feed-forward consumes + :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` + (Newton RNEA via ``eval_inverse_dynamics``) live in the loop, covering the + FK-staleness refresh on every step of phase 2. + """ + robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose( + sim, zero_actuator_pd=True, disable_gravity=False + ) + + osc = OperationalSpaceController( + OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ), + num_envs=1, + device=device, + ) + + sim.step() + robot.update(sim.cfg.dt) + # Hold the initial EE pose: phase-1 steady-state error is pure gravity sag. + target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.0, 0.0, 0.0), device) + + def run_phase(num_steps: int) -> list[float]: + pos_history: list[float] = [] + for _ in range(num_steps): + jacobian_b = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) + mass_matrix = robot.data.mass_matrix.torch[:, arm_joint_ids, :][:, :, arm_joint_ids] + gravity = robot.data.gravity_compensation_forces.torch[:, arm_joint_ids] + ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) + ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) + joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids] + ee_vel_b = _compute_ee_vel_root(jacobian_b, joint_vel) + + osc.set_command(target_pose_b, current_ee_pose_b=ee_pose_b) + joint_efforts = osc.compute( + jacobian_b=jacobian_b, + current_ee_pose_b=ee_pose_b, + current_ee_vel_b=ee_vel_b, + mass_matrix=mass_matrix, + gravity=gravity, + ) + robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) + robot.write_data_to_sim() + sim.step() + robot.update(sim.cfg.dt) + + pos_error, _ = compute_pose_error(ee_pos_b, ee_quat_b, target_pose_b[:, 0:3], target_pose_b[:, 3:7]) + pos_history.append(pos_error.norm(dim=-1).max().item()) + return pos_history + + _, pos_off = _summarize_history(run_phase(400)) + osc.cfg.gravity_compensation = True + _, pos_on = _summarize_history(run_phase(400)) + + print(f"GRAVCOMP_METRIC pos_off={pos_off:.5f} pos_on={pos_on:.5f}") + + # Calibrated on newton 9af5a9f4 / mujoco-warp 3.10.0.1: pos_off ~= 0.024, pos_on ~= 0.0017. + assert pos_off > 1.2e-2, f"uncompensated sag {pos_off:.5f} < 1.2 cm — setup no longer discriminates gravity" + assert pos_on < 5e-3, f"compensated hold {pos_on:.5f} > 5 mm — gravity compensation inaccurate" + assert pos_on < pos_off / 10.0, f"compensation only improved sag {pos_off:.5f} -> {pos_on:.5f} (<10x)" + + if __name__ == "__main__": pytest.main([__file__, "-v", "--maxfail=1"]) From 6af22b122bba762e8971fc56d39c2d06c6e484e0 Mon Sep 17 00:00:00 2001 From: jichuanh Date: Fri, 10 Jul 2026 21:32:49 +0000 Subject: [PATCH 05/14] Tighten gravity compensation test to settled precision MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Extend phase 2 from 400 to 1200 steps so the compensated hold is measured after the residual limit cycle settles: at 400 steps the tail mean still carried ~2x convergence transient (1.65 mm vs 0.71 mm settled). Tighten the precision bound from 5 mm to 2.5 mm (3.5x margin over the deterministic settled floor). Document the measured cross-backend floors at the shared task stiffness of 500: Newton ~0.71 mm vs OVPhysX ~8 um, converging to 36 um at stiffness 2000. The residual is a limit cycle from the RNEA feed-forward being integrated by a different engine (MJWarp), not a static gravity mismatch — uncompensated sag agrees with PhysX to ~1%. --- .../test/assets/test_articulation.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 8f1910075315..7436ef1124cc 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -3531,6 +3531,19 @@ def test_franka_osc_gravity_compensation_precision(sim, device, articulation_typ :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` (Newton RNEA via ``eval_inverse_dynamics``) live in the loop, covering the FK-staleness refresh on every step of phase 2. + + The task stiffness (500) deliberately matches the PhysX-side OSC + gravity-compensation test for a cross-backend-comparable setup. Measured + settled floors at this stiffness (deterministic on the pinned stack): + Newton ~0.71 mm vs PhysX (OVPhysX) ~8 um. Newton's residual is a small + limit cycle from the RNEA feed-forward being integrated by a different + engine (MJWarp implicitfast), not a static gravity mismatch — the + uncompensated sag agrees with PhysX to ~1% (23.7 vs 23.9 mm) and stiffer + gains crush the residual superlinearly (36 um at stiffness 2000). Newton + therefore needs stiffer task gains than PhysX for an equally quiet hold. + Phase 2 runs 1200 steps because the limit cycle needs ~800 steps to + settle; at 400 steps the tail mean still carries ~2x convergence + transient. """ robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose( sim, zero_actuator_pd=True, disable_gravity=False @@ -3585,13 +3598,13 @@ def run_phase(num_steps: int) -> list[float]: _, pos_off = _summarize_history(run_phase(400)) osc.cfg.gravity_compensation = True - _, pos_on = _summarize_history(run_phase(400)) + _, pos_on = _summarize_history(run_phase(1200)) print(f"GRAVCOMP_METRIC pos_off={pos_off:.5f} pos_on={pos_on:.5f}") - # Calibrated on newton 9af5a9f4 / mujoco-warp 3.10.0.1: pos_off ~= 0.024, pos_on ~= 0.0017. + # Calibrated on newton 9af5a9f4 / mujoco-warp 3.10.0.1: pos_off ~= 0.024, pos_on ~= 0.0007. assert pos_off > 1.2e-2, f"uncompensated sag {pos_off:.5f} < 1.2 cm — setup no longer discriminates gravity" - assert pos_on < 5e-3, f"compensated hold {pos_on:.5f} > 5 mm — gravity compensation inaccurate" + assert pos_on < 2.5e-3, f"compensated hold {pos_on:.5f} > 2.5 mm — gravity compensation inaccurate" assert pos_on < pos_off / 10.0, f"compensation only improved sag {pos_off:.5f} -> {pos_on:.5f} (<10x)" From bbdfa49046033ebd7696745d7594f49ef4042ccc Mon Sep 17 00:00:00 2001 From: jichuanh Date: Fri, 10 Jul 2026 21:59:27 +0000 Subject: [PATCH 06/14] Make gravity compensation test window-timing robust Long-run probing showed phase 2 does not settle to a stationary state: after ~1000 steps of decay it enters a slow 0.4-1.0 mm envelope oscillation indefinitely. A single tail-mean measurement therefore depends on where the window lands on the envelope. Bound every 200-step window mean in the second half of a 2000-step phase 2 instead (worst measured window 1.01 mm vs the 2.5 mm bound). Phase 1 does reach a stationary band by ~step 200; add an explicit stationarity guard (tail-half means within 25%, measured drift 2.2%) so an insufficient phase length fails loudly instead of silently measuring a transient. --- .../test/assets/test_articulation.py | 53 +++++++++++++------ 1 file changed, 37 insertions(+), 16 deletions(-) diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 7436ef1124cc..701ad82c108c 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -3522,7 +3522,7 @@ def test_franka_osc_gravity_compensation_precision(sim, device, articulation_typ ``gravity_compensation=False`` and must sag past a floor; phase 2 flips ``osc.cfg.gravity_compensation`` — read per :meth:`compute` call, so the flag is the only variable across phases (the gravity tensor is fetched and - passed in both) — and must recover to the sibling sentinel's 5 mm bound. + passed in both) — and every measurement window must recover under 2.5 mm. The floor assertion keeps the test discriminating: if the task stiffness is ever raised high enough to mask gravity, phase 1 stops clearing the @@ -3534,16 +3534,19 @@ def test_franka_osc_gravity_compensation_precision(sim, device, articulation_typ The task stiffness (500) deliberately matches the PhysX-side OSC gravity-compensation test for a cross-backend-comparable setup. Measured - settled floors at this stiffness (deterministic on the pinned stack): - Newton ~0.71 mm vs PhysX (OVPhysX) ~8 um. Newton's residual is a small - limit cycle from the RNEA feed-forward being integrated by a different - engine (MJWarp implicitfast), not a static gravity mismatch — the - uncompensated sag agrees with PhysX to ~1% (23.7 vs 23.9 mm) and stiffer - gains crush the residual superlinearly (36 um at stiffness 2000). Newton - therefore needs stiffer task gains than PhysX for an equally quiet hold. - Phase 2 runs 1200 steps because the limit cycle needs ~800 steps to - settle; at 400 steps the tail mean still carries ~2x convergence - transient. + floors at this stiffness (deterministic on the pinned stack): Newton + decays for ~1000 steps into a slow 0.4-1.0 mm envelope oscillation that + never fully stills, while PhysX (OVPhysX) freezes at ~8 um. Newton's + residual is a limit cycle from the RNEA feed-forward being integrated by + a different engine (MJWarp implicitfast), not a static gravity mismatch — + the uncompensated sag agrees with PhysX to ~1% (23.7 vs 23.9 mm) and + stiffer gains crush the residual superlinearly (36 um at stiffness 2000). + Newton therefore needs stiffer task gains than PhysX for an equally quiet + hold. Because phase 2 oscillates rather than settles, the precision + assertion bounds EVERY 200-step window mean in the second half of the + phase instead of a single tail mean — immune to where a measurement + window lands on the envelope. Phase 1 does reach a stationary band + (~200 steps) and carries an explicit stationarity guard. """ robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose( sim, zero_actuator_pd=True, disable_gravity=False @@ -3596,15 +3599,33 @@ def run_phase(num_steps: int) -> list[float]: pos_history.append(pos_error.norm(dim=-1).max().item()) return pos_history - _, pos_off = _summarize_history(run_phase(400)) + hist_off = run_phase(400) osc.cfg.gravity_compensation = True - _, pos_on = _summarize_history(run_phase(1200)) + hist_on = run_phase(2000) + + # Phase-1 stationarity guard: sag reaches a stationary band by ~step 200, so the + # two tail-half means must agree (measured drift 2.2%; 25% catches a still-growing sag). + off_a = sum(hist_off[-200:-100]) / 100 + off_b = sum(hist_off[-100:]) / 100 + pos_off = (off_a + off_b) / 2.0 + assert abs(off_a - off_b) < 0.25 * pos_off, ( + f"phase-1 sag not stationary: tail halves {off_a:.5f} vs {off_b:.5f} — extend phase 1 beyond 400 steps" + ) + + # Phase 2 oscillates slowly instead of settling, so bound every 200-step window + # mean in the second half — no single window placement can pass or fail by luck. + second_half = hist_on[len(hist_on) // 2 :] + win_means = [sum(second_half[i : i + 200]) / 200 for i in range(0, len(second_half), 200)] + pos_on = sum(second_half) / len(second_half) - print(f"GRAVCOMP_METRIC pos_off={pos_off:.5f} pos_on={pos_on:.5f}") + print(f"GRAVCOMP_METRIC pos_off={pos_off:.5f} pos_on={pos_on:.5f} win_max={max(win_means):.5f}") - # Calibrated on newton 9af5a9f4 / mujoco-warp 3.10.0.1: pos_off ~= 0.024, pos_on ~= 0.0007. + # Calibrated on newton 9af5a9f4 / mujoco-warp 3.10.0.1: pos_off ~= 0.024, + # pos_on ~= 0.0008, worst window ~= 0.0010. assert pos_off > 1.2e-2, f"uncompensated sag {pos_off:.5f} < 1.2 cm — setup no longer discriminates gravity" - assert pos_on < 2.5e-3, f"compensated hold {pos_on:.5f} > 2.5 mm — gravity compensation inaccurate" + assert max(win_means) < 2.5e-3, ( + f"compensated hold windows {[f'{m:.4f}' for m in win_means]} exceed 2.5 mm — gravity compensation inaccurate" + ) assert pos_on < pos_off / 10.0, f"compensation only improved sag {pos_off:.5f} -> {pos_on:.5f} (<10x)" From 819078243ba0584bf4588657a575ebd9a34b5f0b Mon Sep 17 00:00:00 2001 From: jichuanh Date: Fri, 10 Jul 2026 22:38:57 +0000 Subject: [PATCH 07/14] Run gravity compensation test with 4 solver substeps MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Probing isolated the compensated-hold oscillation to MJWarp implicitfast integration error at a single 1/120 substep: the residual scales linearly with the solver step, worsens with higher damping gains, vanishes in zero gravity, and is unaffected by how stale the per-control-step torque is — with 4 substeps the same frozen torques hold dead-still at ~1 um, quieter than OVPhysX at ~8 um. This supersedes the previous conclusion that Newton needs stiffer task gains than PhysX for a quiet hold; it needs finer solver substeps. Add a panda_fine sim config (panda with num_substeps=4, documented as load-bearing) and switch the test to it. Both phases now reach true steady states, so drop the windowed-envelope assertions for simple stationarity-guarded tail means, shorten phase 2 from 2000 to 600 steps, and tighten the precision bound from 2.5 mm to 0.1 mm (100x margin over the measured floor; detects sub-percent compensation-force errors). --- .../test/assets/test_articulation.py | 87 +++++++++++-------- 1 file changed, 49 insertions(+), 38 deletions(-) diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 701ad82c108c..c05b8e5afdc6 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -98,6 +98,25 @@ debug_mode=False, ), ), + # "panda" with 4 solver substeps: at a single 1/120 substep, MJWarp implicitfast + # carries a ~0.4-1.0 mm integration limit cycle under a gravity load that never + # settles; 4 substeps integrate the same frozen per-control-step torques to a + # dead-still ~1 um hold. Used by the gravity-compensation precision test. + "panda_fine": SimulationCfg( + dt=1 / 120, + physics=NewtonCfg( + solver_cfg=MJWarpSolverCfg( + njmax=20, + nconmax=20, + ls_iterations=20, + cone="pyramidal", + impratio=1, + integrator="implicitfast", + ), + num_substeps=4, + debug_mode=False, + ), + ), "single_joint_implicit": SimulationCfg( dt=1 / 120, physics=NewtonCfg( @@ -3510,7 +3529,7 @@ def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_en @pytest.mark.parametrize("device", ["cuda:0"]) -@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.parametrize("articulation_type", ["panda_fine"]) @pytest.mark.parametrize("gravity_enabled", [True]) @pytest.mark.isaacsim_ci def test_franka_osc_gravity_compensation_precision(sim, device, articulation_type, gravity_enabled): @@ -3522,7 +3541,7 @@ def test_franka_osc_gravity_compensation_precision(sim, device, articulation_typ ``gravity_compensation=False`` and must sag past a floor; phase 2 flips ``osc.cfg.gravity_compensation`` — read per :meth:`compute` call, so the flag is the only variable across phases (the gravity tensor is fetched and - passed in both) — and every measurement window must recover under 2.5 mm. + passed in both) — and must recover the hold to under 0.1 mm. The floor assertion keeps the test discriminating: if the task stiffness is ever raised high enough to mask gravity, phase 1 stops clearing the @@ -3533,20 +3552,16 @@ def test_franka_osc_gravity_compensation_precision(sim, device, articulation_typ FK-staleness refresh on every step of phase 2. The task stiffness (500) deliberately matches the PhysX-side OSC - gravity-compensation test for a cross-backend-comparable setup. Measured - floors at this stiffness (deterministic on the pinned stack): Newton - decays for ~1000 steps into a slow 0.4-1.0 mm envelope oscillation that - never fully stills, while PhysX (OVPhysX) freezes at ~8 um. Newton's - residual is a limit cycle from the RNEA feed-forward being integrated by - a different engine (MJWarp implicitfast), not a static gravity mismatch — - the uncompensated sag agrees with PhysX to ~1% (23.7 vs 23.9 mm) and - stiffer gains crush the residual superlinearly (36 um at stiffness 2000). - Newton therefore needs stiffer task gains than PhysX for an equally quiet - hold. Because phase 2 oscillates rather than settles, the precision - assertion bounds EVERY 200-step window mean in the second half of the - phase instead of a single tail mean — immune to where a measurement - window lands on the envelope. Phase 1 does reach a stationary band - (~200 steps) and carries an explicit stationarity guard. + gravity-compensation test for a cross-backend-comparable setup, and the + ``panda_fine`` sim config (4 solver substeps) is load-bearing: at a single + 1/120 substep MJWarp implicitfast mis-integrates the gravity-loaded hold + into a 0.4-1.0 mm limit cycle that never settles (dt-linear, worsened by + higher damping gains, gone in zero gravity — solver integration error, + not a compensation error). With 4 substeps the same per-control-step + torques hold dead-still at ~1 um, quieter than PhysX (OVPhysX) at ~8 um. + The uncompensated sag agrees with PhysX to ~1% (23.7 vs 23.9 mm), + independently validating the gravity forces. Both phases reach a true + steady state here, enforced by tail-half stationarity guards. """ robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose( sim, zero_actuator_pd=True, disable_gravity=False @@ -3599,34 +3614,30 @@ def run_phase(num_steps: int) -> list[float]: pos_history.append(pos_error.norm(dim=-1).max().item()) return pos_history + def _stationary_tail_mean(history, label): + """Mean of the last 200 samples, asserting the two tail halves agree within 25%.""" + a = sum(history[-200:-100]) / 100 + b = sum(history[-100:]) / 100 + mean = (a + b) / 2.0 + assert abs(a - b) < 0.25 * max(mean, 1e-6), ( + f"{label} not stationary: tail halves {a:.6f} vs {b:.6f} — extend the phase" + ) + return mean + hist_off = run_phase(400) osc.cfg.gravity_compensation = True - hist_on = run_phase(2000) - - # Phase-1 stationarity guard: sag reaches a stationary band by ~step 200, so the - # two tail-half means must agree (measured drift 2.2%; 25% catches a still-growing sag). - off_a = sum(hist_off[-200:-100]) / 100 - off_b = sum(hist_off[-100:]) / 100 - pos_off = (off_a + off_b) / 2.0 - assert abs(off_a - off_b) < 0.25 * pos_off, ( - f"phase-1 sag not stationary: tail halves {off_a:.5f} vs {off_b:.5f} — extend phase 1 beyond 400 steps" - ) + hist_on = run_phase(600) - # Phase 2 oscillates slowly instead of settling, so bound every 200-step window - # mean in the second half — no single window placement can pass or fail by luck. - second_half = hist_on[len(hist_on) // 2 :] - win_means = [sum(second_half[i : i + 200]) / 200 for i in range(0, len(second_half), 200)] - pos_on = sum(second_half) / len(second_half) + pos_off = _stationary_tail_mean(hist_off, "phase-1 sag") + pos_on = _stationary_tail_mean(hist_on, "phase-2 hold") - print(f"GRAVCOMP_METRIC pos_off={pos_off:.5f} pos_on={pos_on:.5f} win_max={max(win_means):.5f}") + print(f"GRAVCOMP_METRIC pos_off={pos_off:.5f} pos_on={pos_on:.6f}") - # Calibrated on newton 9af5a9f4 / mujoco-warp 3.10.0.1: pos_off ~= 0.024, - # pos_on ~= 0.0008, worst window ~= 0.0010. + # Calibrated on newton 9af5a9f4 / mujoco-warp 3.10.0.1 with 4 substeps: + # pos_off ~= 0.024, pos_on ~= 1e-6. assert pos_off > 1.2e-2, f"uncompensated sag {pos_off:.5f} < 1.2 cm — setup no longer discriminates gravity" - assert max(win_means) < 2.5e-3, ( - f"compensated hold windows {[f'{m:.4f}' for m in win_means]} exceed 2.5 mm — gravity compensation inaccurate" - ) - assert pos_on < pos_off / 10.0, f"compensation only improved sag {pos_off:.5f} -> {pos_on:.5f} (<10x)" + assert pos_on < 1e-4, f"compensated hold {pos_on:.6f} > 0.1 mm — gravity compensation inaccurate" + assert pos_on < pos_off / 10.0, f"compensation only improved sag {pos_off:.5f} -> {pos_on:.6f} (<10x)" if __name__ == "__main__": From a660f821167675678df8436325711d8c072fee7e Mon Sep 17 00:00:00 2001 From: jichuanh Date: Tue, 21 Jul 2026 19:28:42 +0000 Subject: [PATCH 08/14] Apply joint-ordering permutation in gravity compensation gather The articulation-reordering series added a user/backend joint ordering to the sibling Jacobian and mass-matrix gathers. Route the same permutation through gather_dof_force_rows so gravity compensation forces come back in user joint order, and extend the Jacobian-identity test with a reversed-ordering variant that fails on a backend-order gather. --- .../assets/articulation/articulation_data.py | 4 ++++ .../assets/articulation/kernels.py | 15 +++++++++++++-- .../test/assets/test_articulation.py | 14 ++++++++++++-- 3 files changed, 29 insertions(+), 4 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py index c57db3688c54..ae022bcf2823 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -1043,12 +1043,16 @@ def gravity_compensation_forces(self) -> ProxyArray: InverseDynamics.EvalType.GRAVITY_FORCE, self._inverse_dynamics_buf, ) + joint_ordering = self.joint_ordering wp.launch( articulation_kernels.gather_dof_force_rows, dim=self._gravity_compensation_forces_buf.shape, inputs=[ self._inverse_dynamics_buf.gravity_force, self._jacobian_view_dof_starts, + joint_ordering.user_to_backend if joint_ordering is not None else None, + self._num_base_dofs, + joint_ordering is not None, ], outputs=[self._gravity_compensation_forces_buf], device=self.device, diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py index 8a54ac7744bd..6fd0524e611e 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py @@ -740,6 +740,9 @@ def gather_mass_matrix_rows( def gather_dof_force_rows( src: wp.array(dtype=wp.float32), view_dof_starts: wp.array(dtype=wp.int32), + joint_user_to_backend: wp.array(dtype=wp.int32), + num_base_dofs: wp.int32, + has_joint_ordering: bool, dst: wp.array2d(dtype=wp.float32), ): """Copy per-view articulation DoF forces from a flat model-sized buffer into a view-sized buffer. @@ -759,11 +762,19 @@ def gather_dof_force_rows( src: Input flat DoF buffer. Shape is (model.joint_dof_count,). view_dof_starts: Flat DoF start index of each view articulation. Shape is (num_instances,). + joint_user_to_backend: Map from public actuated-joint index to backend + actuated-joint index. + num_base_dofs: Number of leading floating-base DoF entries. + has_joint_ordering: Whether ``joint_user_to_backend`` is non-identity. dst: Output DoF-force buffer for this view. Shape is (num_instances, num_joints + num_base_dofs). """ - i, d = wp.tid() - dst[i, d] = src[view_dof_starts[i] + d] + i, user_dof = wp.tid() + backend_dof = user_dof + if has_joint_ordering and user_dof >= num_base_dofs: + backend_dof = num_base_dofs + joint_user_to_backend[user_dof - num_base_dofs] + + dst[i, user_dof] = src[view_dof_starts[i] + backend_dof] @wp.kernel diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 749a73a68d62..ad56fb766516 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -3867,12 +3867,13 @@ def test_get_mass_matrix_symmetry_pd(sim, num_articulations, device, articulatio @pytest.mark.parametrize("num_articulations", [4]) -@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) +@pytest.mark.parametrize("ordering_mode", ["none", "reversed"]) @pytest.mark.isaacsim_ci def test_get_gravity_compensation_forces_matches_jacobian_gravity( - sim, num_articulations, device, add_ground_plane, articulation_type + sim, num_articulations, device, add_ground_plane, articulation_type, ordering_mode ): """``g(q)`` must equal ``-sum_b J_com_b^T (m_b * g_w)`` at the current configuration. @@ -3885,8 +3886,17 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( floating-base — a rotated, lifted root pose guard the corner fixed upstream in newton#2625 (wrong gravity compensation under non-identity root pose). + + With ``ordering_mode="reversed"`` a nonidentity joint ordering is active and + both sides of the identity must be expressed in user joint order: the + Jacobian gather applies the user->backend permutation, so a + ``gather_dof_force_rows`` that skips it returns backend-ordered forces and + breaks the identity row-wise. """ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + if ordering_mode == "reversed": + joint_names = _PANDA_JOINT_NAMES if articulation_type == "panda" else _ANYMAL_C_PHYSX_JOINT_NAMES + articulation_cfg = articulation_cfg.replace(joint_ordering=tuple(reversed(joint_names))) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) sim.reset() assert articulation.is_initialized From 9761bc224f35d2201b43eb7ae6e70a7916b2a16c Mon Sep 17 00:00:00 2001 From: jichuanh Date: Tue, 21 Jul 2026 19:30:33 +0000 Subject: [PATCH 09/14] Use has_joint_ordering flag in gravity gather call --- .../isaaclab_newton/assets/articulation/articulation_data.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py index ae022bcf2823..b769ecb3688c 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -1043,16 +1043,15 @@ def gravity_compensation_forces(self) -> ProxyArray: InverseDynamics.EvalType.GRAVITY_FORCE, self._inverse_dynamics_buf, ) - joint_ordering = self.joint_ordering wp.launch( articulation_kernels.gather_dof_force_rows, dim=self._gravity_compensation_forces_buf.shape, inputs=[ self._inverse_dynamics_buf.gravity_force, self._jacobian_view_dof_starts, - joint_ordering.user_to_backend if joint_ordering is not None else None, + self.joint_ordering.user_to_backend if self.has_joint_ordering else None, self._num_base_dofs, - joint_ordering is not None, + self.has_joint_ordering, ], outputs=[self._gravity_compensation_forces_buf], device=self.device, From d152e182de8fe7f46263a8b75f0fcc529f4fb084 Mon Sep 17 00:00:00 2001 From: jichuanh Date: Tue, 21 Jul 2026 19:52:18 +0000 Subject: [PATCH 10/14] Adapt gravity compensation to refined Newton inverse-dynamics API Newton PR #3530 (post-v1.4.0, on the 1.5 dev line that develop pins) replaced the InverseDynamics container, EvalType flags, and Model.inverse_dynamics() factory with caller-owned output arrays on eval_inverse_dynamics_passive(); each optional output is computed only when its array is passed. The gravity accessor now allocates one flat model-wide gravity-force buffer and requests just that output. Newton allocates its RNEA scratch internally per call through Warp's stream-ordered mempool allocator, which records as allocation nodes under CUDA graph capture; verified on this stack by Newton's own graph-capture test and a composed capture of the accessor's eval + gather path. --- ...uanh-newton-gravity-compensation.minor.rst | 2 +- .../assets/articulation/articulation_data.py | 31 +++++++++---------- .../assets/articulation/kernels.py | 2 +- .../test/assets/test_articulation.py | 8 ++--- 4 files changed, 21 insertions(+), 22 deletions(-) diff --git a/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst b/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst index 5fdde9bdfc1b..00d08d9b9527 100644 --- a/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst +++ b/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst @@ -3,6 +3,6 @@ Added * Added the Newton implementation of :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces`, backed by - Newton's inverse-dynamics API (``eval_inverse_dynamics``). The accessor previously + Newton's inverse-dynamics API (``eval_inverse_dynamics_passive``). The accessor previously raised ``NotImplementedError`` on the Newton backend; gravity compensation in task-space controllers (operational-space control, Pink IK) now works on Newton. diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py index b769ecb3688c..d227682026ec 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -11,7 +11,6 @@ import numpy as np import warp as wp -from newton import InverseDynamics from isaaclab.assets.articulation import ordering_kernels from isaaclab.assets.articulation.base_articulation_data import BaseArticulationData @@ -1028,26 +1027,27 @@ def mass_matrix(self) -> ProxyArray: def gravity_compensation_forces(self) -> ProxyArray: """See :attr:`isaaclab.assets.BaseArticulationData.gravity_compensation_forces`. - Newton implementation: ``eval_inverse_dynamics`` with ``EvalType.GRAVITY_FORCE`` - (writes the model-wide flat DoF buffer) then a gather kernel extracts this - view's DoF segment. + Newton implementation: ``eval_inverse_dynamics_passive`` (writes the model-wide + flat DoF buffer) then a gather kernel extracts this view's DoF segment. """ - # eval_inverse_dynamics reads ``state.body_q``; refresh FK if stale. + # eval_inverse_dynamics_passive reads ``state.body_q``; refresh FK if stale. # Matches the convention in ``body_link_pose_w`` — Python-guarded lazy refresh. self._ensure_fk_fresh() - # eval_inverse_dynamics writes every articulation in the view's model-wide flat + # eval_inverse_dynamics_passive writes every articulation in the model-wide flat # buffer (zeros outside the view); the gather kernel extracts this view's DoF - # segments. All buffers are pre-allocated for CUDA-graph capture safety. - self._root_view.eval_inverse_dynamics( + # segments. Newton allocates its RNEA scratch internally on every call through + # Warp's stream-ordered mempool allocator — capture-safe when mempools are + # enabled (validated by Newton's own graph-capture test) — so only the output + # and gather buffers need pre-allocation here. + self._root_view.eval_inverse_dynamics_passive( SimulationManager.get_state_0(), - InverseDynamics.EvalType.GRAVITY_FORCE, - self._inverse_dynamics_buf, + gravity_force=self._gravity_force_full_buf, ) wp.launch( articulation_kernels.gather_dof_force_rows, dim=self._gravity_compensation_forces_buf.shape, inputs=[ - self._inverse_dynamics_buf.gravity_force, + self._gravity_force_full_buf, self._jacobian_view_dof_starts, self.joint_ordering.user_to_backend if self.has_joint_ordering else None, self._num_base_dofs, @@ -1867,7 +1867,7 @@ def _create_jacobian_buffers(self, model) -> None: """Allocate the scratch + view-sized buffers used by task-space accessors. Newton's :meth:`eval_jacobian` / :meth:`eval_mass_matrix` / - :meth:`eval_inverse_dynamics` write into model-sized scratch buffers spanning + :meth:`eval_inverse_dynamics_passive` write into model-sized scratch buffers spanning every articulation in the model; the gather kernels in :attr:`body_com_jacobian_w` / :attr:`mass_matrix` / :attr:`gravity_compensation_forces` extract this view's rows. The @@ -1935,10 +1935,9 @@ def _create_jacobian_buffers(self, model) -> None: device=self.device, ) - # -- ``gravity_compensation_forces``: model-wide inverse-dynamics container - # (eval_inverse_dynamics output buffers + private RNEA scratch) and per-view - # output (gather output) - self._inverse_dynamics_buf = model.inverse_dynamics() + # -- ``gravity_compensation_forces``: model-wide flat gravity-force output + # (eval_inverse_dynamics_passive) and per-view output (gather output) + self._gravity_force_full_buf = wp.zeros(model.joint_dof_count, dtype=wp.float32, device=self.device) # Flat joint-space (``Model.joint_qd`` layout) DoF start of each view # articulation, for gathering flat per-DoF outputs. Host-computed once: # dof_start = joint_qd_start[first joint of the articulation]. diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py index 6fd0524e611e..3842f6d78fa8 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py @@ -749,7 +749,7 @@ def gather_dof_force_rows( Flat-layout analogue of :func:`gather_mass_matrix_rows` for per-DoF outputs written in :attr:`newton.Model.joint_qd` layout (e.g. the gravity - compensation force from ``newton.eval_inverse_dynamics``). The DoF axis is + compensation force from ``newton.eval_inverse_dynamics_passive``). The DoF axis is preserved in full (including the leading 6 free-root entries for floating-base articulations), matching the cross-library industry convention used by PhysX, Pinocchio, Drake, MuJoCo, RBDL, OCS2, and diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index ad56fb766516..c61dfe877be4 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -3878,7 +3878,7 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( """``g(q)`` must equal ``-sum_b J_com_b^T (m_b * g_w)`` at the current configuration. Newton computes the gravity compensation force through an RNEA pass - (``eval_inverse_dynamics``); the static identity above derives the same + (``eval_inverse_dynamics_passive``); the static identity above derives the same quantity independently from the (already contract-validated) COM-referenced Jacobian and the per-body masses, pinning the sign convention, the DoF ordering (including the 6 floating-base entries), and the flat-buffer view @@ -4027,7 +4027,7 @@ def test_gravity_compensation_refreshes_after_manual_joint_write(sim, num_articu """After ``write_joint_position_to_sim_index`` (no sim step), the gravity compensation read must reflect the new joint state. - ``g(q)`` depends on ``q`` through the RNEA pass in ``eval_inverse_dynamics``, + ``g(q)`` depends on ``q`` through the RNEA pass in ``eval_inverse_dynamics_passive``, which reads ``state.body_q``. Same FK-staleness pattern as the Jacobian and the mass matrix. Gravity stays enabled (the default) — with gravity off, ``g(q)`` is identically zero and the assert would be vacuous. @@ -4046,7 +4046,7 @@ def test_gravity_compensation_refreshes_after_manual_joint_write(sim, num_articu assert not torch.allclose(g_0, g_1, atol=1e-3), ( "gravity_compensation_forces did not change after manual joint write — " - "FK trigger likely missing before eval_inverse_dynamics." + "FK trigger likely missing before eval_inverse_dynamics_passive." ) @@ -4321,7 +4321,7 @@ def test_franka_osc_gravity_compensation_precision(sim, device, articulation_typ floor and the test fails loudly instead of silently passing on a non-discriminating setup. The gravity feed-forward consumes :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` - (Newton RNEA via ``eval_inverse_dynamics``) live in the loop, covering the + (Newton RNEA via ``eval_inverse_dynamics_passive``) live in the loop, covering the FK-staleness refresh on every step of phase 2. The task stiffness (500) deliberately matches the PhysX-side OSC From e02425875708efc471b8a396328fcdd06c4bf345 Mon Sep 17 00:00:00 2001 From: jichuanh Date: Tue, 21 Jul 2026 19:52:18 +0000 Subject: [PATCH 11/14] Add noise floor to OSC precision stationarity guard The compensated hold settles at the ~1 um solver noise floor, where the 25% relative tail-agreement check is GPU-dependent jitter roulette. A 10 um absolute floor (10% of the 0.1 mm verdict threshold) keeps the guard discriminating for real non-stationarity while ignoring jitter that cannot flip the verdict. --- .../isaaclab_newton/test/assets/test_articulation.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index c61dfe877be4..ffd985130f3e 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -4388,11 +4388,17 @@ def run_phase(num_steps: int) -> list[float]: return pos_history def _stationary_tail_mean(history, label): - """Mean of the last 200 samples, asserting the two tail halves agree within 25%.""" + """Mean of the last 200 samples, asserting the two tail halves agree within 25%. + + The relative check carries a 10 µm absolute floor: at the ~1 µm solver noise + floor of the compensated hold, tail jitter is far below the 0.1 mm verdict + threshold and cannot flip the outcome, so demanding 25% relative agreement + of micrometer-scale means would only add GPU-dependent flakiness. + """ a = sum(history[-200:-100]) / 100 b = sum(history[-100:]) / 100 mean = (a + b) / 2.0 - assert abs(a - b) < 0.25 * max(mean, 1e-6), ( + assert abs(a - b) < 0.25 * max(mean, 1e-5), ( f"{label} not stationary: tail halves {a:.6f} vs {b:.6f} — extend the phase" ) return mean From 684c70ae8377bd37137a235cb0aea3e427733353 Mon Sep 17 00:00:00 2001 From: jichuanh Date: Tue, 21 Jul 2026 20:44:38 +0000 Subject: [PATCH 12/14] Compose flat DoF starts on device in gravity gather MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace the host-side numpy precompute of per-view flat DoF start offsets with two integer indirections in the gather kernel over the device-resident Model.articulation_start / Model.joint_qd_start topology arrays — no host round-trip at init and one fewer persistent buffer, matching the device-native style of the sibling gathers. --- .../assets/articulation/articulation_data.py | 16 ++++++++-------- .../assets/articulation/kernels.py | 19 +++++++++++++++---- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py index d227682026ec..9547ca60e877 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -1048,7 +1048,9 @@ def gravity_compensation_forces(self) -> ProxyArray: dim=self._gravity_compensation_forces_buf.shape, inputs=[ self._gravity_force_full_buf, - self._jacobian_view_dof_starts, + self._jacobian_view_art_ids, + self._sim_bind_articulation_start, + self._sim_bind_joint_qd_start, self.joint_ordering.user_to_backend if self.has_joint_ordering else None, self._num_base_dofs, self.has_joint_ordering, @@ -1938,13 +1940,11 @@ def _create_jacobian_buffers(self, model) -> None: # -- ``gravity_compensation_forces``: model-wide flat gravity-force output # (eval_inverse_dynamics_passive) and per-view output (gather output) self._gravity_force_full_buf = wp.zeros(model.joint_dof_count, dtype=wp.float32, device=self.device) - # Flat joint-space (``Model.joint_qd`` layout) DoF start of each view - # articulation, for gathering flat per-DoF outputs. Host-computed once: - # dof_start = joint_qd_start[first joint of the articulation]. - art_first_joint = model.articulation_start.numpy()[self._jacobian_view_art_ids.numpy()] - self._jacobian_view_dof_starts = wp.array( - model.joint_qd_start.numpy()[art_first_joint], dtype=wp.int32, device=self.device - ) + # Model topology arrays for locating each articulation's segment in flat + # (``Model.joint_qd`` layout) per-DoF outputs; the gather kernel composes + # ``joint_qd_start[articulation_start[art_id]]`` on device. + self._sim_bind_articulation_start = model.articulation_start + self._sim_bind_joint_qd_start = model.joint_qd_start self._gravity_compensation_forces_buf = wp.zeros( (self._num_instances, self._num_joints + num_base_dofs), dtype=wp.float32, device=self.device ) diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py index 3842f6d78fa8..24016df6ceb8 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py @@ -739,7 +739,9 @@ def gather_mass_matrix_rows( @wp.kernel def gather_dof_force_rows( src: wp.array(dtype=wp.float32), - view_dof_starts: wp.array(dtype=wp.int32), + art_ids: wp.array(dtype=wp.int32), + articulation_start: wp.array(dtype=wp.int32), + joint_qd_start: wp.array(dtype=wp.int32), joint_user_to_backend: wp.array(dtype=wp.int32), num_base_dofs: wp.int32, has_joint_ordering: bool, @@ -760,8 +762,16 @@ def gather_dof_force_rows( Args: src: Input flat DoF buffer. Shape is (model.joint_dof_count,). - view_dof_starts: Flat DoF start index of each view articulation. Shape - is (num_instances,). + art_ids: Model-level articulation indices owned by this view. Shape is + (num_instances,). + articulation_start: First joint index of each model articulation + (:attr:`newton.Model.articulation_start`). Shape is + (model.articulation_count + 1,). + joint_qd_start: First velocity coordinate of each model joint + (:attr:`newton.Model.joint_qd_start`). Shape is + (model.joint_count + 1,). Composed per thread as + ``joint_qd_start[articulation_start[art_ids[i]]]`` to locate the + articulation's segment in ``src``. joint_user_to_backend: Map from public actuated-joint index to backend actuated-joint index. num_base_dofs: Number of leading floating-base DoF entries. @@ -774,7 +784,8 @@ def gather_dof_force_rows( if has_joint_ordering and user_dof >= num_base_dofs: backend_dof = num_base_dofs + joint_user_to_backend[user_dof - num_base_dofs] - dst[i, user_dof] = src[view_dof_starts[i] + backend_dof] + dof_start = joint_qd_start[articulation_start[art_ids[i]]] + dst[i, user_dof] = src[dof_start + backend_dof] @wp.kernel From 1ef273332c4121e36125001ec9d9e2fcd63e4379 Mon Sep 17 00:00:00 2001 From: jichuanh Date: Tue, 21 Jul 2026 21:30:58 +0000 Subject: [PATCH 13/14] Address self-review findings on tests and naming MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Extend the heterogeneous-scene test to read gravity compensation on both views: the flat-buffer gather's padded-layout failure mode is numerically invisible in homogeneous scenes, and drop the shape test's overclaim about covering it. - Rename the init-captured Model topology refs from _sim_bind_* to _model_* — they are not rebound by _create_simulation_bindings, so the prefix broke the file's naming convention. - Guard the rotated-root identity against vacuous agreement under a hypothetical FK-invalidation regression, and refresh the OSC precision calibration provenance to the validated pin. --- .../assets/articulation/articulation_data.py | 8 +++---- .../test/assets/test_articulation.py | 21 ++++++++++++++++--- 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py index 9547ca60e877..0fbb4cc7179f 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -1049,8 +1049,8 @@ def gravity_compensation_forces(self) -> ProxyArray: inputs=[ self._gravity_force_full_buf, self._jacobian_view_art_ids, - self._sim_bind_articulation_start, - self._sim_bind_joint_qd_start, + self._model_articulation_start, + self._model_joint_qd_start, self.joint_ordering.user_to_backend if self.has_joint_ordering else None, self._num_base_dofs, self.has_joint_ordering, @@ -1943,8 +1943,8 @@ def _create_jacobian_buffers(self, model) -> None: # Model topology arrays for locating each articulation's segment in flat # (``Model.joint_qd`` layout) per-DoF outputs; the gather kernel composes # ``joint_qd_start[articulation_start[art_id]]`` on device. - self._sim_bind_articulation_start = model.articulation_start - self._sim_bind_joint_qd_start = model.joint_qd_start + self._model_articulation_start = model.articulation_start + self._model_joint_qd_start = model.joint_qd_start self._gravity_compensation_forces_buf = wp.zeros( (self._num_instances, self._num_joints + num_base_dofs), dtype=wp.float32, device=self.device ) diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index ffd985130f3e..3f566910204b 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -3547,8 +3547,8 @@ def test_get_gravity_compensation_forces_shape_fixed_base(sim, num_articulations """Fixed-base ``gravity_compensation_forces`` shape ``(N, num_joints)``. No floating-base entries on the DoF axis, and per-articulation output - sizing — a wrong view gather (model-wide ``max_dofs`` or a bad flat DoF - offset) would surface here in heterogeneous scenes. + sizing. Heterogeneous-scene indexing is pinned separately by + ``test_heterogeneous_scene_per_view_shapes``. """ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) @@ -3735,6 +3735,17 @@ def test_heterogeneous_scene_per_view_shapes(sim, device, add_ground_plane, arti "Anymal mass matrix has non-positive diagonal under heterogeneous scene" ) + # Gravity compensation gathers a FLAT model-wide DoF buffer, so a padded-layout + # regression (indexing by ``art_id * max_dofs`` instead of + # ``joint_qd_start[articulation_start[art_id]]``) is numerically invisible in + # homogeneous scenes — this mixed scene is the only place it can surface. + franka_g = franka.data.gravity_compensation_forces.torch + anymal_g = anymal.data.gravity_compensation_forces.torch + assert franka_g.shape == torch.Size((num_per_type, franka_dofs)) + assert anymal_g.shape == torch.Size((num_per_type, anymal_dofs)) + assert franka_g.abs().max() > 1e-3, "Franka gravity compensation is all-zero under heterogeneous scene" + assert anymal_g.abs().max() > 1e-3, "Anymal gravity compensation is all-zero under heterogeneous scene" + @pytest.mark.parametrize("num_articulations", [4]) @pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) @@ -3915,6 +3926,10 @@ def test_get_gravity_compensation_forces_matches_jacobian_gravity( # (x, y, z, w) quaternion — 30 deg roll about x. root_pose[:, 3:] = torch.tensor([0.2588, 0.0, 0.0, 0.9659], device=device) articulation.write_root_pose_to_sim_index(root_pose=root_pose) + # Guard against a vacuous identity: if root-pose FK invalidation ever + # regressed, both sides would be evaluated at the stale identity pose and + # agree trivially, voiding the newton#2625 rotated-root coverage. + torch.testing.assert_close(articulation.data.root_link_pose_w.torch, root_pose, atol=1e-5, rtol=0.0) g_meas = articulation.data.gravity_compensation_forces.torch @@ -4412,7 +4427,7 @@ def _stationary_tail_mean(history, label): print(f"GRAVCOMP_METRIC pos_off={pos_off:.5f} pos_on={pos_on:.6f}") - # Calibrated on newton 9af5a9f4 / mujoco-warp 3.10.0.1 with 4 substeps: + # Re-validated on newton 81cdcfc2 / mujoco-warp 3.10.0.2 with 4 substeps: # pos_off ~= 0.024, pos_on ~= 1e-6. assert pos_off > 1.2e-2, f"uncompensated sag {pos_off:.5f} < 1.2 cm — setup no longer discriminates gravity" assert pos_on < 1e-4, f"compensated hold {pos_on:.6f} > 0.1 mm — gravity compensation inaccurate" From 94ed363ce8b1fce9e6a3aa37095011bf085b4528 Mon Sep 17 00:00:00 2001 From: jichuanh Date: Tue, 21 Jul 2026 21:47:57 +0000 Subject: [PATCH 14/14] Source gather topology arrays from the view's model at read time Caching Model.articulation_start / Model.joint_qd_start at buffer creation added a staleness hazard for no benefit: the eval computes against the view's model, so the gather now reads the topology arrays from that same object per call and can never mix models across a rebuild. --- .../assets/articulation/articulation_data.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py index 0fbb4cc7179f..6470fab876c9 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -1043,14 +1043,17 @@ def gravity_compensation_forces(self) -> ProxyArray: SimulationManager.get_state_0(), gravity_force=self._gravity_force_full_buf, ) + # Topology arrays come from the same Model object the eval above computed + # against (the view's), so the gather can never mix models across a rebuild. + model = self._root_view.model wp.launch( articulation_kernels.gather_dof_force_rows, dim=self._gravity_compensation_forces_buf.shape, inputs=[ self._gravity_force_full_buf, self._jacobian_view_art_ids, - self._model_articulation_start, - self._model_joint_qd_start, + model.articulation_start, + model.joint_qd_start, self.joint_ordering.user_to_backend if self.has_joint_ordering else None, self._num_base_dofs, self.has_joint_ordering, @@ -1940,11 +1943,6 @@ def _create_jacobian_buffers(self, model) -> None: # -- ``gravity_compensation_forces``: model-wide flat gravity-force output # (eval_inverse_dynamics_passive) and per-view output (gather output) self._gravity_force_full_buf = wp.zeros(model.joint_dof_count, dtype=wp.float32, device=self.device) - # Model topology arrays for locating each articulation's segment in flat - # (``Model.joint_qd`` layout) per-DoF outputs; the gather kernel composes - # ``joint_qd_start[articulation_start[art_id]]`` on device. - self._model_articulation_start = model.articulation_start - self._model_joint_qd_start = model.joint_qd_start self._gravity_compensation_forces_buf = wp.zeros( (self._num_instances, self._num_joints + num_base_dofs), dtype=wp.float32, device=self.device )