Fixes FrameTransformer to handle same body names at different hierarchy levels

source/isaaclab/config/extension.toml

@@ -1,7 +1,7 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.50.5"
+version = "0.50.6"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"

source/isaaclab/docs/CHANGELOG.rst

@@ -1,6 +1,17 @@
 Changelog
 ---------
 
+0.50.6 (2025-12-07)
+~~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed FrameTransformer body name collision when tracking bodies with the same name but different hierarchical paths
+  (e.g., Robot/left_hand vs Robot_1/left_hand). The sensor now uses the relative prim path as the unique body identifier
+  instead of body name.
+
+
 0.50.5 (2025-12-15)
 ~~~~~~~~~~~~~~~~~~~
 

source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py

@@ -35,6 +35,32 @@
 # import logger
 logger = logging.getLogger(__name__)
 
+# Regex pattern for environment path parsing
+# Matches: env_0/, env_.*/, etc. and captures the path after
+_ENV_PATH_PATTERN = re.compile(r"env_[^/]+/(.*)")
+
+
+def _get_relative_body_path(prim_path: str) -> str:
+    """Extract the relative body path from a prim path.
+
+    Examples:
+    - '/World/envs/env_0/Robot/torso' -> 'Robot/torso'
+    - '/World/envs/env_123/Robot/left_hand' -> 'Robot/left_hand'
+
+    Args:
+        prim_path: The full prim path. Must contain env_* pattern.
+
+    Returns:
+        The relative body path after the env prefix.
+
+    Raises:
+        ValueError: If the prim path does not contain the expected env pattern.
+    """
+    match = _ENV_PATH_PATTERN.search(prim_path)
+    if match is None:
+        raise ValueError(f"Prim path '{prim_path}' does not match expected pattern 'env_*/...'")
+    return match.group(1)
+
 
 class FrameTransformer(SensorBase):
     """A sensor for reporting frame transforms.
@@ -201,10 +227,10 @@ def _initialize_impl(self):
                         " rigid body. The class only supports transformations between rigid bodies."
                     )
 
-                # Get the name of the body
-                body_name = matching_prim_path.rsplit("/", 1)[-1]
-                # Use body name if frame isn't specified by user
-                frame_name = frame if frame is not None else body_name
+                # Get the name of the body: use relative prim path for unique identification
+                body_name = _get_relative_body_path(matching_prim_path)
+                # Use leaf name of prim path if frame name isn't specified by user
+                frame_name = frame if frame is not None else matching_prim_path.rsplit("/", 1)[-1]
 
                 # Keep track of which frames are associated with which bodies
                 if body_name in body_names_to_frames:
@@ -291,11 +317,11 @@ def extract_env_num_and_prim_path(item: str) -> tuple[int, str]:
             self._per_env_indices = [index for index, _ in sorted(enumerate(all_prim_paths), key=lambda x: x[1])]
             sorted_prim_paths = [all_prim_paths[index] for index in self._per_env_indices]
 
-        # -- target frames
-        self._target_frame_body_names = [prim_path.split("/")[-1] for prim_path in sorted_prim_paths]
+        # -- target frames: use relative prim path for unique identification
+        self._target_frame_body_names = [_get_relative_body_path(prim_path) for prim_path in sorted_prim_paths]
 
-        # -- source frame
-        self._source_frame_body_name = self.cfg.prim_path.split("/")[-1]
+        # -- source frame: use relative prim path for unique identification
+        self._source_frame_body_name = _get_relative_body_path(self.cfg.prim_path)
         source_frame_index = self._target_frame_body_names.index(self._source_frame_body_name)
 
         # Only remove source frame from tracked bodies if it is not also a target frame

source/isaaclab/test/sensors/test_frame_transformer.py

@@ -589,3 +589,207 @@ def test_sensor_print(sim):
     sim.reset()
     # print info
     print(scene.sensors["frame_transformer"])
+
+
+@pytest.mark.isaacsim_ci
+@pytest.mark.parametrize("source_robot", ["Robot", "Robot_1"])
+def test_frame_transformer_duplicate_body_names(sim, source_robot):
+    """Test tracking bodies with same leaf name at different hierarchy levels.
+
+    This test verifies that bodies with the same leaf name but different paths
+    (e.g., Robot/LF_SHANK vs Robot_1/LF_SHANK, or arm/link vs leg/link) are tracked
+    separately using their full relative paths internally.
+
+    The test uses 4 target frames to cover both scenarios:
+
+    Explicit frame names (recommended when bodies share the same leaf name):
+        User provides unique names like "Robot_LF_SHANK" and "Robot_1_LF_SHANK" to
+        distinguish between bodies at different hierarchy levels. This makes it
+        easy to identify which transform belongs to which body.
+
+    Implicit frame names (backward compatibility):
+        When no name is provided, it defaults to the leaf body name (e.g., "RF_SHANK").
+        This preserves backward compatibility for users who may have existing code like
+        `idx = target_frame_names.index("RF_SHANK")`. However, when multiple bodies share
+        the same leaf name, this results in duplicate frame names. The transforms are
+        still distinct because internal body tracking uses full relative paths.
+
+    Args:
+        source_robot: The robot to use as the source frame ("Robot" or "Robot_1").
+                      This tests that both source frames work correctly when there are
+                      duplicate body names.
+    """
+
+    # Create a custom scene config with two robots
+    @configclass
+    class MultiRobotSceneCfg(InteractiveSceneCfg):
+        """Scene with two robots having bodies with same names."""
+
+        terrain = TerrainImporterCfg(prim_path="/World/ground", terrain_type="plane")
+
+        # First robot
+        robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
+
+        # Second robot (same type, different prim path)
+        robot_1 = ANYMAL_C_CFG.replace(
+            prim_path="{ENV_REGEX_NS}/Robot_1",
+            init_state=ANYMAL_C_CFG.init_state.replace(pos=(2.0, 0.0, 0.6)),
+        )
+
+        # Frame transformer will be set after config creation (needs source_robot parameter)
+        frame_transformer: FrameTransformerCfg = None  # type: ignore
+
+    # Spawn things into stage
+    scene_cfg = MultiRobotSceneCfg(num_envs=2, env_spacing=10.0, lazy_sensor_update=False)
+
+    # Frame transformer tracking same-named bodies from both robots
+    # Source frame is parametrized to test both Robot/base and Robot_1/base
+    scene_cfg.frame_transformer = FrameTransformerCfg(
+        prim_path=f"{{ENV_REGEX_NS}}/{source_robot}/base",
+        target_frames=[
+            # Explicit frame names (recommended when bodies share the same leaf name)
+            FrameTransformerCfg.FrameCfg(
+                name="Robot_LF_SHANK",
+                prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK",
+            ),
+            FrameTransformerCfg.FrameCfg(
+                name="Robot_1_LF_SHANK",
+                prim_path="{ENV_REGEX_NS}/Robot_1/LF_SHANK",
+            ),
+            # Implicit frame names (backward compatibility)
+            FrameTransformerCfg.FrameCfg(
+                prim_path="{ENV_REGEX_NS}/Robot/RF_SHANK",
+            ),
+            FrameTransformerCfg.FrameCfg(
+                prim_path="{ENV_REGEX_NS}/Robot_1/RF_SHANK",
+            ),
+        ],
+    )
+    scene = InteractiveScene(scene_cfg)
+
+    # Play the simulator
+    sim.reset()
+
+    # Get target frame names
+    target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names
+
+    # Verify explicit frame names are present
+    assert "Robot_LF_SHANK" in target_frame_names, f"Expected 'Robot_LF_SHANK', got {target_frame_names}"
+    assert "Robot_1_LF_SHANK" in target_frame_names, f"Expected 'Robot_1_LF_SHANK', got {target_frame_names}"
+
+    # Without explicit names, both RF_SHANK frames default to same name "RF_SHANK"
+    # This results in duplicate frame names (expected behavior for backwards compatibility)
+    rf_shank_count = target_frame_names.count("RF_SHANK")
+    assert rf_shank_count == 2, f"Expected 2 'RF_SHANK' entries (name collision), got {rf_shank_count}"
+
+    # Get indices for explicit named frames
+    robot_lf_idx = target_frame_names.index("Robot_LF_SHANK")
+    robot_1_lf_idx = target_frame_names.index("Robot_1_LF_SHANK")
+
+    # Get indices for implicit named frames (both named "RF_SHANK")
+    rf_shank_indices = [i for i, name in enumerate(target_frame_names) if name == "RF_SHANK"]
+    assert len(rf_shank_indices) == 2, f"Expected 2 RF_SHANK indices, got {rf_shank_indices}"
+
+    # Acquire ground truth body indices
+    robot_base_body_idx = scene.articulations["robot"].find_bodies("base")[0][0]
+    robot_1_base_body_idx = scene.articulations["robot_1"].find_bodies("base")[0][0]
+    robot_lf_shank_body_idx = scene.articulations["robot"].find_bodies("LF_SHANK")[0][0]
+    robot_1_lf_shank_body_idx = scene.articulations["robot_1"].find_bodies("LF_SHANK")[0][0]
+    robot_rf_shank_body_idx = scene.articulations["robot"].find_bodies("RF_SHANK")[0][0]
+    robot_1_rf_shank_body_idx = scene.articulations["robot_1"].find_bodies("RF_SHANK")[0][0]
+
+    # Determine expected source frame based on parameter
+    expected_source_robot = "robot" if source_robot == "Robot" else "robot_1"
+    expected_source_base_body_idx = robot_base_body_idx if source_robot == "Robot" else robot_1_base_body_idx
+
+    # Define simulation stepping
+    sim_dt = sim.get_physics_dt()
+
+    # Simulate physics
+    for count in range(20):
+        # Reset periodically
+        if count % 10 == 0:
+            # Reset robot
+            root_state = scene.articulations["robot"].data.default_root_state.clone()
+            root_state[:, :3] += scene.env_origins
+            scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7])
+            scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:])
+            scene.articulations["robot"].write_joint_state_to_sim(
+                scene.articulations["robot"].data.default_joint_pos,
+                scene.articulations["robot"].data.default_joint_vel,
+            )
+            # Reset robot_1
+            root_state_1 = scene.articulations["robot_1"].data.default_root_state.clone()
+            root_state_1[:, :3] += scene.env_origins
+            scene.articulations["robot_1"].write_root_pose_to_sim(root_state_1[:, :7])
+            scene.articulations["robot_1"].write_root_velocity_to_sim(root_state_1[:, 7:])
+            scene.articulations["robot_1"].write_joint_state_to_sim(
+                scene.articulations["robot_1"].data.default_joint_pos,
+                scene.articulations["robot_1"].data.default_joint_vel,
+            )
+            scene.reset()
+
+        # Write data to sim
+        scene.write_data_to_sim()
+        # Perform step
+        sim.step()
+        # Read data from sim
+        scene.update(sim_dt)
+
+        # Get frame transformer data
+        frame_transformer_data = scene.sensors["frame_transformer"].data
+        source_pos_w = frame_transformer_data.source_pos_w
+        source_quat_w = frame_transformer_data.source_quat_w
+        target_pos_w = frame_transformer_data.target_pos_w
+
+        # Get ground truth positions and orientations (after scene.update() so they're current)
+        robot_lf_pos_w = scene.articulations["robot"].data.body_pos_w[:, robot_lf_shank_body_idx]
+        robot_1_lf_pos_w = scene.articulations["robot_1"].data.body_pos_w[:, robot_1_lf_shank_body_idx]
+        robot_rf_pos_w = scene.articulations["robot"].data.body_pos_w[:, robot_rf_shank_body_idx]
+        robot_1_rf_pos_w = scene.articulations["robot_1"].data.body_pos_w[:, robot_1_rf_shank_body_idx]
+
+        # Get expected source frame positions and orientations (after scene.update() so they're current)
+        expected_source_base_pos_w = scene.articulations[expected_source_robot].data.body_pos_w[
+            :, expected_source_base_body_idx
+        ]
+        expected_source_base_quat_w = scene.articulations[expected_source_robot].data.body_quat_w[
+            :, expected_source_base_body_idx
+        ]
+
+        # TEST 1: Verify source frame is correctly resolved
+        # The source_pos_w should match the expected source robot's base world position
+        torch.testing.assert_close(source_pos_w, expected_source_base_pos_w, rtol=1e-5, atol=1e-5)
+        torch.testing.assert_close(source_quat_w, expected_source_base_quat_w, rtol=1e-5, atol=1e-5)
+
+        # TEST 2: Explicit named frames (LF_SHANK) should have DIFFERENT world positions
+        lf_pos_difference = torch.norm(target_pos_w[:, robot_lf_idx] - target_pos_w[:, robot_1_lf_idx], dim=-1)
+        assert torch.all(lf_pos_difference > 1.0), (
+            f"Robot_LF_SHANK and Robot_1_LF_SHANK should have different positions (got diff={lf_pos_difference}). "
+            "This indicates body name collision bug."
+        )
+
+        # Verify explicit named frames match correct robot bodies
+        torch.testing.assert_close(target_pos_w[:, robot_lf_idx], robot_lf_pos_w)
+        torch.testing.assert_close(target_pos_w[:, robot_1_lf_idx], robot_1_lf_pos_w)
+
+        # TEST 3: Implicit named frames (RF_SHANK) should also have DIFFERENT world positions
+        # Even though they have the same frame name, internal body tracking uses full paths
+        rf_pos_difference = torch.norm(
+            target_pos_w[:, rf_shank_indices[0]] - target_pos_w[:, rf_shank_indices[1]], dim=-1
+        )
+        assert torch.all(rf_pos_difference > 1.0), (
+            f"The two RF_SHANK frames should have different positions (got diff={rf_pos_difference}). "
+            "This indicates body name collision bug in internal body tracking."
+        )
+
+        # Verify implicit named frames match correct robot bodies
+        # Note: Order depends on internal processing, so we check both match one of the robots
+        rf_positions = [target_pos_w[:, rf_shank_indices[0]], target_pos_w[:, rf_shank_indices[1]]]
+
+        # Each tracked position should match one of the ground truth positions
+        for rf_pos in rf_positions:
+            matches_robot = torch.allclose(rf_pos, robot_rf_pos_w, atol=1e-5)
+            matches_robot_1 = torch.allclose(rf_pos, robot_1_rf_pos_w, atol=1e-5)
+            assert (
+                matches_robot or matches_robot_1
+            ), f"RF_SHANK position {rf_pos} doesn't match either robot's RF_SHANK position"