diff --git a/scripts/demos/haply_teleoperation.py b/scripts/demos/haply_teleoperation.py
index a1c5a7cec677..0f24ebd4d759 100644
--- a/scripts/demos/haply_teleoperation.py
+++ b/scripts/demos/haply_teleoperation.py
@@ -191,6 +191,9 @@ def run_simulator(
ee_body_name = "panda_hand"
ee_body_idx = robot.body_names.index(ee_body_name)
+ # ``body_link_jacobian_w`` drops the fixed-root body row for fixed-base assets,
+ # so the Jacobian-axis body index is ``body_idx - 1`` in that case.
+ ee_jacobi_body_idx = ee_body_idx - 1 if robot.is_fixed_base else ee_body_idx
joint_pos = robot.data.default_joint_pos.torch.clone()
joint_pos[0, :7] = torch.tensor([0.0, -0.569, 0.0, -2.81, 0.0, 3.037, 0.741], device=robot.device)
@@ -307,8 +310,11 @@ def run_simulator(
ee_pos_w = robot.data.body_pos_w.torch[:, ee_body_idx]
ee_quat_w = robot.data.body_quat_w.torch[:, ee_body_idx]
- # get jacobian to IK controller
- jacobian = robot.root_view.get_jacobians()[:, ee_body_idx, :, arm_joint_indices]
+ # get jacobian to IK controller. The DoF axis prepends ``num_base_dofs``
+ # floating-base columns (0 for fixed-base, 6 for floating-base); shift the
+ # actuated-joint ids by ``num_base_dofs`` to address the actuated columns.
+ jacobi_joint_ids = [j + robot.num_base_dofs for j in arm_joint_indices]
+ jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_body_idx, :, jacobi_joint_ids]
ik_controller.set_command(command=target_pos_tensor, ee_quat=ee_quat_w)
joint_pos_des = ik_controller.compute(ee_pos_w, ee_quat_w, jacobian, current_joint_pos)
diff --git a/scripts/tutorials/05_controllers/run_diff_ik.py b/scripts/tutorials/05_controllers/run_diff_ik.py
index 2922265e7b9b..a4e88b9d215e 100644
--- a/scripts/tutorials/05_controllers/run_diff_ik.py
+++ b/scripts/tutorials/05_controllers/run_diff_ik.py
@@ -159,8 +159,11 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
# change goal
current_goal_idx = (current_goal_idx + 1) % len(ee_goals)
else:
- # obtain quantities from simulation
- jacobian = robot.root_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]
+ # obtain quantities from simulation. The Jacobian DoF axis prepends
+ # ``num_base_dofs`` floating-base columns (0 for fixed-base, 6 for
+ # floating-base); shift the actuated-joint ids accordingly.
+ jacobi_joint_ids = [j + robot.num_base_dofs for j in robot_entity_cfg.joint_ids]
+ jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, jacobi_joint_ids]
ee_pose_w = robot.data.body_pose_w.torch[:, robot_entity_cfg.body_ids[0]]
root_pose_w = robot.data.root_pose_w.torch
joint_pos = robot.data.joint_pos.torch[:, robot_entity_cfg.joint_ids]
diff --git a/scripts/tutorials/05_controllers/run_osc.py b/scripts/tutorials/05_controllers/run_osc.py
index 09e16c40c70e..362efd5f5ee4 100644
--- a/scripts/tutorials/05_controllers/run_osc.py
+++ b/scripts/tutorials/05_controllers/run_osc.py
@@ -314,9 +314,13 @@ def update_states(
"""
# obtain dynamics related quantities from simulation
ee_jacobi_idx = ee_frame_idx - 1
- jacobian_w = robot.root_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids]
- mass_matrix = robot.root_view.get_generalized_mass_matrices()[:, arm_joint_ids, :][:, :, arm_joint_ids]
- gravity = robot.root_view.get_gravity_compensation_forces()[:, arm_joint_ids]
+ # The J / M / g DoF axis prepends ``num_base_dofs`` floating-base columns
+ # (0 for fixed-base, 6 for floating-base); shift the actuated-joint ids by
+ # ``num_base_dofs`` to address the actuated-joint columns directly.
+ jacobi_joint_ids = [j + robot.num_base_dofs for j in arm_joint_ids]
+ jacobian_w = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, jacobi_joint_ids]
+ mass_matrix = robot.data.mass_matrix.torch[:, jacobi_joint_ids, :][:, :, jacobi_joint_ids]
+ gravity = robot.data.gravity_compensation_forces.torch[:, jacobi_joint_ids]
# Convert the Jacobian from world to root frame
jacobian_b = jacobian_w.clone()
root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w.torch))
diff --git a/source/isaaclab/changelog.d/jichuanh-ik-newton-compat-mvp.minor.rst b/source/isaaclab/changelog.d/jichuanh-ik-newton-compat-mvp.minor.rst
new file mode 100644
index 000000000000..b68d62e6b744
--- /dev/null
+++ b/source/isaaclab/changelog.d/jichuanh-ik-newton-compat-mvp.minor.rst
@@ -0,0 +1,60 @@
+Added
+^^^^^
+
+* Added :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` and
+ :attr:`~isaaclab.assets.BaseArticulationData.body_com_jacobian_w` properties,
+ exposing the per-body geometric Jacobian referenced at the link origin and
+ body center of mass respectively. The pair mirrors the existing
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_pose_w` /
+ :attr:`~isaaclab.assets.BaseArticulationData.body_com_pose_w` and
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_vel_w` /
+ :attr:`~isaaclab.assets.BaseArticulationData.body_com_vel_w` exposure pattern.
+ Backends without a native primitive raise :class:`NotImplementedError`.
+* Added :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix` property,
+ exposing the joint-space generalized mass matrix ``M(q)``.
+* Added :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces`
+ property, exposing the joint-space gravity-loading torque vector ``g(q)``.
+* Added :attr:`~isaaclab.assets.BaseArticulation.num_base_dofs` — number of
+ free DoFs of the floating base (``0`` for fixed-base, ``6`` for floating-
+ base). Use it to map an actuated-joint index ``j`` to its column in the
+ Jacobian / mass matrix / gravity vector via ``j + num_base_dofs``.
+
+The Jacobian / mass-matrix / gravity-comp DoF axis includes the floating-
+base DoFs at the front: shape ``(N, num_jacobi_bodies, 6, num_joints +
+num_base_dofs)`` for the Jacobian and ``(N, num_joints + num_base_dofs,
+num_joints + num_base_dofs)`` for the mass matrix. This matches the
+cross-library industry convention (Pinocchio's ``nv = 6 + n_actuated``,
+Drake's ephemeral floating joint, MuJoCo's ````, RBDL's
+``JointTypeFloatingBase``, OCS2's ``generalizedCoordinatesNum =
+6 + actuatedJointsNum``, iDynTree's ``getFreeFloatingMassMatrix``
+returning ``(6 + dofs, 6 + dofs)``).
+
+Changed
+^^^^^^^
+
+* Migrated :class:`~isaaclab.envs.mdp.actions.task_space_actions.DifferentialInverseKinematicsAction`,
+ :class:`~isaaclab.envs.mdp.actions.task_space_actions.OperationalSpaceControllerAction`,
+ and :class:`~isaaclab.envs.mdp.actions.rmpflow_task_space_actions.RMPFlowAction`
+ to fetch dynamic quantities through the new
+ :class:`~isaaclab.assets.BaseArticulationData` properties instead of the
+ PhysX-only ``root_view``. The OSC action term now also gates the
+ per-step mass-matrix and gravity-compensation fetches behind the
+ controller cfg's :attr:`inertial_dynamics_decoupling`,
+ :attr:`nullspace_control`, and :attr:`gravity_compensation` flags
+ so backends without a native primitive are not invoked when the
+ controller does not consume the result.
+* Action terms (DiffIK / OSC / RMPFlow / Pink) compute their Jacobian
+ joint-axis indices via
+ ``[j + asset.num_base_dofs for j in joint_ids]``, which is ``0`` for
+ fixed-base and ``+6`` for floating-base. Pink IK previously hardcoded
+ a private ``_physx_floating_joint_indices_offset = 6``; that was
+ removed in favor of the cross-backend property.
+* PhysX backend's :attr:`body_link_jacobian_w` applies the COM→origin shift to
+ PhysX's natively COM-referenced Jacobian. The previously-exposed
+ ``Articulation.get_jacobians()`` was a passthrough that returned the raw
+ COM-referenced Jacobian, while IK / OSC consumers also read
+ :attr:`body_link_pose_w` as the EE pose setpoint — a frame mismatch that
+ produced a ``ω × r_com_w`` per-body bias in tracking. The new property
+ reads the same engine buffer and applies the shift so ``J · q_dot`` matches
+ ``body_link_lin_vel_w``. Consumers that intentionally want the raw
+ COM-referenced form can read :attr:`body_com_jacobian_w`.
diff --git a/source/isaaclab/isaaclab/assets/articulation/base_articulation.py b/source/isaaclab/isaaclab/assets/articulation/base_articulation.py
index dc9ddc6cb7ad..ff3f99e8a024 100644
--- a/source/isaaclab/isaaclab/assets/articulation/base_articulation.py
+++ b/source/isaaclab/isaaclab/assets/articulation/base_articulation.py
@@ -174,6 +174,19 @@ def root_view(self):
"""
raise NotImplementedError()
+ @property
+ def num_base_dofs(self) -> int:
+ """Number of free DoFs of the floating base.
+
+ A floating-base articulation can translate and rotate freely in space, so
+ its base contributes 6 DoFs (3 linear, 3 angular). A fixed-base articulation
+ is bolted to the world and contributes 0.
+
+ Use this to map an actuated-joint index ``j`` to its column in the Jacobian
+ / mass matrix / gravity vector: ``column = j + num_base_dofs``.
+ """
+ return 0 if self.is_fixed_base else 6
+
@property
@abstractmethod
def instantaneous_wrench_composer(self) -> WrenchComposer:
diff --git a/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py b/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py
index b902db7d29bb..894bb6bd8f44 100644
--- a/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py
+++ b/source/isaaclab/isaaclab/assets/articulation/base_articulation_data.py
@@ -669,6 +669,83 @@ def body_com_pose_b(self) -> ProxyArray:
"""
raise NotImplementedError
+ ##
+ # Dynamics quantities (task-space controllers).
+ ##
+
+ @property
+ def body_link_jacobian_w(self) -> ProxyArray:
+ """Per-body geometric Jacobian referenced at each body's link origin in world frame.
+
+ Shape: ``(num_instances, num_jacobi_bodies, 6, num_joints + num_base_dofs)``,
+ dtype ``wp.float32``. Linear rows ``[0:3]`` [m/s per unit DoF velocity];
+ angular rows ``[3:6]`` [rad/s per unit DoF velocity].
+
+ Contract: for any generalized velocity ``v`` of length
+ ``num_joints + num_base_dofs``,
+
+ .. code-block:: text
+
+ J[:, jacobi_body_idx, 0:3, :] @ v == body_link_lin_vel_w[:, body_idx]
+ J[:, jacobi_body_idx, 3:6, :] @ v == body_link_ang_vel_w[:, body_idx]
+
+ Conventions:
+ * Body axis: ``jacobi_body_idx == body_idx - 1`` for fixed-base (fixed-root
+ row excluded); ``jacobi_body_idx == body_idx`` for floating-base.
+ * DoF axis: leading
+ :attr:`~isaaclab.assets.BaseArticulation.num_base_dofs` floating-base
+ columns (world-frame ``[lin_x, lin_y, lin_z, ang_x, ang_y, ang_z]``),
+ then actuated-joint columns in :attr:`joint_names` order.
+ """
+ raise NotImplementedError(f"{type(self).__name__} does not implement body_link_jacobian_w.")
+
+ @property
+ def body_com_jacobian_w(self) -> ProxyArray:
+ """Per-body geometric Jacobian referenced at each body's center of mass in world frame.
+
+ Same shape and indexing conventions as :attr:`body_link_jacobian_w`. Linear
+ rows ``[0:3]`` give the velocity at the body's center of mass; angular rows
+ ``[3:6]`` are reference-point invariant (identical to
+ :attr:`body_link_jacobian_w`).
+
+ Contract: for any generalized velocity ``v``,
+
+ .. code-block:: text
+
+ J[:, jacobi_body_idx, 0:3, :] @ v == body_com_lin_vel_w[:, body_idx]
+ J[:, jacobi_body_idx, 3:6, :] @ v == body_com_ang_vel_w[:, body_idx]
+ """
+ raise NotImplementedError(f"{type(self).__name__} does not implement body_com_jacobian_w.")
+
+ @property
+ def mass_matrix(self) -> ProxyArray:
+ """Per-env generalized mass matrix ``M(q)`` in joint space.
+
+ Shape: ``(num_instances, num_joints + num_base_dofs, num_joints + num_base_dofs)``,
+ dtype ``wp.float32`` [kg·m² or kg, per DoF type]. DoF-axis convention matches
+ :attr:`body_link_jacobian_w`.
+
+ ``M(q)`` is symmetric positive-definite. ``M[i, j]`` is the coefficient
+ relating DoF ``j``'s acceleration to the inertial torque on DoF ``i`` in
+ ``M(q) q_ddot + C(q, q_dot) q_dot + g(q) = tau``.
+ """
+ raise NotImplementedError(f"{type(self).__name__} does not implement mass_matrix.")
+
+ @property
+ def gravity_compensation_forces(self) -> ProxyArray:
+ """Per-env gravity compensation torques ``g(q)`` in joint space.
+
+ Shape: ``(num_instances, num_joints + num_base_dofs)``, dtype ``wp.float32``
+ [N·m or N, per DoF type]. DoF-axis convention matches
+ :attr:`body_link_jacobian_w`.
+
+ ``g(q)`` is the gravity-loading term in
+ ``M(q) q_ddot + C(q, q_dot) q_dot + g(q) = tau``. Applying ``tau = g(q)`` at
+ ``q_dot = 0`` with no external load yields ``q_ddot = 0`` (static equilibrium
+ under gravity).
+ """
+ raise NotImplementedError(f"{type(self).__name__} does not implement gravity_compensation_forces.")
+
##
# Joint state properties.
##
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 f826c80d51eb..1612c6621c71 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
@@ -9,7 +9,6 @@
from typing import TYPE_CHECKING
import torch
-import warp as wp
from pink.tasks import FrameTask
import isaaclab.utils.math as math_utils
@@ -60,9 +59,6 @@ def __init__(self, cfg: pink_actions_cfg.PinkInverseKinematicsActionCfg, env: Ma
self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device)
self._processed_actions = torch.zeros_like(self._raw_actions)
- # PhysX Articulation Floating joint indices offset from IsaacLab Articulation joint indices
- self._physx_floating_joint_indices_offset = 6
-
# Pre-allocate tensors for runtime use
self._initialize_helper_tensors()
@@ -324,20 +320,24 @@ def apply_actions(self) -> None:
)
def _apply_gravity_compensation(self) -> None:
- """Apply gravity compensation to arm joints if not disabled in props."""
+ """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``.
+ """
if not self._asset.cfg.spawn.rigid_props.disable_gravity:
- # Get gravity compensation forces using cached tensor
+ # ``gravity_compensation_forces`` shape is ``(N, num_joints + num_base_dofs)``.
+ # Shift actuated-joint ids by ``num_base_dofs`` to skip the leading floating-
+ # base columns (0 for fixed-base, 6 for floating-base).
+ jacobi_ids = self._controlled_joint_ids_tensor + self._asset.num_base_dofs
if self._asset.is_fixed_base:
- gravity = torch.zeros_like(
- wp.to_torch(self._asset.root_view.get_gravity_compensation_forces())[
- :, self._controlled_joint_ids_tensor
- ]
- )
+ gravity = torch.zeros_like(self._asset.data.gravity_compensation_forces.torch[:, jacobi_ids])
else:
- # If floating base, then need to skip the first 6 joints (base)
- gravity = wp.to_torch(self._asset.root_view.get_gravity_compensation_forces())[
- :, self._controlled_joint_ids_tensor + self._physx_floating_joint_indices_offset
- ]
+ gravity = self._asset.data.gravity_compensation_forces.torch[:, jacobi_ids]
# Apply gravity compensation to arm joints
self._asset.set_joint_effort_target_index(target=gravity, joint_ids=self._controlled_joint_ids)
diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/rmpflow_task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/rmpflow_task_space_actions.py
index 922db073a300..6cccf308e1d5 100644
--- a/source/isaaclab/isaaclab/envs/mdp/actions/rmpflow_task_space_actions.py
+++ b/source/isaaclab/isaaclab/envs/mdp/actions/rmpflow_task_space_actions.py
@@ -10,7 +10,6 @@
from typing import TYPE_CHECKING
import torch
-import warp as wp
import isaaclab.utils.math as math_utils
import isaaclab.utils.string as string_utils
@@ -56,15 +55,8 @@ def __init__(self, cfg: rmpflow_actions_cfg.RMPFlowActionCfg, env: ManagerBasedE
self._body_idx = body_ids[0]
self._body_name = body_names[0]
- # check if articulation is fixed-base
- # if fixed-base then the jacobian for the base is not computed
- # this means that number of bodies is one less than the articulation's number of bodies
- if self._asset.is_fixed_base:
- self._jacobi_body_idx = self._body_idx - 1
- self._jacobi_joint_ids = self._joint_ids
- else:
- self._jacobi_body_idx = self._body_idx
- self._jacobi_joint_ids = [i + 6 for i in self._joint_ids]
+ self._jacobi_body_idx = self._body_idx - 1 if self._asset.is_fixed_base else self._body_idx
+ self._jacobi_joint_ids = [j + self._asset.num_base_dofs for j in self._joint_ids]
# log info for debugging
logger.info(
@@ -128,7 +120,7 @@ def processed_actions(self) -> torch.Tensor:
@property
def jacobian_w(self) -> torch.Tensor:
- return wp.to_torch(self._asset.root_view.get_jacobians())[:, self._jacobi_body_idx, :, self._jacobi_joint_ids]
+ return self._asset.data.body_link_jacobian_w.torch[:, self._jacobi_body_idx, :, self._jacobi_joint_ids]
@property
def jacobian_b(self) -> torch.Tensor:
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 4aa66367d2c2..507a2053b585 100644
--- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py
+++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py
@@ -10,7 +10,6 @@
from typing import TYPE_CHECKING
import torch
-import warp as wp
from pxr import UsdPhysics
@@ -72,15 +71,8 @@ def __init__(self, cfg: actions_cfg.DifferentialInverseKinematicsActionCfg, env:
# save only the first body index
self._body_idx = body_ids[0]
self._body_name = body_names[0]
- # check if articulation is fixed-base
- # if fixed-base then the jacobian for the base is not computed
- # this means that number of bodies is one less than the articulation's number of bodies
- if self._asset.is_fixed_base:
- self._jacobi_body_idx = self._body_idx - 1
- self._jacobi_joint_ids = self._joint_ids
- else:
- self._jacobi_body_idx = self._body_idx
- self._jacobi_joint_ids = [i + 6 for i in self._joint_ids]
+ self._jacobi_body_idx = self._body_idx - 1 if self._asset.is_fixed_base else self._body_idx
+ self._jacobi_joint_ids = [j + self._asset.num_base_dofs for j in self._joint_ids]
# log info for debugging
logger.info(
@@ -143,7 +135,7 @@ def processed_actions(self) -> torch.Tensor:
@property
def jacobian_w(self) -> torch.Tensor:
- return wp.to_torch(self._asset.root_view.get_jacobians())[:, self._jacobi_body_idx, :, self._jacobi_joint_ids]
+ return self._asset.data.body_link_jacobian_w.torch[:, self._jacobi_body_idx, :, self._jacobi_joint_ids]
@property
def jacobian_b(self) -> torch.Tensor:
@@ -300,15 +292,8 @@ def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: Ma
# save only the first ee body index
self._ee_body_idx = body_ids[0]
self._ee_body_name = body_names[0]
- # check if articulation is fixed-base
- # if fixed-base then the jacobian for the base is not computed
- # this means that number of bodies is one less than the articulation's number of bodies
- if self._asset.is_fixed_base:
- self._jacobi_ee_body_idx = self._ee_body_idx - 1
- self._jacobi_joint_idx = self._joint_ids
- else:
- self._jacobi_ee_body_idx = self._ee_body_idx
- self._jacobi_joint_idx = [i + 6 for i in self._joint_ids]
+ self._jacobi_ee_body_idx = self._ee_body_idx - 1 if self._asset.is_fixed_base else self._ee_body_idx
+ self._jacobi_joint_idx = [j + self._asset.num_base_dofs for j in self._joint_ids]
# log info for debugging
logger.info(
@@ -379,6 +364,15 @@ def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: Ma
self._mass_matrix = torch.zeros(self.num_envs, self._num_DoF, self._num_DoF, device=self.device)
self._gravity = torch.zeros(self.num_envs, self._num_DoF, device=self.device)
+ # Cache the per-step fetch decisions: cfg is immutable after init, so
+ # mass-matrix and gravity-comp needs are constant across steps.
+ # Mass matrix is consumed by inertial-decoupling and (when nullspace
+ # control is enabled) the null-space torque term in OSC.compute().
+ self._needs_mass_matrix = self.cfg.controller_cfg.inertial_dynamics_decoupling or (
+ self.cfg.controller_cfg.nullspace_control != "none"
+ )
+ self._needs_gravity = self.cfg.controller_cfg.gravity_compensation
+
# create tensors for the ee states
self._ee_pose_w = torch.zeros(self.num_envs, 7, device=self.device)
self._ee_pose_b = torch.zeros(self.num_envs, 7, device=self.device)
@@ -435,9 +429,7 @@ def processed_actions(self) -> torch.Tensor:
@property
def jacobian_w(self) -> torch.Tensor:
- return wp.to_torch(self._asset.root_view.get_jacobians())[
- :, self._jacobi_ee_body_idx, :, self._jacobi_joint_idx
- ]
+ return self._asset.data.body_link_jacobian_w.torch[:, self._jacobi_ee_body_idx, :, self._jacobi_joint_idx]
@property
def jacobian_b(self) -> torch.Tensor:
@@ -527,14 +519,18 @@ def apply_actions(self):
self._compute_ee_velocity()
self._compute_ee_force()
self._compute_joint_states()
- # Calculate the joint efforts
+ # Calculate the joint efforts. Pass ``None`` for mass matrix / gravity
+ # when the controller cfg doesn't require them, instead of forwarding
+ # the (stale-zero) buffers — the controller's own ``None`` checks then
+ # raise immediately on any misconfiguration rather than silently
+ # operating on zeros.
self._joint_efforts[:] = self._osc.compute(
jacobian_b=self._jacobian_b,
current_ee_pose_b=self._ee_pose_b,
current_ee_vel_b=self._ee_vel_b,
current_ee_force_b=self._ee_force_b,
- mass_matrix=self._mass_matrix,
- gravity=self._gravity,
+ mass_matrix=self._mass_matrix if self._needs_mass_matrix else None,
+ gravity=self._gravity if self._needs_gravity else None,
current_joint_pos=self._joint_pos,
current_joint_vel=self._joint_vel,
nullspace_joint_pos_target=self._nullspace_joint_pos_target,
@@ -648,18 +644,27 @@ def _resolve_nullspace_joint_pos_targets(self):
def _compute_dynamic_quantities(self):
"""Computes the dynamic quantities for operational space control.
- Note: For floating-base robots, PhysX prepends 6 virtual DOFs (base position and orientation)
- to the generalized mass matrix and gravity compensation forces. We use ``self._jacobi_joint_idx``
- (which applies the +6 offset for floating-base robots) instead of ``self._joint_ids`` to correctly
- index into these quantities. For fixed-base robots, the two are identical.
+ Mass matrix and gravity-compensation forces are only fetched when the
+ controller actually consumes them — gated by
+ :attr:`~isaaclab.controllers.OperationalSpaceControllerCfg.inertial_dynamics_decoupling`
+ / :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).
+
+ Note: For floating-base robots the Jacobian / mass-matrix / gravity-compensation
+ DoF axis prepends 6 floating-base columns. We use ``self._jacobi_joint_idx``
+ (which applies the ``+ num_base_dofs`` shift) instead of ``self._joint_ids`` to
+ correctly index into these quantities. For fixed-base robots the two are identical.
"""
-
- self._mass_matrix[:] = wp.to_torch(self._asset.root_view.get_generalized_mass_matrices())[
- :, self._jacobi_joint_idx, :
- ][:, :, self._jacobi_joint_idx]
- self._gravity[:] = wp.to_torch(self._asset.root_view.get_gravity_compensation_forces())[
- :, self._jacobi_joint_idx
- ]
+ if self._needs_mass_matrix:
+ self._mass_matrix[:] = self._asset.data.mass_matrix.torch[:, self._jacobi_joint_idx, :][
+ :, :, self._jacobi_joint_idx
+ ]
+ if self._needs_gravity:
+ self._gravity[:] = self._asset.data.gravity_compensation_forces.torch[:, self._jacobi_joint_idx]
def _compute_ee_jacobian(self):
"""Computes the geometric Jacobian of the ee body frame in root frame.
diff --git a/source/isaaclab/test/assets/test_articulation_iface.py b/source/isaaclab/test/assets/test_articulation_iface.py
index 3e471026a9dc..31a401396e2c 100644
--- a/source/isaaclab/test/assets/test_articulation_iface.py
+++ b/source/isaaclab/test/assets/test_articulation_iface.py
@@ -329,6 +329,14 @@ def create_newton_articulation(
# Mock NewtonManager (aliased as SimulationManager in Newton modules)
mock_model = MagicMock()
mock_model.gravity = wp.array(np.array([[0.0, 0.0, -9.81]], dtype=np.float32), dtype=wp.vec3f, device=device)
+ # Sizes consumed by the task-space scratch buffers in NewtonArticulationData.__init__.
+ # Model-wide counts equal the per-articulation counts here because the mock contains a
+ # single homogeneous world.
+ mock_model.articulation_count = num_instances
+ mock_model.max_joints_per_articulation = num_bodies
+ mock_model.max_dofs_per_articulation = num_joints
+ mock_model.joint_dof_count = num_instances * num_joints
+ mock_model.body_count = num_instances * num_bodies
mock_state = MagicMock()
mock_control = MagicMock()
diff --git a/source/isaaclab/test/controllers/test_differential_ik.py b/source/isaaclab/test/controllers/test_differential_ik.py
index 47abc78e0de6..2ba7af0ec028 100644
--- a/source/isaaclab/test/controllers/test_differential_ik.py
+++ b/source/isaaclab/test/controllers/test_differential_ik.py
@@ -14,7 +14,6 @@
import pytest
import torch
-import warp as wp
import isaaclab.sim as sim_utils
from isaaclab import cloner
@@ -199,7 +198,7 @@ def _run_ik_controller(
# at reset, the jacobians are not updated to the latest state
# so we MUST skip the first step
# obtain quantities from simulation
- jacobian = wp.to_torch(robot.root_view.get_jacobians())[:, ee_jacobi_idx, :, arm_joint_ids]
+ jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, arm_joint_ids]
ee_pose_w = robot.data.body_pose_w.torch[:, ee_frame_idx]
root_pose_w = robot.data.root_pose_w.torch
base_rot = root_pose_w[:, 3:7]
diff --git a/source/isaaclab/test/controllers/test_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py
index badf51543e17..80f1f8952623 100644
--- a/source/isaaclab/test/controllers/test_operational_space.py
+++ b/source/isaaclab/test/controllers/test_operational_space.py
@@ -14,7 +14,6 @@
import pytest
import torch
-import warp as wp
from flaky import flaky
import isaaclab.envs.mdp as mdp
@@ -1299,8 +1298,11 @@ class _FloatingBaseOscActionsCfg:
controller_cfg=OperationalSpaceControllerCfg(
target_types=["pose_abs"],
impedance_mode="fixed",
+ # Both flags enabled so the action term fetches mass matrix AND
+ # gravity each step, exercising the floating-base +6 indexing on
+ # both quantities.
inertial_dynamics_decoupling=True,
- gravity_compensation=False,
+ gravity_compensation=True,
motion_stiffness_task=500.0,
motion_damping_ratio_task=1.0,
),
@@ -1330,14 +1332,16 @@ def test_floating_base_osc_action_term_indexing():
"""Regression test for #4999 / PR #5107: verify OperationalSpaceControllerAction uses correct
indices for mass matrix and gravity on floating-base robots.
- For floating-base robots, PhysX prepends 6 virtual DOFs to the generalized mass matrix and
- gravity vectors. The action term's ``_compute_dynamic_quantities()`` must use
- ``_jacobi_joint_idx`` (with +6 offset) instead of ``_joint_ids``. This test instantiates the
- real action term via a ManagerBasedEnv, triggers ``_compute_dynamic_quantities()``, and verifies
- the extracted mass matrix and gravity match a manual extraction using the correct PhysX indices.
+ The Jacobian / mass-matrix / gravity-comp DoF axis prepends ``num_base_dofs``
+ floating-base columns (``6`` for floating-base, ``0`` for fixed-base). The action
+ term's ``_compute_dynamic_quantities()`` must use ``_jacobi_joint_idx`` (with the
+ ``+ num_base_dofs`` shift) instead of ``_joint_ids``. This test instantiates the
+ real action term via a ManagerBasedEnv, triggers ``_compute_dynamic_quantities()``,
+ and verifies the extracted mass matrix and gravity match a manual extraction using
+ the correct indices.
- If someone reverts ``_jacobi_joint_idx`` back to ``_joint_ids`` in ``_compute_dynamic_quantities``,
- this test will fail.
+ If someone reverts ``_jacobi_joint_idx`` back to ``_joint_ids`` in
+ ``_compute_dynamic_quantities``, this test will fail.
"""
env_cfg = _FloatingBaseOscEnvCfg()
env_cfg.sim.device = "cuda:0"
@@ -1365,8 +1369,8 @@ def test_floating_base_osc_action_term_indexing():
# --- 5. Manually extract using the CORRECT indices (what the fix does) ---
jacobi_joint_idx = action_term._jacobi_joint_idx
- full_mass_matrix = wp.to_torch(robot.root_view.get_generalized_mass_matrices())
- full_gravity = wp.to_torch(robot.root_view.get_gravity_compensation_forces())
+ full_mass_matrix = robot.data.mass_matrix.torch
+ full_gravity = robot.data.gravity_compensation_forces.torch
manual_mass = full_mass_matrix[:, jacobi_joint_idx, :][:, :, jacobi_joint_idx]
manual_gravity = full_gravity[:, jacobi_joint_idx]
@@ -1375,10 +1379,10 @@ def test_floating_base_osc_action_term_indexing():
torch.testing.assert_close(term_mass, manual_mass, atol=1e-5, rtol=0)
torch.testing.assert_close(term_gravity, manual_gravity, atol=1e-5, rtol=0)
- # --- 7. Verify the full PhysX tensor has +6 virtual DOFs ---
- expected_physx_dofs = robot.num_joints + 6
- assert full_mass_matrix.shape[1] == expected_physx_dofs, (
- f"Mass matrix should have {expected_physx_dofs} DOFs, got {full_mass_matrix.shape[1]}"
+ # --- 7. Verify the data-layer tensor exposes the full DoF axis (J + num_base_dofs) ---
+ expected_dofs = robot.num_joints + robot.num_base_dofs
+ assert full_mass_matrix.shape[1] == expected_dofs, (
+ f"Mass matrix should have {expected_dofs} DoFs, got {full_mass_matrix.shape[1]}"
)
# --- 8. Verify correct indices differ from raw joint_ids (the old bug) ---
@@ -1386,7 +1390,7 @@ def test_floating_base_osc_action_term_indexing():
original_joint_ids, _ = robot.find_joints(_G1_ARM_JOINT_NAMES)
buggy_mass = full_mass_matrix[:, original_joint_ids, :][:, :, original_joint_ids]
assert not torch.allclose(term_mass, buggy_mass, atol=1e-6), (
- "Action term mass matrix should NOT match extraction with raw joint_ids (no +6 offset)"
+ "Action term mass matrix should NOT match extraction with raw joint_ids (no num_base_dofs offset)"
)
# --- 9. Verify physically reasonable values ---
@@ -1591,9 +1595,9 @@ def _update_states(
"""
# obtain dynamics related quantities from simulation
ee_jacobi_idx = ee_frame_idx - 1
- jacobian_w = wp.to_torch(robot.root_view.get_jacobians())[:, ee_jacobi_idx, :, arm_joint_ids]
- mass_matrix = wp.to_torch(robot.root_view.get_generalized_mass_matrices())[:, arm_joint_ids, :][:, :, arm_joint_ids]
- gravity = wp.to_torch(robot.root_view.get_gravity_compensation_forces())[:, arm_joint_ids]
+ jacobian_w = robot.data.body_link_jacobian_w.torch[:, 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]
# Convert the Jacobian from world to root frame
jacobian_b = jacobian_w.clone()
root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w.torch))
diff --git a/source/isaaclab_newton/changelog.d/jichuanh-ik-newton-compat-mvp.minor.rst b/source/isaaclab_newton/changelog.d/jichuanh-ik-newton-compat-mvp.minor.rst
new file mode 100644
index 000000000000..aea1e28e52b9
--- /dev/null
+++ b/source/isaaclab_newton/changelog.d/jichuanh-ik-newton-compat-mvp.minor.rst
@@ -0,0 +1,40 @@
+Added
+^^^^^
+
+* Added Newton implementations of
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w`,
+ :attr:`~isaaclab.assets.BaseArticulationData.body_com_jacobian_w`, and
+ :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix` on
+ :class:`~isaaclab_newton.assets.ArticulationData`. The properties wrap
+ ``ArticulationView.eval_jacobian`` and ``ArticulationView.eval_mass_matrix``
+ with view-sized output buffers cached via the standard timestamped-buffer
+ pattern. Per-step behavior is allocation-free and safe under CUDA-graph
+ capture: source / scratch / output buffers are pre-allocated in
+ ``_create_buffers``, and
+ :func:`~isaaclab_newton.assets.articulation.kernels.gather_jacobian_rows`
+ and :func:`~isaaclab_newton.assets.articulation.kernels.gather_mass_matrix_rows`
+ Warp kernels gather just this view's rows from the model-sized buffers
+ Newton populates. The DoF axis preserves the leading 6 floating-base
+ columns Newton fills for floating-base articulations (matching the
+ cross-library industry convention and PhysX's layout).
+* Added the
+ :func:`~isaaclab_newton.assets.articulation.kernels.shift_jacobian_com_to_origin`
+ Warp kernel applying the
+ ``v_origin = v_com - omega x (R · body_com_pos_b)`` shift to the
+ linear-velocity rows of the gathered, view-sized Jacobian, so the link-
+ origin form matches the cross-backend
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w`
+ contract.
+
+Changed
+^^^^^^^
+
+* :attr:`~isaaclab_newton.assets.ArticulationData.gravity_compensation_forces`
+ raises :class:`NotImplementedError` with a message pointing at the
+ upstream gap. Newton's ``ArticulationView`` does not expose an
+ inverse-dynamics primitive yet (upstream Newton issues
+ `#2497 `_,
+ `#2529 `_,
+ `#2625 `_).
+ OSC users on Newton must set ``gravity_compensation=False`` until
+ upstream lands the primitive.
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 2da95b49b21d..0a2a84392619 100644
--- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py
+++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py
@@ -127,6 +127,19 @@ def update(self, dt: float) -> None:
self.joint_acc
self.body_com_acc_w
+ def _ensure_fk_fresh(self) -> None:
+ """Run forward kinematics if joint state has changed since the last FK update.
+
+ Newton's ``state.body_q`` (per-body world transforms) is updated by ``eval_fk``,
+ invoked here through ``SimulationManager.forward()``. After a manual joint or root
+ write that bypassed the sim step (``write_*_to_sim_*``), ``_fk_timestamp`` is set
+ to ``-1.0`` to force a refresh on the next read of any property that depends on
+ body poses (``body_link_pose_w``, the Jacobian properties, ``mass_matrix``).
+ """
+ if self._fk_timestamp < self._sim_timestamp:
+ SimulationManager.forward()
+ self._fk_timestamp = self._sim_timestamp
+
"""
Names.
"""
@@ -667,9 +680,7 @@ def body_link_pose_w(self) -> ProxyArray:
This quantity is the pose of the articulation links' actor frame relative to the world.
The orientation is provided in (x, y, z, w) format.
"""
- if self._fk_timestamp < self._sim_timestamp:
- SimulationManager.forward()
- self._fk_timestamp = self._sim_timestamp
+ self._ensure_fk_fresh()
return self._body_link_pose_w_ta
@property
@@ -812,6 +823,130 @@ def body_com_pose_b(self) -> ProxyArray:
self._body_com_pose_b.timestamp = self._sim_timestamp
return self._body_com_pose_b_ta
+ """
+ Dynamics quantities (task-space controllers).
+ """
+
+ @property
+ def body_com_jacobian_w(self) -> ProxyArray:
+ """See :attr:`isaaclab.assets.BaseArticulationData.body_com_jacobian_w`.
+
+ Newton implementation: ``eval_jacobian`` (writes the model-wide buffer) then a
+ gather kernel extracts this view's rows. ``link_offset`` drops Newton's fixed-
+ root row for fixed-base; the DoF axis is preserved in full.
+ """
+ # Newton's eval_jacobian reads ``state.body_q`` (link poses); refresh FK if stale.
+ # Matches the convention in ``body_link_pose_w`` — Python-guarded lazy refresh.
+ self._ensure_fk_fresh()
+ # eval_jacobian writes every articulation in the model; gather kernel extracts this
+ # view's rows. ``link_offset`` skips Newton's fixed-root row for fixed-base; the DoF
+ # axis is preserved in full (free-root joint's 6 columns up front for floating-base),
+ # matching the PhysX layout and the cross-library industry convention.
+ self._root_view.eval_jacobian(
+ SimulationManager.get_state_0(),
+ J=self._jacobian_buf_flat,
+ joint_S_s=self._joint_S_s_buf,
+ )
+ wp.launch(
+ articulation_kernels.gather_jacobian_rows,
+ dim=self._body_com_jacobian_w_buf.shape,
+ inputs=[
+ self._jacobian_buf,
+ self._jacobian_view_art_ids,
+ self._jacobian_link_offset,
+ ],
+ outputs=[self._body_com_jacobian_w_buf],
+ device=self.device,
+ )
+ return self._body_com_jacobian_w_ta
+
+ @property
+ def body_link_jacobian_w(self) -> ProxyArray:
+ """See :attr:`isaaclab.assets.BaseArticulationData.body_link_jacobian_w`.
+
+ Newton implementation: applies the COM→origin shift kernel to
+ :attr:`body_com_jacobian_w` (Newton's ``eval_jacobian`` is COM-referenced).
+ """
+ # ``body_link_pose_w`` accessor triggers ``SimulationManager.forward()`` if FK is
+ # stale (after a manual joint / root write that bypassed the sim step). Reading the
+ # property here — not ``_sim_bind_body_link_pose_w`` directly — keeps the shift
+ # kernel from using stale link rotations during reset / IK-warm-start paths.
+ link_pose_w = self.body_link_pose_w.warp
+ com_jac = self.body_com_jacobian_w
+ wp.launch(
+ articulation_kernels.shift_jacobian_com_to_origin,
+ dim=self._body_link_jacobian_w_buf.shape[:2] + (self._body_link_jacobian_w_buf.shape[3],),
+ inputs=[
+ link_pose_w,
+ self._sim_bind_body_com_pos_b,
+ self._jacobian_link_offset,
+ com_jac.warp,
+ ],
+ outputs=[self._body_link_jacobian_w_buf],
+ device=self.device,
+ )
+ return self._body_link_jacobian_w_ta
+
+ @property
+ def mass_matrix(self) -> ProxyArray:
+ """See :attr:`isaaclab.assets.BaseArticulationData.mass_matrix`.
+
+ Newton implementation: ``eval_mass_matrix`` (writes the model-wide buffer) then a
+ gather kernel extracts this view's rows.
+ """
+ # eval_jacobian / eval_mass_matrix read ``state.body_q``; refresh FK if stale.
+ # Matches the convention in ``body_link_pose_w`` — Python-guarded lazy refresh.
+ self._ensure_fk_fresh()
+ # eval_mass_matrix treats ``J`` as an input (skips its own jacobian compute when
+ # provided), so we must populate the scratch first via eval_jacobian. Reusing
+ # ``_jacobian_buf_flat`` (same shape) avoids a second allocation. All scratch buffers
+ # are pre-allocated for CUDA-graph capture safety.
+ state = SimulationManager.get_state_0()
+ self._root_view.eval_jacobian(
+ state,
+ J=self._jacobian_buf_flat,
+ joint_S_s=self._joint_S_s_buf,
+ )
+ self._root_view.eval_mass_matrix(
+ state,
+ H=self._mass_matrix_full_buf,
+ J=self._jacobian_buf_flat,
+ body_I_s=self._mass_matrix_body_I_s_buf,
+ joint_S_s=self._joint_S_s_buf,
+ )
+ wp.launch(
+ articulation_kernels.gather_mass_matrix_rows,
+ dim=self._mass_matrix_buf.shape,
+ inputs=[
+ self._mass_matrix_full_buf,
+ self._jacobian_view_art_ids,
+ ],
+ outputs=[self._mass_matrix_buf],
+ device=self.device,
+ )
+ return self._mass_matrix_ta
+
+ @property
+ 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."
+ )
+
"""
Joint state properties.
"""
@@ -1301,18 +1436,21 @@ def _create_simulation_bindings(self) -> None:
# -- root properties
self._sim_bind_root_link_pose_w = self._root_view.get_root_transforms(SimulationManager.get_state_0())[:, 0]
- self._sim_bind_root_com_vel_w = self._root_view.get_root_velocities(SimulationManager.get_state_0())
- if self._sim_bind_root_com_vel_w is not None:
+ # ``get_root_velocities`` returns ``None`` for fixed-base articulations; the
+ # ``wp.zeros`` fallback set by :meth:`_create_buffers` must survive subsequent
+ # resets, so only overwrite when the solver actually exposes the binding.
+ root_vel_w = self._root_view.get_root_velocities(SimulationManager.get_state_0())
+ if root_vel_w is not None:
if self._root_view.is_fixed_base:
- self._sim_bind_root_com_vel_w = self._sim_bind_root_com_vel_w[:, 0, 0]
+ self._sim_bind_root_com_vel_w = root_vel_w[:, 0, 0]
else:
- self._sim_bind_root_com_vel_w = self._sim_bind_root_com_vel_w[:, 0]
+ self._sim_bind_root_com_vel_w = root_vel_w[:, 0]
# -- body properties
self._sim_bind_body_com_pos_b = self._root_view.get_attribute("body_com", SimulationManager.get_model())[:, 0]
self._sim_bind_body_link_pose_w = self._root_view.get_link_transforms(SimulationManager.get_state_0())[:, 0]
- self._sim_bind_body_com_vel_w = self._root_view.get_link_velocities(SimulationManager.get_state_0())
- if self._sim_bind_body_com_vel_w is not None:
- self._sim_bind_body_com_vel_w = self._sim_bind_body_com_vel_w[:, 0]
+ body_com_vel_w = self._root_view.get_link_velocities(SimulationManager.get_state_0())
+ if body_com_vel_w is not None:
+ self._sim_bind_body_com_vel_w = body_com_vel_w[:, 0]
self._sim_bind_body_mass = self._root_view.get_attribute("body_mass", SimulationManager.get_model())[:, 0]
# Newton stores body_inertia as (N, 1, B) mat33f — the [:, 0] removes the padding dim
# giving (N, B) mat33f. Reinterpret as (N, B, 9) float32 via pointer aliasing.
@@ -1516,6 +1654,8 @@ def _create_buffers(self) -> None:
self._joint_acc = TimestampedBuffer(
shape=(self._num_instances, self._num_joints), dtype=wp.float32, device=self.device
)
+ # -- dynamics quantities for task-space controllers
+ self._create_jacobian_buffers(SimulationManager.get_model())
# Empty memory pre-allocations
self._root_link_lin_vel_b = None
self._root_link_ang_vel_b = None
@@ -1552,6 +1692,74 @@ def _create_buffers(self) -> None:
# Pin all ProxyArray wrappers to current buffers.
self._pin_proxy_arrays()
+ 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
+ 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
+ columns up front (0 for fixed-base, 6 for floating-base), matching the cross-
+ library industry convention (PhysX, Pinocchio, Drake, MuJoCo, RBDL, OCS2, iDynTree).
+
+ Args:
+ model: Newton ``Model`` from :meth:`SimulationManager.get_model`. Read for
+ ``articulation_count``, ``max_joints_per_articulation``,
+ ``max_dofs_per_articulation``, ``joint_dof_count``, ``body_count``.
+ """
+ max_links = model.max_joints_per_articulation
+ max_dofs = model.max_dofs_per_articulation
+
+ # -- shared scratch (eval_jacobian outputs; consumed by ``body_com_jacobian_w``
+ # and reused as ``eval_mass_matrix``'s ``J`` input to skip a re-compute)
+ self._jacobian_buf_flat = wp.zeros(
+ (model.articulation_count, max_links * 6, max_dofs), dtype=wp.float32, device=self.device
+ )
+ # Motion subspace (Featherstone ``S``, spatial frame); produced by eval_jacobian,
+ # also consumed by eval_mass_matrix.
+ self._joint_S_s_buf = wp.zeros(model.joint_dof_count, dtype=wp.spatial_vector, device=self.device)
+
+ # -- per-view gather config (shared by every gather/shift kernel below)
+ # Link-row offset: fixed-base skips Newton's row-0 fixed-root row; floating-base keeps it.
+ self._jacobian_link_offset = 1 if self._root_view.is_fixed_base else 0
+ num_jacobi_bodies = self._num_bodies - self._jacobian_link_offset
+ # Free-root DoF columns Newton fills for floating-base (0 fixed-base, 6 floating-base);
+ # included in the DoF axis to match the cross-library industry convention.
+ num_base_dofs = 0 if self._root_view.is_fixed_base else 6
+ # Flattened (num_worlds*num_per_view,) view-to-model index map for the gather kernels.
+ self._jacobian_view_art_ids = self._root_view.articulation_ids.reshape((-1,))
+
+ # -- ``body_com_jacobian_w``: 4-D reshape view of the shared scratch (kernel input
+ # to the gather) and the per-view output buffer (gather output)
+ self._jacobian_buf = self._jacobian_buf_flat.reshape((model.articulation_count, max_links, 6, max_dofs))
+ self._body_com_jacobian_w_buf = wp.zeros(
+ (self._num_instances, num_jacobi_bodies, 6, self._num_joints + num_base_dofs),
+ dtype=wp.float32,
+ device=self.device,
+ )
+
+ # -- ``body_link_jacobian_w``: output of the COM→origin shift kernel applied to
+ # the COM-referenced Jacobian above; same shape, link-origin reference
+ self._body_link_jacobian_w_buf = wp.zeros(
+ (self._num_instances, num_jacobi_bodies, 6, self._num_joints + num_base_dofs),
+ dtype=wp.float32,
+ device=self.device,
+ )
+
+ # -- ``mass_matrix``: model-wide ``H`` scratch (eval_mass_matrix output), per-body
+ # spatial-inertia aux (Featherstone ``I``), and per-view output (gather output)
+ self._mass_matrix_full_buf = wp.zeros(
+ (model.articulation_count, max_dofs, max_dofs), dtype=wp.float32, device=self.device
+ )
+ self._mass_matrix_body_I_s_buf = wp.zeros(model.body_count, dtype=wp.spatial_matrix, device=self.device)
+ self._mass_matrix_buf = wp.zeros(
+ (self._num_instances, self._num_joints + num_base_dofs, 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.
@@ -1625,6 +1833,9 @@ def _pin_proxy_arrays(self) -> None:
self._projected_gravity_b_ta = ProxyArray(self._projected_gravity_b.data)
self._heading_w_ta = ProxyArray(self._heading_w.data)
self._joint_acc_ta = ProxyArray(self._joint_acc.data)
+ 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)
# -- 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 5e66b867c09a..a928bd524761 100644
--- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py
+++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py
@@ -618,3 +618,142 @@ def concat_joint_pos_limits_lower_and_upper(
"""
i, j = wp.tid()
joint_pos_limits[i, j] = wp.vec2f(joint_pos_limits_lower[i, j], joint_pos_limits_upper[i, j])
+
+
+@wp.kernel
+def gather_jacobian_rows(
+ src: wp.array4d(dtype=wp.float32),
+ art_ids: wp.array(dtype=wp.int32),
+ link_offset: wp.int32,
+ dst: wp.array4d(dtype=wp.float32),
+):
+ """Copy per-view articulation jacobian rows from a model-sized buffer into a view-sized buffer.
+
+ Newton's ``eval_jacobian`` writes every articulation in the model (across all
+ :class:`~newton.selection.ArticulationView` instances) into a single 4-D output
+ shaped ``(model.articulation_count, max_links, 6, max_dofs)``. An
+ ``ArticulationView`` owns only the subset indexed by ``articulation_ids``. This
+ kernel gathers those rows into a contiguous view-sized destination so the
+ caller-facing
+ :attr:`~isaaclab.assets.BaseArticulationData.body_com_jacobian_w` contract
+ ``(num_instances, num_jacobi_bodies, 6, num_joints + num_base_dofs)`` is
+ preserved.
+
+ For fixed-base articulations Newton fills link row 0 with the fixed root joint
+ (zero motion subspace), so we skip it with ``link_offset = 1``. For floating-
+ base, ``link_offset = 0`` and the full DoF axis is preserved including the 6
+ leading free-root joint columns, 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 jacobian buffer reshaped to 4-D. Shape is
+ (model.articulation_count, max_links, 6, max_dofs).
+ art_ids: Model-level articulation indices owned by this view. Shape is
+ (num_instances,).
+ link_offset: Constant offset added to the destination link index when
+ reading from ``src``. ``1`` for fixed-base views, ``0`` for
+ floating-base.
+ dst: Output jacobian buffer for this view. Shape is
+ (num_instances, num_jacobi_bodies, 6, num_joints + num_base_dofs),
+ where ``num_jacobi_bodies = this asset's num_bodies - link_offset``
+ (per-asset count, not the model-wide ``max_links``).
+ """
+ i, link, s, d = wp.tid()
+ dst[i, link, s, d] = src[art_ids[i], link + link_offset, s, d]
+
+
+@wp.kernel
+def gather_mass_matrix_rows(
+ src: wp.array3d(dtype=wp.float32),
+ art_ids: wp.array(dtype=wp.int32),
+ dst: wp.array3d(dtype=wp.float32),
+):
+ """Copy per-view articulation mass-matrix rows from a model-sized buffer into a view-sized buffer.
+
+ 3-D analogue of :func:`gather_jacobian_rows` for the joint-space mass
+ matrix written by :func:`newton.sim.articulation.eval_mass_matrix`. The
+ DoF axis is preserved in full (including the leading 6 free-root rows/cols
+ for floating-base articulations), matching the cross-library industry
+ convention used by PhysX, Pinocchio, Drake, MuJoCo, RBDL, OCS2, and iDynTree.
+
+ Args:
+ src: Input mass-matrix buffer. Shape is
+ (model.articulation_count, max_dofs, max_dofs).
+ art_ids: Model-level articulation indices owned by this view. Shape is
+ (num_instances,).
+ dst: Output mass-matrix buffer for this view. Shape is
+ (num_instances, num_joints + num_base_dofs,
+ num_joints + num_base_dofs).
+ """
+ i, r, c = wp.tid()
+ dst[i, r, c] = src[art_ids[i], r, c]
+
+
+@wp.kernel
+def shift_jacobian_com_to_origin(
+ body_link_pose: wp.array2d(dtype=wp.transformf),
+ body_com_pos_b: wp.array2d(dtype=wp.vec3f),
+ link_offset: wp.int32,
+ src: wp.array4d(dtype=wp.float32),
+ dst: wp.array4d(dtype=wp.float32),
+):
+ """Shift the linear-velocity rows of the Jacobian from COM to link origin.
+
+ Newton's ``eval_jacobian`` returns ``J · q_dot = [v_com_world, omega_world]``
+ per link — the linear rows are the velocity of the link's center of mass,
+ expressed in world frame. The
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` contract
+ requires the linear rows to be the velocity at the link **origin**
+ (USD prim transform) so that ``J · q_dot[body_idx]`` matches
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_lin_vel_w` /
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_ang_vel_w`.
+
+ The shift is the same one applied per-body by
+ :func:`get_link_vel_from_root_com_vel_func`, but layered onto every
+ Jacobian column: each column represents the spatial velocity contribution
+ of one DoF, and shifting a spatial velocity from COM to link origin uses
+ the same ``v_origin = v_com - omega x (R · body_com_pos_b)`` identity.
+
+ Notes on layout:
+ * Jacobian rows ``[0:3]`` are linear velocity, ``[3:6]`` are angular.
+ * ``body_link_pose`` and ``body_com_pos_b`` are indexed by the
+ articulation's full body count, so ``link_offset`` must be applied
+ to map a row in the (already-gathered) ``src`` to its body index in
+ the asset data. ``link_offset = 1`` for fixed-base (Newton's row 0
+ fixed-root row was dropped during the prior gather);
+ ``link_offset = 0`` for floating-base.
+
+ Args:
+ body_link_pose: Per-body link pose in world frame. Shape is
+ (num_instances, num_bodies).
+ body_com_pos_b: Per-body center-of-mass offset expressed in the body's
+ link frame. Shape is (num_instances, num_bodies).
+ link_offset: Offset added to the jacobian-row body index to reach the
+ full body index. ``1`` for fixed-base, ``0`` for floating-base.
+ src: COM-referenced Jacobian (read-only). Shape is
+ (num_instances, num_jacobi_bodies, 6, num_joints + num_base_dofs).
+ dst: Output buffer for the link-origin Jacobian. Same shape as
+ ``src``. Linear rows ``[0:3]`` are written with the shifted
+ velocity; angular rows ``[3:6]`` are copied unchanged (angular
+ velocity is reference-point invariant).
+ """
+ n, b, dof = wp.tid()
+ full_body_idx = b + link_offset
+
+ R = wp.transform_get_rotation(body_link_pose[n, full_body_idx])
+ c_world = wp.quat_rotate(R, body_com_pos_b[n, full_body_idx])
+
+ v_com = wp.vec3(src[n, b, 0, dof], src[n, b, 1, dof], src[n, b, 2, dof])
+ omega = wp.vec3(src[n, b, 3, dof], src[n, b, 4, dof], src[n, b, 5, dof])
+
+ v_origin = v_com - wp.cross(omega, c_world)
+
+ dst[n, b, 0, dof] = v_origin[0]
+ dst[n, b, 1, dof] = v_origin[1]
+ dst[n, b, 2, dof] = v_origin[2]
+ dst[n, b, 3, dof] = omega[0]
+ dst[n, b, 4, dof] = omega[1]
+ dst[n, b, 5, dof] = omega[2]
diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py
index cd0f2dcb03b9..5202da63418b 100644
--- a/source/isaaclab_newton/test/assets/test_articulation.py
+++ b/source/isaaclab_newton/test/assets/test_articulation.py
@@ -18,6 +18,7 @@
"""Rest everything follows."""
import sys
+from copy import deepcopy
import pytest
import torch
@@ -32,16 +33,23 @@
import isaaclab.utils.string as string_utils
from isaaclab.actuators import ActuatorBase, IdealPDActuatorCfg, ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg
+from isaaclab.controllers import (
+ DifferentialIKController,
+ DifferentialIKControllerCfg,
+ OperationalSpaceController,
+ OperationalSpaceControllerCfg,
+)
from isaaclab.envs.mdp.terminations import joint_effort_out_of_limit
from isaaclab.managers import SceneEntityCfg
from isaaclab.sim import SimulationCfg, build_simulation_context
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
+from isaaclab.utils.math import compute_pose_error, matrix_from_quat, quat_inv, subtract_frame_transforms
from isaaclab.utils.version import get_isaac_sim_version, has_kit
##
# Pre-defined configs
##
-from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_CFG # isort:skip
+from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_CFG, FRANKA_PANDA_HIGH_PD_CFG # isort:skip
# , SHADOW_HAND_CFG # isort:skip
SIM_CFGs = {
@@ -353,6 +361,98 @@ def generate_articulation(
return articulation, translations
+# ---------------------------------------------------------------------------
+# Franka task-space tracking helpers (shared between IK and OSC tests).
+# ---------------------------------------------------------------------------
+
+
+def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False):
+ """Build a Franka articulation at its configured home pose.
+
+ Constructs :data:`FRANKA_PANDA_HIGH_PD_CFG`, optionally zeroes the
+ arm-actuator PD gains, resets the simulator, and teleports the
+ arm joints to :attr:`default_joint_pos` (the env reset path that
+ normally does this is not invoked for standalone tests, so the
+ robot would otherwise sit at the URDF-neutral pose where the
+ Franka wrist is near-singular).
+
+ Args:
+ sim: The simulation context to use.
+ zero_actuator_pd: If True, sets the panda_shoulder/panda_forearm
+ 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)``.
+
+ Returns:
+ Tuple of ``(robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids)``.
+ """
+ cfg = FRANKA_PANDA_HIGH_PD_CFG.copy().replace(prim_path="/World/Env_.*/Robot")
+ if zero_actuator_pd:
+ cfg.actuators["panda_shoulder"].stiffness = 0.0
+ cfg.actuators["panda_shoulder"].damping = 0.0
+ cfg.actuators["panda_forearm"].stiffness = 0.0
+ cfg.actuators["panda_forearm"].damping = 0.0
+ sim_utils.create_prim("/World/Env_0", "Xform", translation=(0.0, 0.0, 0.0))
+ robot = Articulation(cfg)
+ sim.reset()
+ assert robot.is_initialized
+
+ ee_frame_idx = robot.find_bodies("panda_hand")[0][0]
+ ee_jacobi_idx = ee_frame_idx - 1
+ arm_joint_ids = robot.find_joints(["panda_joint.*"])[0]
+
+ robot.write_joint_position_to_sim_index(position=robot.data.default_joint_pos.torch[:, :].clone())
+ robot.write_joint_velocity_to_sim_index(velocity=robot.data.default_joint_vel.torch[:, :].clone())
+ return robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids
+
+
+def _compute_ee_pose_root(robot, ee_frame_idx):
+ """Return ``(ee_pos_b, ee_quat_b, root_pose_w)`` in the root frame."""
+ ee_pose_w = robot.data.body_pose_w.torch[:, ee_frame_idx]
+ root_pose_w = robot.data.root_pose_w.torch
+ ee_pos_b, ee_quat_b = subtract_frame_transforms(
+ root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
+ )
+ return ee_pos_b, ee_quat_b, root_pose_w
+
+
+def _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids):
+ """Return the EE Jacobian sliced to ``arm_joint_ids`` and rotated to the root frame."""
+ jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, arm_joint_ids]
+ base_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_pose_w.torch[:, 3:7]))
+ jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :])
+ jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :])
+ return jacobian
+
+
+def _compute_ee_vel_root(jacobian_b, joint_vel):
+ """Return the EE 6D velocity in the root frame as ``J · q_dot``.
+
+ Required to make OSC's ``kd * ee_vel_b`` damping term meaningful.
+ Passing zero EE velocity (the convenient hack) leaves the impedance
+ undamped and the EE oscillates around the target. We use ``J · q_dot``
+ rather than reading ``data.body_vel_w`` because Newton's lazy
+ velocity buffers can return stale/zero values until forced
+ materialization, while ``joint_vel`` and ``J`` are already pulled
+ by the loop. ``J`` correctness is pinned independently by
+ ``test_get_jacobians_link_origin_contract``.
+ """
+ return torch.bmm(jacobian_b, joint_vel.unsqueeze(-1)).squeeze(-1)
+
+
+def _build_relative_pose_target(robot, ee_frame_idx, delta_xyz, device):
+ """Build a target pose = (current EE pose) + ``delta_xyz``, preserving orientation."""
+ initial_ee_pos_b, initial_ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx)
+ target_pos_b = initial_ee_pos_b + torch.tensor([list(delta_xyz)], device=device, dtype=initial_ee_pos_b.dtype)
+ return torch.cat([target_pos_b, initial_ee_quat_b], dim=-1)
+
+
+def _summarize_history(history, tail: int = 200):
+ """Return ``(min, mean)`` over the last ``tail`` samples."""
+ tail_slice = history[-tail:]
+ return min(tail_slice), sum(tail_slice) / len(tail_slice)
+
+
@pytest.fixture
def sim(request):
"""Create simulation context with the specified device."""
@@ -366,8 +466,13 @@ def sim(request):
else:
add_ground_plane = False # default to no ground plane
articulation_type = request.getfixturevalue("articulation_type")
- sim_cfg = SIM_CFGs[articulation_type]
+ sim_cfg = deepcopy(SIM_CFGs[articulation_type])
sim_cfg.device = device
+ # ``gravity_enabled`` is silently ignored by ``build_simulation_context``
+ # when an explicit ``sim_cfg`` is also passed; apply it here so the
+ # fixture honors what its parameter advertises.
+ if not gravity_enabled:
+ sim_cfg.gravity = (0.0, 0.0, 0.0)
with build_simulation_context(
device=device,
auto_add_lighting=True,
@@ -715,7 +820,7 @@ def test_initialization_fixed_base_made_floating_base(
sim: The simulation fixture
num_articulations: Number of articulations to test
"""
- articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type)
+ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).copy()
# Unfix root link by making it non-kinematic
articulation_cfg.spawn.articulation_props.fix_root_link = False
articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device)
@@ -2442,5 +2547,647 @@ 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
+# to model-wide max sizing or to the wrong fixed-base row offset) fail fast.
+##
+
+
+@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_jacobians_shape_fixed_base(sim, num_articulations, device, articulation_type):
+ """Fixed-base ``body_link_jacobian_w`` must drop the fixed-root row.
+
+ Contract: shape ``(N, num_bodies - 1, 6, num_joints)``. Catches
+ regressions of (a) the link_offset fix that drops Newton's row 0 for
+ fixed-base, and (b) the per-articulation output sizing — using
+ model-wide ``max_links`` here would over-allocate 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"
+
+ J = articulation.data.body_link_jacobian_w.torch
+
+ expected_shape = (num_articulations, articulation.num_bodies - 1, 6, articulation.num_joints)
+ assert J.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(J.shape)}"
+ assert J.dtype == torch.float32
+
+
+@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_mass_matrix_shape_and_nonsingular_fixed_base(sim, num_articulations, device, articulation_type):
+ """Fixed-base ``mass_matrix`` shape + non-singularity.
+
+ Contract: shape ``(N, num_joints, num_joints)`` and the matrix must be
+ non-singular. The non-singularity check catches the heterogeneous
+ padding bug — if the wrapper accidentally returns ``model.max_dofs``
+ sized output, the padded zero rows/cols make the matrix rank-deficient.
+ """
+ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type)
+ articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device)
+ sim.reset()
+ assert articulation.is_initialized
+
+ sim.step()
+ articulation.update(sim.cfg.dt)
+
+ M = articulation.data.mass_matrix.torch
+
+ expected_shape = (num_articulations, articulation.num_joints, articulation.num_joints)
+ assert M.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(M.shape)}"
+ assert M.dtype == torch.float32
+
+ # Each diagonal entry is a joint's effective inertia and must be strictly
+ # positive for any physical articulation. Padded zero rows/cols (the
+ # heterogeneous bug) would surface as zero diagonal entries — much more
+ # sensitive than checking the determinant, which can be small purely from
+ # numerical conditioning of a well-formed 9x9 mass matrix (Franka det
+ # is ~1e-13 in practice).
+ diag = M.diagonal(dim1=-2, dim2=-1)
+ 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("add_ground_plane", [True])
+@pytest.mark.parametrize("articulation_type", ["anymal"])
+@pytest.mark.isaacsim_ci
+def test_get_jacobians_shape_floating_base(sim, num_articulations, device, add_ground_plane, articulation_type):
+ """Floating-base ``body_link_jacobian_w`` keeps every body row and prepends 6 base-DoF columns.
+
+ Contract for floating-base: shape
+ ``(N, num_bodies, 6, num_joints + num_base_dofs)`` — no fixed-root row
+ to drop, and the leading 6 DoF columns are the floating-base spatial-
+ velocity columns Newton's ``eval_jacobian`` writes for the free root
+ joint. Matches the cross-library industry convention (Pinocchio, Drake,
+ MuJoCo, RBDL, OCS2, iDynTree).
+ """
+ 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"
+
+ J = articulation.data.body_link_jacobian_w.torch
+
+ expected_shape = (
+ num_articulations,
+ articulation.num_bodies,
+ 6,
+ articulation.num_joints + articulation.num_base_dofs,
+ )
+ assert J.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(J.shape)}"
+ assert J.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_mass_matrix_shape_floating_base(sim, num_articulations, device, add_ground_plane, articulation_type):
+ """Floating-base ``mass_matrix`` shape ``(N, num_joints + 6, num_joints + 6)``.
+
+ Includes the 6 floating-base rows/cols 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
+
+ sim.step()
+ articulation.update(sim.cfg.dt)
+
+ M = articulation.data.mass_matrix.torch
+
+ expected_dofs = articulation.num_joints + articulation.num_base_dofs
+ expected_shape = (num_articulations, expected_dofs, expected_dofs)
+ assert M.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(M.shape)}"
+ assert M.dtype == torch.float32
+
+
+@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_heterogeneous_scene_per_view_shapes(sim, device, add_ground_plane, articulation_type):
+ """Mixed-articulation scene: each view returns ITS OWN asset's shape.
+
+ Direct regression test for the Codex round-2 finding. With Franka
+ (9 DoFs) and Anymal-C (18 DoFs) co-resident in the model,
+ ``model.max_dofs_per_articulation == 18`` and
+ ``model.max_joints_per_articulation == anymal.num_bodies``. The Franka
+ view's ``body_link_jacobian_w`` / ``mass_matrix`` outputs must use
+ Franka's per-asset counts, NOT the model-wide maxima — otherwise
+ Franka's mass matrix would carry zero-padded rows/cols and be
+ singular.
+
+ Uses the ``anymal`` ``SIM_CFGs`` entry (more capable solver settings)
+ for the host sim; the ``articulation_type`` parametrize is only there
+ so the ``sim`` fixture picks a config — the test itself constructs
+ both Anymal and Franka articulations directly.
+ """
+ # ``num_per_type=1`` keeps the actuator-default replication path off —
+ # Newton's USD default loader hits a (1, num_joints) vs (num_envs,
+ # num_joints) shape mismatch with multi-instance multi-type scenes; one
+ # of each is the minimum heterogeneous setup that still exercises the
+ # per-articulation shape gate without that pre-existing quirk.
+ num_per_type = 1
+
+ franka_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Env_franka_.*/Robot")
+ anymal_cfg = ANYMAL_C_CFG.replace(prim_path="/World/Env_anymal_.*/Robot")
+
+ for i in range(num_per_type):
+ sim_utils.create_prim(f"/World/Env_franka_{i}", "Xform", translation=(2.5 * i, 0.0, 0.0))
+ sim_utils.create_prim(f"/World/Env_anymal_{i}", "Xform", translation=(2.5 * i, 5.0, 0.0))
+
+ franka = Articulation(franka_cfg)
+ anymal = Articulation(anymal_cfg)
+ sim.reset()
+ assert franka.is_initialized and anymal.is_initialized
+ assert franka.is_fixed_base and not anymal.is_fixed_base
+
+ # Sanity: the model-wide maxima are larger than at least one view's
+ # per-asset count, so a regression to model-wide sizing would manifest
+ # as wrong shapes here. Assert that precondition explicitly so the test
+ # fails clearly if the fixture stops being heterogeneous.
+ model = SimulationManager.get_model()
+ assert model.max_dofs_per_articulation > min(franka.num_joints, anymal.num_joints), (
+ "scene is no longer heterogeneous; this test relies on model.max_dofs > one view's num_joints"
+ )
+
+ franka_J = franka.data.body_link_jacobian_w.torch
+ anymal_J = anymal.data.body_link_jacobian_w.torch
+
+ # Each view's output uses its OWN per-asset count, not the model-wide max.
+ # Floating-base assets prepend ``num_base_dofs`` floating-base columns; fixed-base
+ # assets have ``num_base_dofs == 0``.
+ franka_dofs = franka.num_joints + franka.num_base_dofs
+ anymal_dofs = anymal.num_joints + anymal.num_base_dofs
+ assert franka_J.shape == torch.Size((num_per_type, franka.num_bodies - 1, 6, franka_dofs)), (
+ f"Franka jacobian leaked model-wide shape: got {tuple(franka_J.shape)}"
+ )
+ assert anymal_J.shape == torch.Size((num_per_type, anymal.num_bodies, 6, anymal_dofs)), (
+ f"Anymal jacobian leaked model-wide shape: got {tuple(anymal_J.shape)}"
+ )
+
+ sim.step()
+ franka.update(sim.cfg.dt)
+ anymal.update(sim.cfg.dt)
+
+ franka_M = franka.data.mass_matrix.torch
+ anymal_M = anymal.data.mass_matrix.torch
+
+ assert franka_M.shape == torch.Size((num_per_type, franka_dofs, franka_dofs))
+ assert anymal_M.shape == torch.Size((num_per_type, anymal_dofs, anymal_dofs))
+
+ # Each view's mass matrix must have positive diagonals — padded zero
+ # rows/cols (the round-2 bug) would surface as zero diagonals on the
+ # smaller-DoF view. Using a per-diagonal check here instead of det()
+ # because det of a real Franka mass matrix is naturally ~1e-13.
+ assert (franka_M.diagonal(dim1=-2, dim2=-1) > 1e-6).all(), (
+ "Franka mass matrix has non-positive diagonal under heterogeneous scene"
+ )
+ assert (anymal_M.diagonal(dim1=-2, dim2=-1) > 1e-6).all(), (
+ "Anymal mass matrix has non-positive diagonal under heterogeneous scene"
+ )
+
+
+@pytest.mark.parametrize("num_articulations", [4])
+@pytest.mark.parametrize("device", ["cuda:0"])
+@pytest.mark.parametrize("articulation_type", ["panda", "anymal"])
+@pytest.mark.parametrize("gravity_enabled", [False])
+@pytest.mark.isaacsim_ci
+def test_get_jacobians_link_origin_contract(sim, num_articulations, device, articulation_type, gravity_enabled):
+ """``J · q_dot`` must encode the link-origin twist (after the COM->origin shift).
+
+ The IsaacLab task-space controllers (IK / OSC / RMPFlow) silently
+ rely on :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w`
+ returning a Jacobian whose linear rows reference each link's origin
+ (the body's USD prim transform), not its COM. Newton's ``eval_jacobian``
+ natively produces COM-referenced rows; the wrapper applies a per-column
+ shift ``v_origin = v_com - omega x (R · body_com_pos_b)`` to honor the
+ contract. This test asserts the identity by computing both sides
+ independently:
+
+ * Predicted by ``J · q_dot``: takes the (already-shifted) Jacobian
+ and the same ``q_dot`` Newton has post-step. Linear rows should
+ equal v_origin.
+ * Ground truth from ``state.body_qd``: read Newton's per-body spatial
+ twist directly via ``ArticulationView.get_link_velocities`` (which
+ returns ``(v_com_world, omega_world)``), then apply the same shift
+ in python and compare.
+
+ Reading the velocity from the ArticulationView state rather than
+ ``data.body_com_lin_vel_w`` bypasses the IsaacLab lazy-buffer chain,
+ which is irrelevant to the contract being tested.
+ """
+ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type)
+ articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device)
+ sim.reset()
+ assert articulation.is_initialized
+
+ # Reproducible non-trivial q_dot — large enough to drive omega well above
+ # the floor where COM offset effects would round into noise.
+ torch.manual_seed(0)
+ qdot = torch.randn(num_articulations, articulation.num_joints, device=device) * 0.5
+ articulation.write_joint_velocity_to_sim_index(velocity=qdot)
+ sim.step()
+ articulation.update(sim.cfg.dt)
+
+ # body_link_jacobian_w prepends ``num_base_dofs`` floating-base columns; slice past
+ # them so the joint axis aligns with joint_vel (actuated-only).
+ J = articulation.data.body_link_jacobian_w.torch[..., articulation.num_base_dofs :]
+ qdot_view = articulation.data.joint_vel.torch
+ v_pred = torch.einsum("nbij,nj->nbi", J, qdot_view) # (N, B_jac, 6)
+ v_pred_lin = v_pred[..., 0:3]
+ v_pred_ang = v_pred[..., 3:6]
+
+ # Ground truth from Newton state. ``get_link_velocities`` returns shape
+ # (num_instances, 1, num_bodies, 6) — per-articulation grouping with
+ # one articulation per instance — so we squeeze the inner dim.
+ state = SimulationManager.get_state_0()
+ body_qd_view = wp.to_torch(articulation.root_view.get_link_velocities(state)).squeeze(1)
+ body_v_com = body_qd_view[..., :3]
+ body_omega = body_qd_view[..., 3:]
+
+ # World-frame COM-to-origin offset, derived from already-computed
+ # data layer outputs (avoids quaternion-convention pitfalls).
+ body_com_pos_w = articulation.data.body_com_pos_w.torch # (N, num_bodies, 3)
+ body_link_pos_w = articulation.data.body_link_pos_w.torch # (N, num_bodies, 3)
+ c_world = body_com_pos_w - body_link_pos_w
+
+ if articulation.is_fixed_base:
+ body_v_com = body_v_com[:, 1:]
+ body_omega = body_omega[:, 1:]
+ c_world = c_world[:, 1:]
+
+ # Expected v_origin = v_com - omega x c_world.
+ v_origin_expected = body_v_com - torch.cross(body_omega, c_world, dim=-1)
+
+ # Tolerance: 5 mm absolute. The COM-offset bug produces a ~3 cm bias
+ # on the panda hand under the 0.5-rad/s injected qdot, well above
+ # this floor; numerical noise from kernel ordering stays under 1 mm.
+ torch.testing.assert_close(v_pred_ang, body_omega, atol=5e-3, rtol=1e-2)
+ torch.testing.assert_close(v_pred_lin, v_origin_expected, atol=5e-3, rtol=1e-2)
+
+
+@pytest.mark.parametrize("num_articulations", [4])
+@pytest.mark.parametrize("device", ["cuda:0"])
+@pytest.mark.parametrize("articulation_type", ["panda", "anymal"])
+@pytest.mark.parametrize("gravity_enabled", [False])
+@pytest.mark.isaacsim_ci
+def test_get_mass_matrix_symmetry_pd(sim, num_articulations, device, articulation_type, gravity_enabled):
+ """The joint-space mass matrix ``M(q)`` must be square, symmetric, and positive-definite.
+
+ This pins three structural properties of
+ :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`:
+
+ * **Square**: shape ``(N, num_joints + num_base_dofs, num_joints + num_base_dofs)``.
+ A transposed gather or a non-square scratch buffer would be caught
+ here before downstream OSC inversion silently propagates garbage.
+ * **Symmetric**: ``M == M.T`` to numerical precision. The joint-
+ space inertia tensor is symmetric by construction; an asymmetric
+ result indicates a wrong-axis gather, half-populated buffer, or
+ Cholesky-input bug.
+ * **Positive-definite**: ``torch.linalg.cholesky(M)`` succeeds. OSC
+ computes ``M_b = (J · M^-1 · J^T)^-1`` which requires PD on every
+ step. A non-PD M would fail downstream as ``LinAlgError``; this
+ test catches it earlier and pinpoints the source.
+
+ Parameterized on both fixed-base (panda) and floating-base (anymal).
+ Both backends include the floating-base DoF rows/cols on the front of
+ the DoF axis for floating-base assets.
+ """
+ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type)
+ articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device)
+ sim.reset()
+ assert articulation.is_initialized
+
+ sim.step()
+ articulation.update(sim.cfg.dt)
+
+ M = articulation.data.mass_matrix.torch # (N, J, J)
+ assert M.dim() == 3, f"expected 3-D mass matrix, got shape {tuple(M.shape)}"
+ assert M.shape[0] == num_articulations
+ assert M.shape[1] == M.shape[2], f"mass matrix is not square: {tuple(M.shape)}"
+
+ # Symmetric to numerical precision.
+ asym = (M - M.transpose(-1, -2)).abs().max().item()
+ assert asym < 1e-4, f"|M - M^T|_max = {asym:.3e} — mass matrix is not symmetric"
+
+ # Positive-definite via Cholesky. Adds a tiny diagonal jitter to
+ # tolerate the floor of float32 PD eigenvalues without masking real
+ # non-PD bugs (the jitter is well below realistic inertia scales).
+ eye = torch.eye(M.shape[-1], device=M.device, dtype=M.dtype).expand_as(M)
+ torch.linalg.cholesky(M + 1e-6 * eye)
+
+
+@pytest.mark.parametrize("num_articulations", [1])
+@pytest.mark.parametrize("device", ["cuda:0"])
+@pytest.mark.parametrize("articulation_type", ["panda", "anymal"])
+@pytest.mark.parametrize("gravity_enabled", [False])
+@pytest.mark.isaacsim_ci
+def test_jacobian_refreshes_after_manual_joint_write(
+ sim, num_articulations, device, articulation_type, gravity_enabled
+):
+ """After ``write_joint_position_to_sim_index`` (no sim step), the Jacobian read
+ must reflect the new joint state — not the previous one.
+
+ Catches:
+ - Missing FK trigger in :attr:`body_com_jacobian_w` (eval_jacobian uses stale
+ ``state.body_q``).
+ - Missing FK trigger in :attr:`body_link_jacobian_w` shift kernel.
+
+ The contract: ``J`` read directly after a manual write must equal ``J`` read
+ after ``sim.step + update`` — the latter is the ground-truth fresh-FK reference.
+ """
+ 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)
+
+ # Read J / M at the baseline joint state.
+ q_baseline = articulation.data.joint_pos.torch.clone()
+ J_link_0 = articulation.data.body_link_jacobian_w.torch.clone()
+ J_com_0 = articulation.data.body_com_jacobian_w.torch.clone()
+
+ # Manually write a different joint state — large delta to make Jacobian change visible.
+ # No sim.step / update — FK becomes stale (write_joint_position_to_sim sets _fk_timestamp = -1).
+ q_target = q_baseline + 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)
+
+ # If the FK trigger works: forward() runs, body_q is refreshed to match q_target,
+ # eval_jacobian / shift kernel see fresh body poses, J reflects q_target → differs from J at baseline.
+ # If the trigger is missing: body_q stays at baseline, J unchanged from J_link_0 / J_com_0.
+ J_link_1 = articulation.data.body_link_jacobian_w.torch.clone()
+ J_com_1 = articulation.data.body_com_jacobian_w.torch.clone()
+
+ assert not torch.allclose(J_link_0, J_link_1, atol=1e-3), (
+ "body_link_jacobian_w did not change after manual joint write — "
+ "FK trigger likely missing (eval_jacobian / shift kernel reading stale state.body_q)."
+ )
+ assert not torch.allclose(J_com_0, J_com_1, atol=1e-3), (
+ "body_com_jacobian_w did not change after manual joint write — FK trigger likely missing before eval_jacobian."
+ )
+
+
+@pytest.mark.parametrize("num_articulations", [1])
+@pytest.mark.parametrize("device", ["cuda:0"])
+@pytest.mark.parametrize("articulation_type", ["panda", "anymal"])
+@pytest.mark.parametrize("gravity_enabled", [False])
+@pytest.mark.isaacsim_ci
+def test_mass_matrix_refreshes_after_manual_joint_write(
+ sim, num_articulations, device, articulation_type, gravity_enabled
+):
+ """After ``write_joint_position_to_sim_index`` (no sim step), the mass matrix read
+ must reflect the new joint state.
+
+ The mass matrix depends on ``q`` (joint positions) through the body-spatial-inertia
+ transformation in eval_mass_matrix's ``compute_body_spatial_inertia`` step, which
+ reads ``state.body_q``. Same FK-staleness pattern as the Jacobian.
+ """
+ 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)
+
+ M_0 = articulation.data.mass_matrix.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)
+ M_1 = articulation.data.mass_matrix.torch.clone()
+
+ assert not torch.allclose(M_0, M_1, atol=1e-3), (
+ "mass_matrix did not change after manual joint write — "
+ "FK trigger likely missing before eval_mass_matrix (compute_body_spatial_inertia "
+ "reads stale state.body_q)."
+ )
+
+
+@pytest.mark.parametrize("device", ["cuda:0"])
+@pytest.mark.parametrize("articulation_type", ["panda"])
+@pytest.mark.parametrize("gravity_enabled", [False])
+@pytest.mark.isaacsim_ci
+def test_franka_ik_tracking_accuracy(sim, device, articulation_type, gravity_enabled):
+ """Newton-side IK convergence sentinel.
+
+ Runs a full IK tracking loop end-to-end through the new
+ ``robot.data.body_link_jacobian_w`` accessor and records the steady-state EE
+ pose error. With the robot teleported to its configured init_state
+ home pose and scene gravity off, Newton's IK converges to
+ machine-precision tracking (sub-mm). A bridge regression
+ (wrong-reference-frame Jacobian, missing COM->origin shift, DoF
+ mis-ordering) would push the steady-state error well above the
+ threshold below.
+
+ The pose teleport is deliberate: the standalone test path does not
+ invoke a manager-based env reset (which is what normally pushes
+ :attr:`~isaaclab.assets.ArticulationData.default_joint_pos` to sim).
+ Without it, the robot starts at the URDF-neutral pose where the
+ Franka wrist axes nearly align (rank-deficient Jacobian) and DLS
+ plateaus at multi-cm error -- a kinematic-singularity artifact, not
+ a bridge or Newton issue.
+
+ See ``test_get_jacobians_link_origin_contract`` (above) for the
+ sharper unit-level pin on the Jacobian's reference-point contract.
+ """
+ robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim)
+
+ sim.step()
+ robot.update(sim.cfg.dt)
+ target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.05, 0.0, 0.0), device)
+
+ ik = DifferentialIKController(
+ DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"),
+ num_envs=1,
+ device=device,
+ )
+ ik.set_command(target_pose_b)
+
+ pos_history: list[float] = []
+ rot_history: list[float] = []
+ for _ in range(800):
+ jacobian = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids)
+ ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx)
+ joint_pos = robot.data.joint_pos.torch[:, arm_joint_ids]
+
+ joint_pos_des = ik.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)
+
+ robot.set_joint_position_target(joint_pos_des, joint_ids=arm_joint_ids)
+ robot.write_data_to_sim()
+ sim.step()
+ robot.update(sim.cfg.dt)
+
+ pos_error, rot_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())
+ rot_history.append(rot_error.norm(dim=-1).max().item())
+
+ pos_min, pos_mean = _summarize_history(pos_history)
+ rot_min, rot_mean = _summarize_history(rot_history)
+
+ # Print metrics every run for stress-test capture.
+ print(f"IK_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}")
+
+ # Regression sentinel: assert on tail mean rather than min. Tail
+ # min is the bottom of any oscillation envelope and can be tiny
+ # while the actual tracking error is much larger. With the
+ # configured home pose and scene gravity off, Newton converges to
+ # machine precision (sub-mm). The 5 mm bound absorbs any CUDA-
+ # kernel-ordering noise while remaining well below the "totally
+ # broken" regime: a bridge regression (wrong-frame Jacobian,
+ # missing COM->origin shift, DoF mis-ordering) would push the
+ # steady-state error well past this bound.
+ assert pos_mean < 5e-3, f"IK pos_mean {pos_mean:.5f} > 5 mm — bridge regression?"
+ assert rot_mean < 5e-2, f"IK 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", [False])
+@pytest.mark.isaacsim_ci
+def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_enabled):
+ """Newton-side OSC pose tracking sentinel.
+
+ Mirror of the existing PhysX-side OSC tests in
+ :mod:`isaaclab.test.controllers.test_operational_space`, scoped to
+ Franka pose-abs tracking on Newton. Like the IK sentinel above, this
+ test exercises the full controller-bridge pipeline
+ (:attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` +
+ :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`) end-to-end
+ 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``
+ 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)``.
+ """
+ robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim, zero_actuator_pd=True)
+
+ 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)
+ target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.05, 0.0, 0.0), device)
+
+ pos_history: list[float] = []
+ rot_history: list[float] = []
+ for _ in range(800):
+ 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]
+ 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=None,
+ )
+
+ 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, rot_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())
+ rot_history.append(rot_error.norm(dim=-1).max().item())
+
+ pos_min, pos_mean = _summarize_history(pos_history)
+ rot_min, rot_mean = _summarize_history(rot_history)
+
+ print(f"OSC_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}")
+
+ # Regression sentinel: assert on tail mean rather than min. With
+ # ``current_ee_vel_b = J · q_dot`` providing OSC's damping term and
+ # the actuator PD zeroed, the impedance settles to machine
+ # precision -- same ballpark as the IK test. The 5 mm bound is a
+ # bridge regression sentinel: a wrong J, wrong mass matrix, or
+ # DoF mis-ordering pushes the steady-state error well past it
+ # because OSC consumes both ``body_link_jacobian_w`` and
+ # ``mass_matrix`` per step.
+ assert pos_mean < 5e-3, f"OSC pos_mean {pos_mean:.5f} > 5 mm — bridge regression?"
+ assert rot_mean < 5e-2, f"OSC rot_mean {rot_mean:.5f} > 0.05 rad — bridge regression?"
+
+
if __name__ == "__main__":
pytest.main([__file__, "-v", "--maxfail=1"])
diff --git a/source/isaaclab_ovphysx/changelog.d/jichuanh-ik-newton-compat-mvp.rst b/source/isaaclab_ovphysx/changelog.d/jichuanh-ik-newton-compat-mvp.rst
new file mode 100644
index 000000000000..f2cc47afe8a2
--- /dev/null
+++ b/source/isaaclab_ovphysx/changelog.d/jichuanh-ik-newton-compat-mvp.rst
@@ -0,0 +1,12 @@
+Changed
+^^^^^^^
+
+* Inherits the base
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w`,
+ :attr:`~isaaclab.assets.BaseArticulationData.body_com_jacobian_w`,
+ :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`, and
+ :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces`
+ :class:`NotImplementedError` defaults — ovphysx's OmniGraph-based view
+ does not expose articulation Jacobians, mass matrices, or gravity
+ compensation. Use the PhysX or Newton backends for task-space
+ controllers.
diff --git a/source/isaaclab_physx/changelog.d/jichuanh-ik-newton-compat-mvp.rst b/source/isaaclab_physx/changelog.d/jichuanh-ik-newton-compat-mvp.rst
new file mode 100644
index 000000000000..8ffa5ad63b15
--- /dev/null
+++ b/source/isaaclab_physx/changelog.d/jichuanh-ik-newton-compat-mvp.rst
@@ -0,0 +1,31 @@
+Added
+^^^^^
+
+* Added PhysX implementations of
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w`,
+ :attr:`~isaaclab.assets.BaseArticulationData.body_com_jacobian_w`,
+ :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`, and
+ :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces`
+ on :class:`~isaaclab_physx.assets.ArticulationData`. The COM
+ variant is a passthrough to ``physx.ArticulationView.get_jacobians``;
+ the link-origin variant applies a new
+ :func:`~isaaclab_physx.assets.articulation.kernels.shift_jacobian_com_to_origin`
+ Warp kernel to convert the COM-referenced linear-velocity rows to
+ link-origin references using each body's pose and COM offset. All
+ four properties preserve the full DoF axis, including the 6 leading
+ floating-base columns/rows PhysX's raw tensor view prepends on
+ floating-base assets — matching the cross-library industry convention
+ (Pinocchio, Drake, MuJoCo, RBDL, OCS2, iDynTree) and Newton's
+ ``ArticulationView`` layout.
+
+Fixed
+^^^^^
+
+* Fixed a latent correctness bug in IK / OSC controllers on the PhysX
+ backend, where the previously-exposed Jacobian was COM-referenced but
+ the controllers used :attr:`~isaaclab_physx.assets.ArticulationData.body_link_pose_w`
+ as the EE pose setpoint. The frame mismatch caused tracking error on
+ bodies whose COM offset is non-trivial. The new
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w`
+ applies the COM→origin shift so the Jacobian and pose share a
+ reference point.
diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py
index 6258b5c5b8e4..ea2c85e4c3bd 100644
--- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py
+++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py
@@ -464,6 +464,10 @@ def write_root_link_pose_to_sim_index(
self.data._body_state_w.timestamp = -1.0
self.data._body_link_state_w.timestamp = -1.0
self.data._body_com_state_w.timestamp = -1.0
+ # Task-space accessors: body-frame Jacobian + gravity comp depend on root orientation;
+ # mass_matrix does not (configuration-space).
+ self.data._body_com_jacobian_w.timestamp = -1.0
+ self.data._gravity_compensation_forces.timestamp = -1.0
# set into simulation
self.root_view.set_root_transforms(self._get_root_link_pose_w_f32(), indices=env_ids)
@@ -553,6 +557,10 @@ def write_root_com_pose_to_sim_index(
self.data._body_state_w.timestamp = -1.0
self.data._body_link_state_w.timestamp = -1.0
self.data._body_com_state_w.timestamp = -1.0
+ # Task-space accessors: body-frame Jacobian + gravity comp depend on root orientation;
+ # mass_matrix does not (configuration-space).
+ self.data._body_com_jacobian_w.timestamp = -1.0
+ self.data._gravity_compensation_forces.timestamp = -1.0
# set into simulation
self.root_view.set_root_transforms(self._get_root_link_pose_w_f32(), indices=env_ids)
@@ -874,6 +882,10 @@ def write_joint_state_to_sim_index(
self.data._body_state_w.timestamp = -1.0
self.data._body_link_state_w.timestamp = -1.0
self.data._body_com_state_w.timestamp = -1.0
+ # Task-space accessors all depend on joint state.
+ self.data._body_com_jacobian_w.timestamp = -1.0
+ self.data._mass_matrix.timestamp = -1.0
+ self.data._gravity_compensation_forces.timestamp = -1.0
# set into simulation
self.root_view.set_dof_positions(self.data._joint_pos.data, indices=env_ids)
self.root_view.set_dof_velocities(self.data._joint_vel.data, indices=env_ids)
@@ -963,6 +975,10 @@ def write_joint_position_to_sim_index(
self.data._body_state_w.timestamp = -1.0
self.data._body_link_state_w.timestamp = -1.0
self.data._body_com_state_w.timestamp = -1.0
+ # Task-space accessors all depend on joint state.
+ self.data._body_com_jacobian_w.timestamp = -1.0
+ self.data._mass_matrix.timestamp = -1.0
+ self.data._gravity_compensation_forces.timestamp = -1.0
# set into simulation
self.root_view.set_dof_positions(self.data._joint_pos.data, indices=env_ids)
diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py
index 3d7e6e9cb483..6f1ab28547c9 100644
--- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py
+++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py
@@ -857,6 +857,77 @@ def body_com_pose_b(self) -> ProxyArray:
self._body_com_pose_b_ta = ProxyArray(self._body_com_pose_b.data)
return self._body_com_pose_b_ta
+ """
+ Dynamics quantities (task-space controllers).
+ """
+
+ @property
+ def body_com_jacobian_w(self) -> ProxyArray:
+ """See :attr:`isaaclab.assets.BaseArticulationData.body_com_jacobian_w`.
+
+ PhysX implementation: passthrough of ``_root_view.get_jacobians()``, which is
+ natively Center-Of-Mass-referenced. Refresh is gated by ``_sim_timestamp`` and
+ invalidated by ``write_*_to_sim_index``; the ``ProxyArray`` wrapper is lazy-init
+ once and reused thereafter.
+ """
+ if self._body_com_jacobian_w.timestamp < self._sim_timestamp:
+ self._body_com_jacobian_w.data = self._root_view.get_jacobians()
+ self._body_com_jacobian_w.timestamp = self._sim_timestamp
+ if self._body_com_jacobian_w_ta is None:
+ self._body_com_jacobian_w_ta = ProxyArray(self._body_com_jacobian_w.data)
+ return self._body_com_jacobian_w_ta
+
+ @property
+ def body_link_jacobian_w(self) -> ProxyArray:
+ """See :attr:`isaaclab.assets.BaseArticulationData.body_link_jacobian_w`.
+
+ PhysX implementation: applies the COM→origin shift kernel to
+ :attr:`body_com_jacobian_w` (PhysX's engine output is COM-referenced).
+ """
+ wp.launch(
+ articulation_kernels.shift_jacobian_com_to_origin,
+ dim=self._body_link_jacobian_w_buf.shape[:2] + (self._body_link_jacobian_w_buf.shape[3],),
+ inputs=[
+ self.body_link_pose_w.warp,
+ self.body_com_pos_b.warp,
+ self._jacobian_link_offset,
+ self.body_com_jacobian_w.warp,
+ ],
+ outputs=[self._body_link_jacobian_w_buf],
+ device=self.device,
+ )
+ return self._body_link_jacobian_w_ta
+
+ @property
+ def mass_matrix(self) -> ProxyArray:
+ """See :attr:`isaaclab.assets.BaseArticulationData.mass_matrix`.
+
+ PhysX implementation: passthrough of ``_root_view.get_generalized_mass_matrices()``.
+ Refresh is gated by ``_sim_timestamp`` and invalidated by ``write_*_to_sim_index``;
+ the ``ProxyArray`` wrapper is lazy-init once and reused thereafter.
+ """
+ if self._mass_matrix.timestamp < self._sim_timestamp:
+ self._mass_matrix.data = self._root_view.get_generalized_mass_matrices()
+ self._mass_matrix.timestamp = self._sim_timestamp
+ if self._mass_matrix_ta is None:
+ self._mass_matrix_ta = ProxyArray(self._mass_matrix.data)
+ return self._mass_matrix_ta
+
+ @property
+ def gravity_compensation_forces(self) -> ProxyArray:
+ """See :attr:`isaaclab.assets.BaseArticulationData.gravity_compensation_forces`.
+
+ PhysX implementation: passthrough of ``_root_view.get_gravity_compensation_forces()``.
+ Refresh is gated by ``_sim_timestamp`` and invalidated by ``write_*_to_sim_index``;
+ the ``ProxyArray`` wrapper is lazy-init once and reused thereafter.
+ """
+ if self._gravity_compensation_forces.timestamp < self._sim_timestamp:
+ self._gravity_compensation_forces.data = self._root_view.get_gravity_compensation_forces()
+ self._gravity_compensation_forces.timestamp = self._sim_timestamp
+ if self._gravity_compensation_forces_ta is None:
+ self._gravity_compensation_forces_ta = ProxyArray(self._gravity_compensation_forces.data)
+ return self._gravity_compensation_forces_ta
+
"""
Joint state properties.
"""
@@ -1370,6 +1441,46 @@ def _create_buffers(self) -> None:
self._root_com_lin_vel_b = TimestampedBuffer((self._num_instances), self.device, wp.vec3f)
self._root_com_ang_vel_b = TimestampedBuffer((self._num_instances), self.device, wp.vec3f)
+ # -- dynamics quantities for task-space controllers
+ # PhysX's Jacobian rows include the root body for floating-base and exclude only the
+ # fixed root for fixed-base (``_jacobian_link_offset`` handles the body axis). PhysX's
+ # raw Jacobian / mass matrix / gravity-comp prepend 6 base-DoF columns on floating-
+ # base (the engine's natural form), matching the industry-standard convention used by
+ # Pinocchio, Drake, MuJoCo, RBDL, OCS2, and iDynTree. We pass through the full DoF
+ # axis: shape ``(N, num_jacobi_bodies, 6, num_joints + num_base_dofs)``. Newton wraps
+ # ``eval_jacobian`` to match the same column layout. ``body_com_jacobian_w`` /
+ # ``mass_matrix`` / ``gravity_compensation_forces`` pass through the engine buffer on
+ # every read; we only own a buffer for the link-origin Jacobian (output of the shift
+ # kernel).
+ is_fixed_base = self._root_view.shared_metatype.fixed_base
+ self._jacobian_link_offset = 1 if is_fixed_base else 0
+ num_jacobi_bodies = self._num_bodies - self._jacobian_link_offset
+ num_base_dofs = 0 if is_fixed_base else 6
+ self._body_link_jacobian_w_buf = wp.zeros(
+ (self._num_instances, num_jacobi_bodies, 6, self._num_joints + num_base_dofs),
+ dtype=wp.float32,
+ device=self.device,
+ )
+ # ``TimestampedBuffer``s for the three engine-passthrough properties. The placeholder
+ # ``wp.zeros`` allocation is replaced on first read by the engine view returned from
+ # ``_root_view.get_*()``; timestamps are advanced on each refresh and invalidated by
+ # write-paths.
+ self._body_com_jacobian_w = TimestampedBuffer(
+ (self._num_instances, num_jacobi_bodies, 6, self._num_joints + num_base_dofs),
+ self.device,
+ wp.float32,
+ )
+ self._mass_matrix = TimestampedBuffer(
+ (self._num_instances, self._num_joints + num_base_dofs, self._num_joints + num_base_dofs),
+ self.device,
+ wp.float32,
+ )
+ self._gravity_compensation_forces = TimestampedBuffer(
+ (self._num_instances, self._num_joints + num_base_dofs),
+ self.device,
+ wp.float32,
+ )
+
# Default root pose and velocity
self._default_root_pose = wp.zeros((self._num_instances), dtype=wp.transformf, device=self.device)
self._default_root_vel = wp.zeros((self._num_instances), dtype=wp.spatial_vectorf, device=self.device)
@@ -1532,6 +1643,16 @@ def _pin_proxy_arrays(self) -> None:
self._body_com_vel_w_ta: ProxyArray | None = None
self._body_com_acc_w_ta: ProxyArray | None = None
self._body_com_pose_b_ta: ProxyArray | None = None
+ # Dynamics quantities (task-space controllers). ``_body_link_jacobian_w`` wraps our
+ # own pre-allocated buffer (pointer-stable, eager wrap). The three engine-passthrough
+ # wrappers are lazy-init inside their property bodies on first read, matching the
+ # ``TimestampedBuffer`` + ``ProxyArray`` cache pattern used by ``body_link_pose_w``,
+ # ``joint_pos``, and the rest of this file. Refresh is gated by ``_sim_timestamp`` and
+ # invalidated by ``write_*_to_sim_index`` setting ``timestamp = -1.0``.
+ self._body_link_jacobian_w_ta = ProxyArray(self._body_link_jacobian_w_buf)
+ self._body_com_jacobian_w_ta: ProxyArray | None = None
+ self._mass_matrix_ta: ProxyArray | None = None
+ self._gravity_compensation_forces_ta: ProxyArray | None = None
# Body properties
self._body_mass_ta: ProxyArray | None = None
self._body_inertia_ta: ProxyArray | None = None
diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/kernels.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/kernels.py
index 5686c864dd94..0c2e385af173 100644
--- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/kernels.py
+++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/kernels.py
@@ -486,3 +486,65 @@ def extract_friction_properties(
out_friction[i, j] = friction_props[i, j, 0]
out_dynamic_friction[i, j] = friction_props[i, j, 1]
out_viscous_friction[i, j] = friction_props[i, j, 2]
+
+
+@wp.kernel
+def shift_jacobian_com_to_origin(
+ body_link_pose: wp.array2d(dtype=wp.transformf),
+ body_com_pos_b: wp.array2d(dtype=wp.vec3f),
+ link_offset: wp.int32,
+ src: wp.array4d(dtype=wp.float32),
+ dst: wp.array4d(dtype=wp.float32),
+):
+ """Shift the linear-velocity rows of the Jacobian from COM to link origin.
+
+ PhysX's ``ArticulationView.get_jacobians()`` returns ``J · q_dot = [v_com_world, omega_world]``
+ per link — the linear rows are the velocity at the link's center of mass, expressed in
+ world frame. The :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` contract
+ requires the linear rows to be the velocity at the link **origin** (USD prim transform) so
+ that ``J · q_dot[body_idx]`` matches
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_lin_vel_w` /
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_ang_vel_w`.
+
+ The shift identity is the same one applied per-body by
+ :func:`~isaaclab_physx.assets.kernels.get_link_vel_from_root_com_vel_func`, but layered onto
+ every Jacobian column: each column represents the spatial velocity contribution of one DoF,
+ and shifting a spatial velocity from COM to link origin uses ``v_origin = v_com - omega x
+ (R · body_com_pos_b)``.
+
+ Notes on layout:
+ * Jacobian rows ``[0:3]`` are linear velocity, ``[3:6]`` are angular.
+ * ``body_link_pose`` and ``body_com_pos_b`` are indexed by the articulation's full body
+ count. PhysX's Jacobian rows are also indexed by the full body count for floating-base
+ and exclude only the root for fixed-base, so ``link_offset = 1`` for fixed-base and
+ ``link_offset = 0`` for floating-base, matching Newton's convention.
+
+ Args:
+ body_link_pose: Per-body link pose in world frame. Shape is (num_instances, num_bodies).
+ body_com_pos_b: Per-body center-of-mass offset expressed in the body's link frame. Shape
+ is (num_instances, num_bodies).
+ link_offset: Offset added to the jacobian-row body index to reach the full body index.
+ ``1`` for fixed-base, ``0`` for floating-base.
+ src: COM-referenced Jacobian (read-only). Shape is (num_instances, num_jacobi_bodies, 6,
+ num_joints + num_base_dofs).
+ dst: Output buffer for the link-origin Jacobian. Same shape as ``src``. Linear rows
+ ``[0:3]`` are written with the shifted velocity; angular rows ``[3:6]`` are copied
+ unchanged (angular velocity is reference-point invariant).
+ """
+ n, b, dof = wp.tid()
+ full_body_idx = b + link_offset
+
+ R = wp.transform_get_rotation(body_link_pose[n, full_body_idx])
+ c_world = wp.quat_rotate(R, body_com_pos_b[n, full_body_idx])
+
+ v_com = wp.vec3(src[n, b, 0, dof], src[n, b, 1, dof], src[n, b, 2, dof])
+ omega = wp.vec3(src[n, b, 3, dof], src[n, b, 4, dof], src[n, b, 5, dof])
+
+ v_origin = v_com - wp.cross(omega, c_world)
+
+ dst[n, b, 0, dof] = v_origin[0]
+ dst[n, b, 1, dof] = v_origin[1]
+ dst[n, b, 2, dof] = v_origin[2]
+ dst[n, b, 3, dof] = omega[0]
+ dst[n, b, 4, dof] = omega[1]
+ dst[n, b, 5, dof] = omega[2]
diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py
index 227c091a1652..af36a365cb66 100644
--- a/source/isaaclab_physx/test/assets/test_articulation.py
+++ b/source/isaaclab_physx/test/assets/test_articulation.py
@@ -29,16 +29,23 @@
import isaaclab.utils.string as string_utils
from isaaclab.actuators import ActuatorBase, IdealPDActuatorCfg, ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg
+from isaaclab.controllers import (
+ DifferentialIKController,
+ DifferentialIKControllerCfg,
+ OperationalSpaceController,
+ OperationalSpaceControllerCfg,
+)
from isaaclab.envs.mdp.terminations import joint_effort_out_of_limit
from isaaclab.managers import SceneEntityCfg
from isaaclab.sim import build_simulation_context
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
+from isaaclab.utils.math import compute_pose_error, matrix_from_quat, quat_inv, subtract_frame_transforms
from isaaclab.utils.version import get_isaac_sim_version, has_kit
##
# Pre-defined configs
##
-from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_CFG, SHADOW_HAND_CFG # isort:skip
+from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_CFG, FRANKA_PANDA_HIGH_PD_CFG, SHADOW_HAND_CFG # isort:skip
def generate_articulation_cfg(
@@ -182,6 +189,107 @@ def generate_articulation(
return articulation, translations
+# ---------------------------------------------------------------------------
+# Franka task-space tracking helpers (shared between IK and OSC tests).
+# Mirrors the helpers in ``isaaclab_newton/test/assets/test_articulation.py``.
+# ---------------------------------------------------------------------------
+
+
+def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False, enable_rigid_body_gravity: bool = False):
+ """Build a Franka articulation at its configured home pose.
+
+ See the Newton-side mirror for full docs. Standalone tests skip the
+ env reset path that normally pushes ``default_joint_pos`` to sim,
+ so we teleport explicitly to avoid the URDF-neutral
+ near-singular pose where the Franka wrist axes nearly align.
+
+ Args:
+ sim: The simulation context to use.
+ zero_actuator_pd: If True, sets the panda_shoulder/panda_forearm
+ actuator stiffness and damping to zero.
+ enable_rigid_body_gravity: If True, override
+ ``FRANKA_PANDA_HIGH_PD_CFG.spawn.rigid_props.disable_gravity``
+ (which defaults to True) so gravity actually loads the arm. Required
+ for any test that wants to exercise gravity-related dynamics
+ (e.g. gravity-compensation accuracy tests).
+
+ Returns:
+ Tuple of ``(robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids)``.
+ """
+ cfg = FRANKA_PANDA_HIGH_PD_CFG.copy().replace(prim_path="/World/Env_.*/Robot")
+ if zero_actuator_pd:
+ cfg.actuators["panda_shoulder"].stiffness = 0.0
+ cfg.actuators["panda_shoulder"].damping = 0.0
+ cfg.actuators["panda_forearm"].stiffness = 0.0
+ cfg.actuators["panda_forearm"].damping = 0.0
+ if enable_rigid_body_gravity:
+ cfg = cfg.replace(
+ spawn=cfg.spawn.replace(
+ rigid_props=cfg.spawn.rigid_props.replace(disable_gravity=False),
+ ),
+ )
+ sim_utils.create_prim("/World/Env_0", "Xform", translation=(0.0, 0.0, 0.0))
+ robot = Articulation(cfg)
+ sim.reset()
+ assert robot.is_initialized
+
+ ee_frame_idx = robot.find_bodies("panda_hand")[0][0]
+ ee_jacobi_idx = ee_frame_idx - 1
+ arm_joint_ids = robot.find_joints(["panda_joint.*"])[0]
+
+ robot.write_joint_state_to_sim(
+ position=robot.data.default_joint_pos.torch[:, :].clone(),
+ velocity=robot.data.default_joint_vel.torch[:, :].clone(),
+ )
+ return robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids
+
+
+def _compute_ee_pose_root(robot, ee_frame_idx):
+ """Return ``(ee_pos_b, ee_quat_b, root_pose_w)`` in the root frame."""
+ ee_pose_w = robot.data.body_pose_w.torch[:, ee_frame_idx]
+ root_pose_w = robot.data.root_pose_w.torch
+ ee_pos_b, ee_quat_b = subtract_frame_transforms(
+ root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
+ )
+ return ee_pos_b, ee_quat_b, root_pose_w
+
+
+def _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids):
+ """Return the EE Jacobian sliced to ``arm_joint_ids`` and rotated to the root frame."""
+ jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, :][:, :, arm_joint_ids]
+ base_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_pose_w.torch[:, 3:7]))
+ jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :])
+ jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :])
+ return jacobian
+
+
+def _compute_ee_vel_root(jacobian_b, joint_vel):
+ """Return the EE 6D velocity in the root frame as ``J · q_dot``.
+
+ Required to make OSC's ``kd * ee_vel_b`` damping term meaningful.
+ Passing zero EE velocity (the convenient hack) leaves the impedance
+ undamped and the EE oscillates around the target. ``J · q_dot``
+ avoids relying on ``data.body_vel_w`` (Newton's lazy velocity
+ buffers can return stale/zero values until forced materialization),
+ keeping the helper backend-symmetric. ``J`` correctness is pinned
+ independently by ``test_get_jacobians_link_origin_contract``.
+ """
+ return torch.bmm(jacobian_b, joint_vel.unsqueeze(-1)).squeeze(-1)
+
+
+def _build_relative_pose_target(robot, ee_frame_idx, delta_xyz, device):
+ """Build a target pose = (current EE pose) + ``delta_xyz``, preserving orientation."""
+ initial_ee_pos_b, initial_ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx)
+ target_pos_b = initial_ee_pos_b + torch.tensor([list(delta_xyz)], device=device, dtype=initial_ee_pos_b.dtype)
+ return torch.cat([target_pos_b, initial_ee_quat_b], dim=-1)
+
+
+def _summarize_history(history, tail: int = 200):
+ """Return ``(min, mean)`` over the last ``tail`` samples."""
+ tail_slice = history[-tail:]
+ return min(tail_slice), sum(tail_slice) / len(tail_slice)
+
+
@pytest.fixture
def sim(request):
"""Create simulation context with the specified device."""
@@ -572,7 +680,7 @@ def test_initialization_fixed_base_made_floating_base(sim, num_articulations, de
sim: The simulation fixture
num_articulations: Number of articulations to test
"""
- articulation_cfg = generate_articulation_cfg(articulation_type="panda")
+ articulation_cfg = generate_articulation_cfg(articulation_type="panda").copy()
# Unfix root link by making it non-kinematic
articulation_cfg.spawn.articulation_props.fix_root_link = False
articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device)
@@ -2127,5 +2235,635 @@ def test_set_material_properties(sim, num_articulations, device, add_ground_plan
torch.testing.assert_close(materials_check, materials)
+##
+# Shape-contract regression tests for the new BaseArticulation accessors.
+# Mirror the Newton-side tests so both backends can be diffed against the
+# same documented contract. These are PhysX's reference shapes — when the
+# Newton-side tests pass with the same expected_shape formulas, the
+# cross-backend contract holds.
+##
+
+
+@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_jacobians_shape_fixed_base(sim, num_articulations, device, articulation_type):
+ """PhysX reference: fixed-base ``body_link_jacobian_w`` is ``(N, num_bodies-1, 6, num_joints)``."""
+ 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
+
+ J = articulation.data.body_link_jacobian_w.torch
+ expected = (num_articulations, articulation.num_bodies - 1, 6, articulation.num_joints)
+ assert J.shape == torch.Size(expected), f"expected {expected}, got {tuple(J.shape)}"
+
+
+@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_mass_matrix_shape_and_nonsingular_fixed_base(sim, num_articulations, device, articulation_type):
+ """PhysX reference: fixed-base ``mass_matrix`` shape + non-singular."""
+ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type)
+ articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device)
+ sim.reset()
+ assert articulation.is_initialized
+
+ sim.step()
+ articulation.update(sim.cfg.dt)
+
+ M = articulation.data.mass_matrix.torch
+ expected = (num_articulations, articulation.num_joints, articulation.num_joints)
+ assert M.shape == torch.Size(expected), f"expected {expected}, got {tuple(M.shape)}"
+
+ # Each diagonal entry is the joint's effective inertia and must be positive
+ # for any physical articulation. Padded zero rows/cols (the bug) would show
+ # up here as zero diagonal entries — much more sensitive than checking the
+ # determinant, which can be small for a well-conditioned 9x9 just from
+ # numerical cancellation.
+ diag = M.diagonal(dim1=-2, dim2=-1)
+ 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("add_ground_plane", [True])
+@pytest.mark.parametrize("articulation_type", ["anymal"])
+@pytest.mark.isaacsim_ci
+def test_get_jacobians_shape_floating_base(sim, num_articulations, device, add_ground_plane, articulation_type):
+ """PhysX reference: floating-base ``body_link_jacobian_w``.
+
+ Floating-base articulations include the 6 floating-base spatial-velocity columns
+ at the front of the DoF axis, so the shape is
+ ``(N, num_bodies, 6, num_joints + num_base_dofs)`` — matching Newton and the
+ cross-library industry convention (Pinocchio, Drake, MuJoCo, RBDL, OCS2,
+ iDynTree).
+ """
+ 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
+
+ J = articulation.data.body_link_jacobian_w.torch
+ expected = (num_articulations, articulation.num_bodies, 6, articulation.num_joints + articulation.num_base_dofs)
+ assert J.shape == torch.Size(expected), f"expected {expected}, got {tuple(J.shape)}"
+
+
+@pytest.mark.parametrize("num_articulations", [4])
+@pytest.mark.parametrize("device", ["cuda:0"])
+@pytest.mark.parametrize("articulation_type", ["panda", "anymal"])
+@pytest.mark.parametrize("gravity_enabled", [False])
+@pytest.mark.isaacsim_ci
+def test_get_jacobians_link_origin_contract(sim, num_articulations, device, articulation_type, gravity_enabled):
+ """PhysX reference: ``J · q_dot`` matches ``[body_link_lin_vel_w; body_link_ang_vel_w]``.
+
+ The cross-backend contract on
+ :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` says
+ the Jacobian's linear rows reference each body's link origin. PhysX's
+ raw ``_root_view.get_jacobians()`` returns COM-referenced linear rows;
+ the IsaacLab wrapper applies the COM→origin shift kernel so the contract
+ holds. This test pins the identity from the PhysX side and parametrizes
+ on Anymal so the (non-trivial) shift surfaces if it ever regresses.
+
+ Scene gravity is disabled (``gravity_enabled=False``) so the only source
+ of a J · q_dot ↔ body_*_w mismatch is the reference-point contract (or a
+ regression). The tolerance ``5e-2`` is loose enough to absorb the small
+ PhysX state-propagation lag between the Jacobian and the velocity
+ buffers (~2% on max angular speed) but well below the
+ COM-vs-link-origin bug magnitude (panda hand COM offset ≈ 3 cm × ω at
+ typical motion ≈ several rad/s gives a 0.1+ m/s linear-row residual,
+ 2× the tolerance).
+ """
+ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type)
+ articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device)
+ sim.reset()
+ assert articulation.is_initialized
+
+ torch.manual_seed(0)
+ qdot = torch.randn(num_articulations, articulation.num_joints, device=device) * 0.5
+ articulation.write_joint_velocity_to_sim(velocity=qdot)
+ sim.step()
+ articulation.update(sim.cfg.dt)
+
+ # body_link_jacobian_w prepends ``num_base_dofs`` floating-base columns; slice past
+ # them so the joint axis aligns with joint_vel (actuated-only).
+ J = articulation.data.body_link_jacobian_w.torch[..., articulation.num_base_dofs :]
+ qdot_view = articulation.data.joint_vel.torch
+ v_pred = torch.einsum("nbij,nj->nbi", J, qdot_view)
+
+ body_lin_w = articulation.data.body_link_lin_vel_w.torch
+ body_ang_w = articulation.data.body_link_ang_vel_w.torch
+ if articulation.is_fixed_base:
+ body_lin_w = body_lin_w[:, 1:]
+ body_ang_w = body_ang_w[:, 1:]
+
+ torch.testing.assert_close(v_pred[..., 3:6], body_ang_w, atol=1.5e-1, rtol=5e-2)
+ torch.testing.assert_close(v_pred[..., 0:3], body_lin_w, atol=1.5e-1, rtol=5e-2)
+
+
+@pytest.mark.parametrize("num_articulations", [4])
+@pytest.mark.parametrize("device", ["cuda:0"])
+@pytest.mark.parametrize("articulation_type", ["panda", "anymal"])
+@pytest.mark.parametrize("gravity_enabled", [False])
+@pytest.mark.isaacsim_ci
+def test_get_mass_matrix_symmetry_pd(sim, num_articulations, device, articulation_type, gravity_enabled):
+ """The joint-space mass matrix ``M(q)`` must be square, symmetric, and positive-definite.
+
+ Mirrors the Newton-side test in
+ ``source/isaaclab_newton/test/assets/test_articulation.py``. Pins
+ three structural properties of :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`
+ that every backend must satisfy. Both backends include the 6 floating-base
+ rows/cols on floating-base assets (matching the cross-library industry
+ convention); this test cares about square + symmetric + PD across both
+ fixed- and floating-base, not the absolute column count.
+ """
+ articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type)
+ articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device)
+ sim.reset()
+ assert articulation.is_initialized
+
+ sim.step()
+ articulation.update(sim.cfg.dt)
+
+ M = articulation.data.mass_matrix.torch # (N, J, J)
+ assert M.dim() == 3, f"expected 3-D mass matrix, got shape {tuple(M.shape)}"
+ assert M.shape[0] == num_articulations
+ assert M.shape[1] == M.shape[2], f"mass matrix is not square: {tuple(M.shape)}"
+
+ asym = (M - M.transpose(-1, -2)).abs().max().item()
+ assert asym < 1e-4, f"|M - M^T|_max = {asym:.3e} — mass matrix is not symmetric"
+
+ eye = torch.eye(M.shape[-1], device=M.device, dtype=M.dtype).expand_as(M)
+ torch.linalg.cholesky(M + 1e-6 * eye)
+
+
+@pytest.mark.parametrize("num_articulations", [1])
+@pytest.mark.parametrize("device", ["cuda:0"])
+@pytest.mark.parametrize("articulation_type", ["panda", "anymal"])
+@pytest.mark.parametrize("gravity_enabled", [False])
+@pytest.mark.isaacsim_ci
+def test_jacobian_refreshes_after_manual_joint_write(
+ sim, num_articulations, device, articulation_type, gravity_enabled
+):
+ """After ``write_joint_position_to_sim_index`` (no sim step), the Jacobian read
+ must reflect the new joint state — not the previous one.
+
+ PhysX-side counterpart to the Newton test of the same name. PhysX's
+ :attr:`body_link_jacobian_w` triggers FK indirectly through
+ :attr:`body_link_pose_w` (used by the shift kernel); :attr:`body_com_jacobian_w` is
+ a passthrough to ``_root_view.get_jacobians()``. This test confirms that PhysX's
+ tensor view returns up-to-date Jacobians after a manual joint write — i.e., that
+ PhysX internally refreshes FK on ``get_jacobians`` (or that our property does).
+ Failure means we need to add ``update_articulations_kinematic()`` before the
+ passthrough.
+ """
+ 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)
+
+ # Read J at the baseline joint state.
+ J_link_0 = articulation.data.body_link_jacobian_w.torch.clone()
+ J_com_0 = articulation.data.body_com_jacobian_w.torch.clone()
+
+ # Manually write a different joint state — large delta to make the change visible.
+ # No sim.step / update — FK becomes stale.
+ 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)
+
+ # Read J again. With the FK trigger, J reflects q_target and differs from J at baseline.
+ # Without the trigger, body_q stays at baseline, J unchanged.
+ J_link_1 = articulation.data.body_link_jacobian_w.torch.clone()
+ J_com_1 = articulation.data.body_com_jacobian_w.torch.clone()
+
+ assert not torch.allclose(J_link_0, J_link_1, atol=1e-3), (
+ "body_link_jacobian_w did not change after manual joint write — "
+ "FK trigger likely missing (eval_jacobian / shift kernel reading stale state.body_q)."
+ )
+ assert not torch.allclose(J_com_0, J_com_1, atol=1e-3), (
+ "body_com_jacobian_w did not change after manual joint write — "
+ "PhysX get_jacobians may not auto-refresh FK; consider adding update_articulations_kinematic()."
+ )
+
+
+@pytest.mark.parametrize("num_articulations", [1])
+@pytest.mark.parametrize("device", ["cuda:0"])
+@pytest.mark.parametrize("articulation_type", ["panda", "anymal"])
+@pytest.mark.parametrize("gravity_enabled", [False])
+@pytest.mark.isaacsim_ci
+def test_mass_matrix_refreshes_after_manual_joint_write(
+ sim, num_articulations, device, articulation_type, gravity_enabled
+):
+ """After ``write_joint_position_to_sim_index`` (no sim step), the mass matrix read
+ must reflect the new joint state.
+
+ PhysX-side counterpart. :attr:`mass_matrix` is a passthrough to
+ ``_root_view.get_generalized_mass_matrices()``. Failure means PhysX's tensor view
+ does not auto-refresh FK on this getter, and we need to add
+ ``update_articulations_kinematic()`` before the passthrough.
+ """
+ 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)
+
+ M_0 = articulation.data.mass_matrix.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)
+ M_1 = articulation.data.mass_matrix.torch.clone()
+
+ assert not torch.allclose(M_0, M_1, atol=1e-3), (
+ "mass_matrix did not change after manual joint write — "
+ "PhysX get_generalized_mass_matrices may not auto-refresh FK."
+ )
+
+
+@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):
+ """PhysX accuracy: ``τ_gc`` must hold the manipulator in static equilibrium.
+
+ 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.
+
+ 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.
+ """
+ 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_state_to_sim(default_q, 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`` (actuated only).
+ tau_gc = articulation.data.gravity_compensation_forces.torch[:, articulation.num_base_dofs :]
+ articulation.set_joint_effort_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])
+@pytest.mark.isaacsim_ci
+def test_franka_ik_tracking_accuracy(sim, device, articulation_type, gravity_enabled):
+ """PhysX-side IK convergence sentinel — backend parity with the Newton test.
+
+ Mirrors :func:`isaaclab_newton.test.assets.test_articulation.test_franka_ik_tracking_accuracy`
+ so both backends are pinned by the same IK trajectory. With the
+ robot teleported to its configured init_state home pose and scene
+ gravity off, PhysX's IK converges to ~mm precision on this 5 cm
+ Cartesian step. A bridge regression (wrong J shape, wrong DoF
+ ordering) would push the steady-state error well past the
+ threshold.
+ """
+ robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim)
+
+ sim.step()
+ robot.update(sim.cfg.dt)
+ target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.05, 0.0, 0.0), device)
+
+ ik = DifferentialIKController(
+ DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"),
+ num_envs=1,
+ device=device,
+ )
+ ik.set_command(target_pose_b)
+
+ pos_history: list[float] = []
+ rot_history: list[float] = []
+ for _ in range(800):
+ jacobian = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids)
+ ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx)
+ joint_pos = robot.data.joint_pos.torch[:, arm_joint_ids]
+
+ joint_pos_des = ik.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)
+
+ robot.set_joint_position_target(joint_pos_des, joint_ids=arm_joint_ids)
+ robot.write_data_to_sim()
+ sim.step()
+ robot.update(sim.cfg.dt)
+
+ pos_error, rot_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())
+ rot_history.append(rot_error.norm(dim=-1).max().item())
+
+ pos_min, pos_mean = _summarize_history(pos_history)
+ rot_min, rot_mean = _summarize_history(rot_history)
+
+ print(f"IK_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}")
+
+ # Assert on tail mean (not min) so an oscillating envelope can't
+ # squeeze through. Threshold matched to the Newton-side test
+ # (5 mm / 0.05 rad).
+ assert pos_mean < 5e-3, f"IK pos_mean {pos_mean:.5f} > 5 mm — bridge regression?"
+ assert rot_mean < 5e-2, f"IK 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", [False])
+@pytest.mark.isaacsim_ci
+def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_enabled):
+ """PhysX-side OSC pose tracking sentinel — backend parity with Newton.
+
+ Mirrors :func:`isaaclab_newton.test.assets.test_articulation.test_franka_osc_tracking_accuracy`.
+ Zero out the actuator's PD gains so OSC's joint-effort output is
+ not opposed by the implicit-PD term, matching the Newton test setup.
+ """
+ robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim, zero_actuator_pd=True)
+
+ 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)
+ target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.05, 0.0, 0.0), device)
+
+ pos_history: list[float] = []
+ rot_history: list[float] = []
+ for _ in range(800):
+ 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]
+ 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=None,
+ )
+
+ 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, rot_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())
+ rot_history.append(rot_error.norm(dim=-1).max().item())
+
+ pos_min, pos_mean = _summarize_history(pos_history)
+ rot_min, rot_mean = _summarize_history(rot_history)
+
+ print(f"OSC_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}")
+
+ # Assert on tail mean. Threshold matched to the Newton-side test
+ # (5 mm / 0.05 rad). Both backends converge to machine precision
+ # with proper ee-velocity feedback (``J · q_dot``).
+ assert pos_mean < 5e-3, f"OSC pos_mean {pos_mean:.5f} > 5 mm — bridge regression?"
+ assert rot_mean < 5e-2, f"OSC rot_mean {rot_mean:.5f} > 0.05 rad — bridge regression?"
+
+
+def _run_osc_stay_still_under_gravity(
+ sim,
+ device: str,
+ *,
+ gravity_compensation_enabled: bool,
+ num_steps: int = 100,
+):
+ """Run OSC with a stay-still target on Franka under gravity, return EE drift summary.
+
+ Shared helper for the gravity-comp tests. Setup mirrors
+ :func:`test_franka_osc_tracking_accuracy` (zero actuator PD so OSC's joint-effort
+ output is not opposed by an implicit-PD spring), but with scene gravity ON and the
+ target = the EE pose captured after the first sim step (which already includes a
+ fraction-of-a-mm of gravity-induced motion; that's the baseline drift starts from).
+
+ Args:
+ gravity_compensation_enabled: If ``True``, the OSC controller cfg has
+ ``gravity_compensation=True`` and ``osc.compute(gravity=g(q))`` receives
+ the data-layer ``gravity_compensation_forces`` slice. If ``False``,
+ ``gravity_compensation=False`` and ``gravity=None``.
+
+ Returns:
+ Tuple ``((pos_min, pos_mean), (rot_min, rot_mean))`` over the last 20% of
+ steps (per :func:`_summarize_history`), where ``pos`` is in meters and
+ ``rot`` in radians.
+ """
+ # Enable rigid-body gravity so the arm actually feels weight.
+ # ``FRANKA_PANDA_HIGH_PD_CFG`` defaults ``disable_gravity=True`` for IK/OSC tests.
+ robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(
+ sim, zero_actuator_pd=True, enable_rigid_body_gravity=True
+ )
+
+ osc = OperationalSpaceController(
+ OperationalSpaceControllerCfg(
+ target_types=["pose_abs"],
+ impedance_mode="fixed",
+ inertial_dynamics_decoupling=True,
+ partial_inertial_dynamics_decoupling=False,
+ gravity_compensation=gravity_compensation_enabled,
+ motion_stiffness_task=500.0,
+ motion_damping_ratio_task=1.0,
+ ),
+ num_envs=1,
+ device=device,
+ )
+
+ sim.step()
+ robot.update(sim.cfg.dt)
+
+ # Stay-still target = current EE pose in root frame, captured right after the
+ # first step. The OSC loop must hold this pose under gravity.
+ initial_ee_pos_b, initial_ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx)
+ target_pose_b = torch.cat([initial_ee_pos_b, initial_ee_quat_b], dim=-1)
+
+ pos_history: list[float] = []
+ rot_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]
+ 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)
+
+ # ``gravity_compensation_forces`` shape is ``(N, num_joints + num_base_dofs)``;
+ # slice past the leading floating-base columns (0 for fixed-base Franka, so a
+ # no-op here, but the pattern matches the action-term convention).
+ gravity = (
+ robot.data.gravity_compensation_forces.torch[:, [j + robot.num_base_dofs for j in arm_joint_ids]]
+ if gravity_compensation_enabled
+ else None
+ )
+
+ 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, rot_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())
+ rot_history.append(rot_error.norm(dim=-1).max().item())
+
+ return _summarize_history(pos_history), _summarize_history(rot_history)
+
+
+@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_holds_under_gravity(sim, device, articulation_type, gravity_enabled):
+ """OSC with ``gravity_compensation=True`` must hold the EE pose under gravity.
+
+ With scene gravity ON and zero actuator PD (so OSC torques are not opposed by an
+ implicit-PD spring), passing
+ :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` through
+ ``osc.compute(gravity=...)`` should keep the arm at the initial pose.
+
+ Pins three things that the existing direct-primitive
+ :func:`test_get_gravity_compensation_forces_static_equilibrium` does not:
+ 1. OSC's ``_jacobi_joint_idx`` indexing — the ``+ num_base_dofs`` shift.
+ 2. OSC's :meth:`OperationalSpaceController.compute` correctly adds ``g(q)`` to
+ its torque output.
+ 3. The data-property ``gravity_compensation_forces`` is reachable from the OSC
+ pipeline (catches gating regressions in
+ :meth:`OperationalSpaceControllerAction._compute_dynamic_quantities`).
+
+ Companion test :func:`test_franka_osc_no_gravity_compensation_sags_under_gravity`
+ runs the same setup with ``gravity_compensation=False`` and reports the
+ uncompensated drift magnitude — a sanity check that gravity is loading the arm.
+ """
+ (pos_min, pos_mean), (rot_min, rot_mean) = _run_osc_stay_still_under_gravity(
+ sim, device, gravity_compensation_enabled=True
+ )
+ print(f"OSC_GC_ON pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}")
+
+ assert pos_mean < 5e-3, f"OSC + gravity_compensation pos_mean {pos_mean:.5f} > 5 mm — regression?"
+ assert rot_mean < 5e-2, f"OSC + gravity_compensation rot_mean {rot_mean:.5f} > 0.05 rad — 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_no_gravity_compensation_sags_under_gravity(sim, device, articulation_type, gravity_enabled):
+ """OSC without ``gravity_compensation`` under gravity: sanity check that the arm sags.
+
+ Companion to :func:`test_franka_osc_gravity_compensation_holds_under_gravity`.
+ Same setup, but ``gravity_compensation=False`` and ``osc.compute(gravity=None)``.
+ With zero actuator PD, OSC's task-space impedance is the only restoring force —
+ the steady-state solution is whatever pose error the impedance produces enough
+ joint torque to balance ``g(q)``.
+
+ Asserts the drift is **non-trivially larger** than the with-comp threshold (5 mm).
+ Without this check, a regression that broke ``gravity_compensation_forces`` by
+ returning zeros (or a no-op `g(q)`) would pass the with-comp test silently. The
+ bound here proves gravity is actually loading the arm and the with-comp pass is
+ meaningful.
+ """
+ (pos_min, pos_mean), (rot_min, rot_mean) = _run_osc_stay_still_under_gravity(
+ sim, device, gravity_compensation_enabled=False
+ )
+ print(f"OSC_GC_OFF pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}")
+
+ # Sanity: with gravity on and no comp, OSC's task-space spring vs gravity-load
+ # equilibrium produces a non-zero pose error. If this asserts fails, the test
+ # setup itself is broken (e.g., gravity is not on, or the home pose has no
+ # gravity load), which would invalidate the with-comp test as well.
+ assert pos_mean > 5e-3, (
+ f"OSC + no gravity_compensation pos_mean {pos_mean:.5f} ≤ 5 mm — gravity not loading the arm?"
+ )
+
+
if __name__ == "__main__":
pytest.main([__file__, "-v", "--maxfail=1"])
diff --git a/source/isaaclab_tasks/changelog.d/jichuanh-ik-newton-compat-mvp.rst b/source/isaaclab_tasks/changelog.d/jichuanh-ik-newton-compat-mvp.rst
new file mode 100644
index 000000000000..361dbe9e52b1
--- /dev/null
+++ b/source/isaaclab_tasks/changelog.d/jichuanh-ik-newton-compat-mvp.rst
@@ -0,0 +1,13 @@
+Changed
+^^^^^^^
+
+* Removed the ``self.sim.physics = PhysxCfg(...)`` overrides from
+ ``Isaac-Reach-Franka-{IK-Abs,IK-Rel,OSC}-v0`` env configs so they
+ inherit the parent ``ReachPhysicsCfg`` preset. Selecting
+ ``presets=newton`` now picks ``NewtonCfg``; the previous
+ ``bounce_threshold_velocity=0.2`` PhysX behavior is preserved as
+ the default in ``ReachPhysicsCfg``. Direct-workflow callers in
+ ``automate``, ``factory``, and the deploy MDP events module were
+ migrated to the new
+ :class:`~isaaclab.assets.BaseArticulationData` properties
+ (:attr:`body_link_jacobian_w`, :attr:`mass_matrix`).
diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py
index 438f0f80603b..e3037d67712d 100644
--- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py
+++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py
@@ -288,12 +288,12 @@ def _compute_intermediate_values(self, dt):
self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w.torch[:, self.fingertip_body_idx]
self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w.torch[:, self.fingertip_body_idx]
- jacobians = wp.to_torch(self._robot.root_view.get_jacobians())
+ jacobians = self._robot.data.body_link_jacobian_w.torch
self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7]
self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7]
self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5
- self.arm_mass_matrix = wp.to_torch(self._robot.root_view.get_generalized_mass_matrices())[:, 0:7, 0:7]
+ self.arm_mass_matrix = self._robot.data.mass_matrix.torch[:, 0:7, 0:7]
self.joint_pos = self._robot.data.joint_pos.torch.clone()
self.joint_vel = self._robot.data.joint_vel.torch.clone()
diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py
index c79441f223ae..1982786f65a7 100644
--- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py
+++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py
@@ -215,12 +215,12 @@ def _compute_intermediate_values(self, dt):
self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w.torch[:, self.fingertip_body_idx]
self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w.torch[:, self.fingertip_body_idx]
- jacobians = wp.to_torch(self._robot.root_view.get_jacobians())
+ jacobians = self._robot.data.body_link_jacobian_w.torch
self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7]
self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7]
self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5
- self.arm_mass_matrix = wp.to_torch(self._robot.root_view.get_generalized_mass_matrices())[:, 0:7, 0:7]
+ self.arm_mass_matrix = self._robot.data.mass_matrix.torch[:, 0:7, 0:7]
self.joint_pos = self._robot.data.joint_pos.torch.clone()
self.joint_vel = self._robot.data.joint_vel.torch.clone()
diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py
index ecc1ef33a038..c38fe071b161 100644
--- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py
+++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py
@@ -5,7 +5,6 @@
import numpy as np
import torch
-import warp as wp
import carb
@@ -131,12 +130,12 @@ def _compute_intermediate_values(self, dt):
self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w.torch[:, self.fingertip_body_idx]
self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w.torch[:, self.fingertip_body_idx]
- jacobians = wp.to_torch(self._robot.root_view.get_jacobians())
+ jacobians = self._robot.data.body_link_jacobian_w.torch
self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7]
self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7]
self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5
- self.arm_mass_matrix = wp.to_torch(self._robot.root_view.get_generalized_mass_matrices())[:, 0:7, 0:7]
+ self.arm_mass_matrix = self._robot.data.mass_matrix.torch[:, 0:7, 0:7]
self.joint_pos = self._robot.data.joint_pos.torch.clone()
self.joint_vel = self._robot.data.joint_vel.torch.clone()
diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/events.py
index b651a002966e..0156d486d1f0 100644
--- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/events.py
+++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/events.py
@@ -11,7 +11,6 @@
from typing import TYPE_CHECKING
import torch
-import warp as wp
import isaaclab.utils.math as math_utils
from isaaclab.managers import EventTermCfg, ManagerTermBase, SceneEntityCfg
@@ -327,9 +326,11 @@ def __call__(
if torch.all(pos_error_norm < pos_threshold) and torch.all(rot_error_norm < rot_threshold):
break
- # Solve IK using jacobian
- jacobians = wp.to_torch(self.robot_asset.root_view.get_jacobians()).clone()
- jacobian = jacobians[env_ids, self.jacobi_body_idx, :, :]
+ # Solve IK using jacobian. ``body_link_jacobian_w`` prepends ``num_base_dofs``
+ # floating-base columns on the DoF axis (0 for fixed-base, 6 for floating-base);
+ # slice past them so the column axis aligns with the actuated-joint state.
+ jacobians = self.robot_asset.data.body_link_jacobian_w.torch.clone()
+ jacobian = jacobians[env_ids, self.jacobi_body_idx, :, self.robot_asset.num_base_dofs :]
delta_dof_pos = fc._get_delta_dof_pos(
delta_pose=delta_hand_pose,
diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py
index e8e955718559..b090e568965e 100644
--- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py
+++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py
@@ -3,8 +3,6 @@
#
# SPDX-License-Identifier: BSD-3-Clause
-from isaaclab_physx.physics import PhysxCfg
-
from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg
from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg
from isaaclab.utils import configclass
@@ -36,9 +34,6 @@ def __post_init__(self):
body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]),
)
- # IK control is not supported with Newton physics; use PhysX only.
- self.sim.physics = PhysxCfg(bounce_threshold_velocity=0.2)
-
@configclass
class FrankaReachEnvCfg_PLAY(FrankaReachEnvCfg):
diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py
index 488e92493289..024a42270d85 100644
--- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py
+++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py
@@ -3,8 +3,6 @@
#
# SPDX-License-Identifier: BSD-3-Clause
-from isaaclab_physx.physics import PhysxCfg
-
from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg
from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg
from isaaclab.utils import configclass
@@ -37,9 +35,6 @@ def __post_init__(self):
body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]),
)
- # IK control is not supported with Newton physics; use PhysX only.
- self.sim.physics = PhysxCfg(bounce_threshold_velocity=0.2)
-
@configclass
class FrankaReachEnvCfg_PLAY(FrankaReachEnvCfg):
diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py
index cca92aa019bb..e612439fda70 100644
--- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py
+++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py
@@ -3,8 +3,6 @@
#
# SPDX-License-Identifier: BSD-3-Clause
-from isaaclab_physx.physics import PhysxCfg
-
from isaaclab.controllers.operational_space_cfg import OperationalSpaceControllerCfg
from isaaclab.envs.mdp.actions.actions_cfg import OperationalSpaceControllerActionCfg
from isaaclab.utils import configclass
@@ -62,9 +60,6 @@ def __post_init__(self):
self.observations.policy.joint_pos = None
self.observations.policy.joint_vel = None
- # OSC control is not supported with Newton physics; use PhysX only.
- self.sim.physics = PhysxCfg(bounce_threshold_velocity=0.2)
-
@configclass
class FrankaReachEnvCfg_PLAY(FrankaReachEnvCfg):