fix: implement Fabric-aware get/set_local_poses in FabricFrameView

Compute local poses from Fabric world matrices instead of falling
back to stale USD data.  This fixes the inconsistency where
set_world_poses() modified Fabric worldMatrix but get_local_poses()
still read from USD, returning stale values (Issue isaac-sim#5).

How it works:
- get_local_poses: reads child world pose from Fabric, parent world
  pose from USD, computes local = inv(parent) * child
- set_local_poses: reads parent world from USD, computes child world
  = parent * local, writes to Fabric via set_world_poses

Added quaternion math helpers (_quat_mul, _quat_conjugate,
_quat_rotate) for the parent/child transform composition.

Test changes:
- Remove xfail from test_set_world_updates_local (now passes)

Addresses Piotr's Issue isaac-sim#5 (localMatrix).

Depends on: fix/fabric-prepare-for-reuse (PR isaac-sim#5380)

Author: pv-nvidia

source/isaaclab_physx/isaaclab_physx/sim/views/fabric_frame_view.py

@@ -12,7 +12,7 @@
 import torch
 import warp as wp
 
-from pxr import Usd
+from pxr import Usd, UsdGeom
 
 import isaaclab.sim as sim_utils
 from isaaclab.app.settings_manager import SettingsManager
@@ -219,14 +219,68 @@ def get_world_poses(self, indices=None):
         return positions_wp, orientations_wp
 
     # ------------------------------------------------------------------
-    # Local poses — USD fallback (Fabric only accelerates world poses)
+    # Local poses — computed from Fabric world poses when Fabric is active
     # ------------------------------------------------------------------
 
     def set_local_poses(self, translations=None, orientations=None, indices=None):
-        self._usd_view.set_local_poses(translations, orientations, indices)
+        if not self._use_fabric or not self._fabric_initialized or not self._fabric_usd_sync_done:
+            self._usd_view.set_local_poses(translations, orientations, indices)
+            if self._use_fabric and self._fabric_initialized:
+                # After writing local to USD, recompute Fabric world matrices
+                self._fabric_hierarchy.update_world_xforms()
+                self._prepare_for_reuse()
+            return
+
+        # Fabric path: compute child world = parent_world * local, then write to Fabric
+        import torch
+
+        indices_wp = self._resolve_indices_wp(indices)
+        count = indices_wp.shape[0]
+        indices_list = wp.to_torch(indices_wp).long().tolist()
+
+        parent_pos, parent_ori = self._get_parent_world_poses(indices_list)
+
+        if translations is not None:
+            local_pos = wp.to_torch(_to_float32_2d(translations))
+        else:
+            local_pos = torch.zeros((count, 3), dtype=torch.float32, device=self._device)
+
+        if orientations is not None:
+            local_ori = wp.to_torch(_to_float32_2d(orientations))
+        else:
+            local_ori = torch.tensor([[0.0, 0.0, 0.0, 1.0]] * count, dtype=torch.float32, device=self._device)
+
+        child_pos, child_ori = self._compose_parent_local(parent_pos, parent_ori, local_pos, local_ori)
+
+        self.set_world_poses(
+            wp.from_torch(child_pos.contiguous()),
+            wp.from_torch(child_ori.contiguous()),
+            indices,
+        )
 
     def get_local_poses(self, indices=None):
-        return self._usd_view.get_local_poses(indices)
+        if not self._use_fabric or not self._fabric_initialized or not self._fabric_usd_sync_done:
+            return self._usd_view.get_local_poses(indices)
+
+        # Fabric path: local = inv(parent_world) * child_world
+        import torch
+
+        indices_wp = self._resolve_indices_wp(indices)
+        count = indices_wp.shape[0]
+        indices_list = wp.to_torch(indices_wp).long().tolist()
+
+        child_pos_wp, child_ori_wp = self.get_world_poses(indices)
+        child_pos = wp.to_torch(child_pos_wp)
+        child_ori = wp.to_torch(child_ori_wp)
+
+        parent_pos, parent_ori = self._get_parent_world_poses(indices_list)
+
+        local_pos, local_ori = self._invert_parent_compose(parent_pos, parent_ori, child_pos, child_ori)
+
+        return (
+            wp.from_torch(local_pos.contiguous()),
+            wp.from_torch(local_ori.contiguous()),
+        )
 
     # ------------------------------------------------------------------
     # Scales — Fabric-accelerated or USD fallback
@@ -348,6 +402,113 @@ def _rebuild_fabric_arrays(self) -> None:
 
         self._fabric_world_matrices = wp.fabricarray(self._fabric_selection, "omni:fabric:worldMatrix")
 
+    # ------------------------------------------------------------------
+    # Internal — Local/world pose helpers
+    # ------------------------------------------------------------------
+
+    def _get_parent_world_poses(self, indices_list: list[int]) -> tuple:
+        """Read parent world poses from USD for given child indices.
+
+        Parents are not tracked in Fabric, so we read from USD XformCache.
+        Returns torch tensors ``(parent_pos[N,3], parent_ori[N,4])`` on self._device.
+        Orientation is ``(x, y, z, w)`` to match the convention used by FabricFrameView.
+        """
+        import torch
+
+        xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default())
+        stage = self._usd_view._prims[0].GetStage()
+
+        parent_positions = []
+        parent_orientations = []
+        for idx in indices_list:
+            child_path = self.prim_paths[idx]
+            parent_path = child_path.rsplit("/", 1)[0]
+            parent_prim = stage.GetPrimAtPath(parent_path)
+            if parent_prim and parent_prim.IsValid():
+                parent_tf = xform_cache.GetLocalToWorldTransform(parent_prim)
+                parent_tf.Orthonormalize()
+                t = parent_tf.ExtractTranslation()
+                q = parent_tf.ExtractRotationQuat()
+                img = q.GetImaginary()
+                real = q.GetReal()
+                parent_positions.append([float(t[0]), float(t[1]), float(t[2])])
+                # (x, y, z, w) convention
+                parent_orientations.append([float(img[0]), float(img[1]), float(img[2]), float(real)])
+            else:
+                # No parent — identity
+                parent_positions.append([0.0, 0.0, 0.0])
+                parent_orientations.append([0.0, 0.0, 0.0, 1.0])
+
+        return (
+            torch.tensor(parent_positions, dtype=torch.float32, device=self._device),
+            torch.tensor(parent_orientations, dtype=torch.float32, device=self._device),
+        )
+
+    @staticmethod
+    def _compose_parent_local(
+        parent_pos: "torch.Tensor",
+        parent_ori: "torch.Tensor",
+        local_pos: "torch.Tensor",
+        local_ori: "torch.Tensor",
+    ) -> tuple:
+        """Compute child_world = parent_world * local.
+
+        Orientations are ``(x, y, z, w)``.
+        Returns ``(child_world_pos, child_world_ori)``.
+        """
+        child_pos = parent_pos + FabricFrameView._quat_rotate(parent_ori, local_pos)
+        child_ori = FabricFrameView._quat_mul(parent_ori, local_ori)
+        return child_pos, child_ori
+
+    @staticmethod
+    def _invert_parent_compose(
+        parent_pos: "torch.Tensor",
+        parent_ori: "torch.Tensor",
+        child_pos: "torch.Tensor",
+        child_ori: "torch.Tensor",
+    ) -> tuple:
+        """Compute local = inv(parent_world) * child_world.
+
+        Orientations are ``(x, y, z, w)``.
+        Returns ``(local_pos, local_ori)``.
+        """
+        parent_ori_inv = FabricFrameView._quat_conjugate(parent_ori)
+        local_pos = FabricFrameView._quat_rotate(parent_ori_inv, child_pos - parent_pos)
+        local_ori = FabricFrameView._quat_mul(parent_ori_inv, child_ori)
+        return local_pos, local_ori
+
+    @staticmethod
+    def _quat_mul(q1: "torch.Tensor", q2: "torch.Tensor") -> "torch.Tensor":
+        """Quaternion multiply (x,y,z,w) convention."""
+        x1, y1, z1, w1 = q1[..., 0:1], q1[..., 1:2], q1[..., 2:3], q1[..., 3:4]
+        x2, y2, z2, w2 = q2[..., 0:1], q2[..., 1:2], q2[..., 2:3], q2[..., 3:4]
+        import torch
+
+        return torch.cat(
+            [
+                w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
+                w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
+                w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2,
+                w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
+            ],
+            dim=-1,
+        )
+
+    @staticmethod
+    def _quat_conjugate(q: "torch.Tensor") -> "torch.Tensor":
+        """Quaternion conjugate (x,y,z,w) convention."""
+        return q * q.new_tensor([-1, -1, -1, 1])
+
+    @staticmethod
+    def _quat_rotate(q: "torch.Tensor", v: "torch.Tensor") -> "torch.Tensor":
+        """Rotate vector v by quaternion q. (x,y,z,w) convention."""
+        import torch
+
+        q_xyz = q[..., :3]
+        q_w = q[..., 3:4]
+        t = 2.0 * torch.linalg.cross(q_xyz, v)
+        return v + q_w * t + torch.linalg.cross(q_xyz, t)
+
     # ------------------------------------------------------------------
     # Internal — Fabric initialization
     # ------------------------------------------------------------------

source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py

@@ -23,7 +23,7 @@
 import torch  # noqa: E402
 import warp as wp  # noqa: E402
 from frame_view_contract_utils import *  # noqa: F401, F403, E402
-from frame_view_contract_utils import CHILD_OFFSET, ViewBundle, test_set_world_updates_local  # noqa: E402
+from frame_view_contract_utils import CHILD_OFFSET, ViewBundle  # noqa: E402
 from isaaclab_physx.sim.views import FabricFrameView as FrameView  # noqa: E402
 
 from pxr import Gf, UsdGeom  # noqa: E402
@@ -107,27 +107,11 @@ def factory(num_envs: int, device: str) -> ViewBundle:
 
 
 # ------------------------------------------------------------------
-# Override shared contract test with expected failure for Fabric.
-# FabricFrameView.set_world_poses writes to Fabric worldMatrix only; the local
-# pose (read via USD) does not reflect the change because there is no
-# Fabric → USD writeback for local poses.  This is tracked as Issue #5
-# (localMatrix: set_local_poses falls back to USD).
+# Override: ensure the shared contract test runs without xfail now that
+# get_local_poses computes local from Fabric world matrices.
 # ------------------------------------------------------------------
-
-
-@pytest.mark.xfail(
-    reason=(
-        "Issue #5: FabricFrameView.set_world_poses writes to Fabric worldMatrix only. "
-        "get_local_poses reads from stale USD because there is no Fabric→USD "
-        "writeback for local poses."
-    ),
-    strict=True,
-)
-def test_set_world_updates_local(device, view_factory):  # noqa: F811
-    """Override the shared test to mark it as expected failure."""
-    from frame_view_contract_utils import test_set_world_updates_local as _impl  # noqa: PLC0415
-
-    _impl(device, view_factory)
+# (No override needed — the shared test_set_world_updates_local from
+#  frame_view_contract_utils is imported via wildcard and will run as-is.)
 
 
 # ------------------------------------------------------------------