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..dee74bf27bd9 --- /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 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 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..00d08d9b9527 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/jichuanh-newton-gravity-compensation.minor.rst @@ -0,0 +1,8 @@ +Added +^^^^^ + +* Added the Newton implementation of + :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces`, backed by + 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 4c27091dd3cb..6470fab876c9 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -1027,22 +1027,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_passive`` (writes the model-wide + flat DoF buffer) then a gather kernel extracts this view's DoF segment. + """ + # 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_passive writes every articulation in the model-wide flat + # buffer (zeros outside the view); the gather kernel extracts this view's DoF + # 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(), + 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, + 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, + ], + outputs=[self._gravity_compensation_forces_buf], + device=self.device, + ) + return self._gravity_compensation_forces_ta """ Joint state properties. @@ -1852,9 +1871,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_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 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 @@ -1919,6 +1940,13 @@ def _create_jacobian_buffers(self, model) -> None: device=self.device, ) + # -- ``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) + self._gravity_compensation_forces_buf = wp.zeros( + (self._num_instances, self._num_joints + num_base_dofs), dtype=wp.float32, device=self.device + ) + def _validate_joint_ordering_buffers(self) -> None: """Validate buffers required while nonidentity joint ordering is active.""" required_fields = ( @@ -2361,6 +2389,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 8ee274e31a99..24016df6ceb8 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py @@ -736,6 +736,58 @@ def gather_mass_matrix_rows( dst[i, user_row, user_col] = src[art_ids[i], backend_row, backend_col] +@wp.kernel +def gather_dof_force_rows( + src: wp.array(dtype=wp.float32), + 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, + 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_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 + 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,). + 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. + 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, 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] + + dof_start = joint_qd_start[articulation_start[art_ids[i]]] + dst[i, user_dof] = src[dof_start + backend_dof] + + @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 5f6218320ab4..3f566910204b 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -106,6 +106,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( @@ -438,7 +457,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 @@ -454,6 +473,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)``. @@ -464,6 +486,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() @@ -3447,51 +3470,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", test_devices(DeviceScope.CUDA)) -@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 @@ -3561,6 +3539,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", test_devices(DeviceScope.CUDA)) +@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. 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) + 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", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -3621,6 +3623,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", test_devices(DeviceScope.CUDA)) +@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", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("articulation_type", ["anymal"]) @@ -3707,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)) @@ -3838,6 +3877,78 @@ 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", 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, ordering_mode +): + """``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_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 + 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). + + 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 + + # 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) + # 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 + + # 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", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) @@ -3923,6 +4034,122 @@ def test_mass_matrix_refreshes_after_manual_joint_write( ) +@pytest.mark.parametrize("num_articulations", [1]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) +@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_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. + """ + 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_passive." + ) + + +@pytest.mark.parametrize("num_articulations", [1]) +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) +@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", test_devices(DeviceScope.CUDA)) @pytest.mark.parametrize("articulation_type", ["panda"]) @pytest.mark.parametrize("gravity_enabled", [False]) @@ -4016,10 +4243,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)``. @@ -4089,5 +4316,123 @@ 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_fine"]) +@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 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 + 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_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 + 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 + ) + + 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 + + def _stationary_tail_mean(history, label): + """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-5), ( + 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(600) + + 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:.6f}") + + # 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" + assert pos_on < pos_off / 10.0, f"compensation only improved sag {pos_off:.5f} -> {pos_on:.6f} (<10x)" + + if __name__ == "__main__": pytest.main([__file__, "-v", "--maxfail=1"]) 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 133dd31b875b..543f2c150219 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -2816,12 +2816,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