Fix contact sensor activation for nested rigid bodies (isaac-sim#6259)

## Summary
- use `get_all_matching_child_prims` when activating PhysX contact
report schemas
- continue matching rigid descendants under nested rigid-body trees
instead of stopping at the first rigid body
- add regression coverage for nested rigid bodies receiving
`PhysxContactReportAPI`

## Testing
- ./isaaclab.sh -p -m pytest source/isaaclab/test/sim/test_schemas.py -k
activate_contact_sensors_nested_rigid_bodies -q
- ./isaaclab.sh -p -m py_compile
source/isaaclab/isaaclab/sim/schemas/schemas.py
source/isaaclab/test/sim/test_schemas.py

## Note
- Attempted `./isaaclab.sh -p -m pytest
source/isaaclab_physx/test/sensors/test_contact_sensor.py -k
contact_sensor_threshold -q`, but collection failed because `flaky` is
not installed in the local environment.

Author: ooctipus

source/isaaclab/changelog.d/fix-activate-contact-sensors-nested-rigid-bodies.rst

@@ -0,0 +1,4 @@
+Fixed
+^^^^^
+
+* Fixed contact sensor activation for rigid bodies nested below other rigid bodies.

source/isaaclab/isaaclab/sim/schemas/schemas.py

@@ -933,33 +933,25 @@ def activate_contact_sensors(prim_path: str, threshold: float = 0.0, stage: Usd.
     # check if prim is valid
     if not prim.IsValid():
         raise ValueError(f"Prim path '{prim_path}' is not valid.")
-    # iterate over all children
-    num_contact_sensors = 0
-    all_prims = [prim]
-    while len(all_prims) > 0:
-        # get current prim
-        child_prim = all_prims.pop(0)
-        # check if prim is a rigid body
-        # nested rigid bodies are not allowed by SDK so we can safely assume that
-        # if a prim has a rigid body API, it is a rigid body and we don't need to
-        # check its children
-        if child_prim.HasAPI(UsdPhysics.RigidBodyAPI):
-            # set sleep threshold to zero
-            child_applied = child_prim.GetAppliedSchemas()
-            if "PhysxRigidBodyAPI" not in child_applied:
-                child_prim.AddAppliedSchema("PhysxRigidBodyAPI")
-            safe_set_attribute_on_usd_prim(child_prim, "physxRigidBody:sleepThreshold", 0.0, camel_case=False)
-            # add contact report API with threshold of zero
-            if "PhysxContactReportAPI" not in child_applied:
-                child_prim.AddAppliedSchema("PhysxContactReportAPI")
-            safe_set_attribute_on_usd_prim(child_prim, "physxContactReport:threshold", threshold, camel_case=False)
-            # increment number of contact sensors
-            num_contact_sensors += 1
-        else:
-            # add all children to tree
-            all_prims += child_prim.GetChildren()
+    # collect all rigid bodies under the prim, including nested rigid-body trees
+    rigid_body_prims = get_all_matching_child_prims(
+        prim_path,
+        predicate=lambda child_prim: child_prim.HasAPI(UsdPhysics.RigidBodyAPI),
+        stage=stage,
+        traverse_instance_prims=False,
+    )
+    for child_prim in rigid_body_prims:
+        # set sleep threshold to zero
+        child_applied = child_prim.GetAppliedSchemas()
+        if "PhysxRigidBodyAPI" not in child_applied:
+            child_prim.AddAppliedSchema("PhysxRigidBodyAPI")
+        safe_set_attribute_on_usd_prim(child_prim, "physxRigidBody:sleepThreshold", 0.0, camel_case=False)
+        # add contact report API with threshold of zero
+        if "PhysxContactReportAPI" not in child_applied:
+            child_prim.AddAppliedSchema("PhysxContactReportAPI")
+        safe_set_attribute_on_usd_prim(child_prim, "physxContactReport:threshold", threshold, camel_case=False)
     # check if no contact sensors were found
-    if num_contact_sensors == 0:
+    if not rigid_body_prims:
         descendant_count = 0
         frontier = [prim]
         while frontier:

source/isaaclab/test/sim/test_schemas.py

@@ -752,6 +752,33 @@ def test_modify_properties_on_articulation_usd(setup_simulation):
     _validate_articulation_properties_on_prim("/World/asset", arti_cfg, True)
 
 
+@pytest.mark.isaacsim_ci
+def test_activate_contact_sensors_nested_rigid_bodies(setup_simulation):
+    """Test contact-report schemas are applied to nested rigid-body trees."""
+    stage = sim_utils.get_current_stage()
+
+    rigid_body_paths = [
+        "/World/Robot/Geometry/pelvis",
+        "/World/Robot/Geometry/pelvis/left_hip",
+        "/World/Robot/Geometry/pelvis/left_hip/left_knee",
+    ]
+    sim_utils.create_prim("/World/Robot", prim_type="Xform")
+    sim_utils.create_prim("/World/Robot/Geometry", prim_type="Xform")
+    for prim_path in rigid_body_paths:
+        sim_utils.create_prim(prim_path, prim_type="Xform")
+        UsdPhysics.RigidBodyAPI.Apply(stage.GetPrimAtPath(prim_path))
+
+    schemas.activate_contact_sensors("/World/Robot", threshold=2.5)
+
+    for prim_path in rigid_body_paths:
+        prim = stage.GetPrimAtPath(prim_path)
+        applied_schemas = prim.GetAppliedSchemas()
+        assert "PhysxRigidBodyAPI" in applied_schemas
+        assert "PhysxContactReportAPI" in applied_schemas
+        assert prim.GetAttribute("physxRigidBody:sleepThreshold").Get() == pytest.approx(0.0)
+        assert prim.GetAttribute("physxContactReport:threshold").Get() == pytest.approx(2.5)
+
+
 @pytest.mark.isaacsim_ci
 def test_defining_rigid_body_properties_on_prim(setup_simulation):
     """Test defining rigid body properties on a prim."""