From a1871e8137b9afbef23ef7203e166fee6f54c3e1 Mon Sep 17 00:00:00 2001 From: Bikram Pandit Date: Sun, 7 Dec 2025 15:05:41 -0800 Subject: [PATCH 1/8] Fixes body name collision when multiple bodies share the same name --- .../frame_transformer/frame_transformer.py | 29 ++++++++++++++----- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py index b6d2e766302..f8b6c8d69a3 100644 --- a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py +++ b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py @@ -201,10 +201,12 @@ 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 relative prim path as unique body identifier (e.g., "Robot/left_hand" instead of just "left_hand") + # This allows tracking bodies with the same name at different hierarchy levels + env_match = re.search(r"env_\d+/(.*)", matching_prim_path) + body_name = env_match.group(1) if env_match else matching_prim_path.rsplit("/", 1)[-1] + # Use leaf name if frame 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 +293,22 @@ 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 + def _get_rel_path(p): + m = re.search(r"env_\d+/(.*)", p) + return m.group(1) if m else p.rsplit("/", 1)[-1] - # -- source frame - self._source_frame_body_name = self.cfg.prim_path.split("/")[-1] + self._target_frame_body_names = [_get_rel_path(prim_path) for prim_path in sorted_prim_paths] + + # -- source frame: extract relative path from config pattern + # Handle patterns like "{ENV_REGEX_NS}/Robot/torso" or "/World/envs/env_.*/Robot/torso" -> "Robot/torso" + source_path = self.cfg.prim_path + # Try to match {PATTERN}/, env_.*/, or env_X/ + pattern_match = re.search(r"(?:\{[^}]+\}|env_[^/]+)/(.*)", source_path) + if pattern_match: + self._source_frame_body_name = pattern_match.group(1) + else: + self._source_frame_body_name = _get_rel_path(source_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 From b21c8ef1f4b02edc05c210b246bbd4dcb05a26c8 Mon Sep 17 00:00:00 2001 From: Bikram Pandit Date: Sun, 7 Dec 2025 15:16:57 -0800 Subject: [PATCH 2/8] Adds test case to verify that bodies with the same name but different hierarchical paths (e.g., Robot/LF_SHANK vs Robot_1/LF_SHANK) are tracked separately and return distinct transforms. --- .../test/sensors/test_frame_transformer.py | 135 ++++++++++++++++++ 1 file changed, 135 insertions(+) diff --git a/source/isaaclab/test/sensors/test_frame_transformer.py b/source/isaaclab/test/sensors/test_frame_transformer.py index 5257e732420..b78f511c6c7 100644 --- a/source/isaaclab/test/sensors/test_frame_transformer.py +++ b/source/isaaclab/test/sensors/test_frame_transformer.py @@ -589,3 +589,138 @@ def test_sensor_print(sim): sim.reset() # print info print(scene.sensors["frame_transformer"]) + + +@pytest.mark.isaacsim_ci +def test_frame_transformer_duplicate_body_names(sim): + """Test that bodies with same name but different paths are tracked separately. + + This test verifies the fix for body name collision when multiple bodies + share the same name but exist at different hierarchy levels. + """ + + # 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 tracking same-named bodies from both robots + frame_transformer: FrameTransformerCfg = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/base", + target_frames=[ + # Robot's LF_FOOT + FrameTransformerCfg.FrameCfg( + name="LF_FOOT", + prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK", + offset=OffsetCfg( + pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), + rot=quat_from_euler_rpy(0, 0, -math.pi / 2), + ), + ), + # Robot_1's LF_FOOT (same body name, different robot) + FrameTransformerCfg.FrameCfg( + name="LF_FOOT_1", + prim_path="{ENV_REGEX_NS}/Robot_1/LF_SHANK", + offset=OffsetCfg( + pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), + rot=quat_from_euler_rpy(0, 0, -math.pi / 2), + ), + ), + ], + ) + + # Spawn things into stage + scene_cfg = MultiRobotSceneCfg(num_envs=2, env_spacing=10.0, lazy_sensor_update=False) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + + # Get target frame names + target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names + assert "LF_FOOT" in target_frame_names, "LF_FOOT should be in target frames" + assert "LF_FOOT_1" in target_frame_names, "LF_FOOT_1 should be in target frames" + + lf_foot_idx = target_frame_names.index("LF_FOOT") + lf_foot_1_idx = target_frame_names.index("LF_FOOT_1") + + # Acquire ground truth body indices + robot_lf_shank_idx = scene.articulations["robot"].find_bodies("LF_SHANK")[0][0] + robot_1_lf_shank_idx = scene.articulations["robot_1"].find_bodies("LF_SHANK")[0][0] + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + + # Simulate physics + for count in range(50): + # Reset periodically + if count % 25 == 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 + target_pos_w = scene.sensors["frame_transformer"].data.target_pos_w + + # Get ground truth positions + robot_lf_pos_w = scene.articulations["robot"].data.body_pos_w[:, robot_lf_shank_idx] + robot_1_lf_pos_w = scene.articulations["robot_1"].data.body_pos_w[:, robot_1_lf_shank_idx] + + # The two positions should be DIFFERENT (robots are at different locations) + # If the bug exists, both would return the same position + pos_difference = torch.norm(target_pos_w[:, lf_foot_idx] - target_pos_w[:, lf_foot_1_idx], dim=-1) + + # The robots are 2m apart, so the feet should also be roughly 2m apart + assert torch.all(pos_difference > 1.0), ( + f"LF_FOOT and LF_FOOT_1 should have different positions (got diff={pos_difference}). " + "This indicates body name collision bug." + ) + + # Verify each frame transformer output matches the correct robot's body + # (with some tolerance for the offset transformation) + torch.testing.assert_close( + target_pos_w[:, lf_foot_idx], + robot_lf_pos_w, + atol=0.5, # Allow tolerance for offset + rtol=0.1, + ) + torch.testing.assert_close( + target_pos_w[:, lf_foot_1_idx], + robot_1_lf_pos_w, + atol=0.5, # Allow tolerance for offset + rtol=0.1, + ) From 1cb49a099f68b99097447effd9d4a81c92d9d312 Mon Sep 17 00:00:00 2001 From: Bikram Pandit Date: Sun, 7 Dec 2025 15:24:26 -0800 Subject: [PATCH 3/8] Update changelog and version --- source/isaaclab/config/extension.toml | 2 +- source/isaaclab/docs/CHANGELOG.rst | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index d031ffce038..a7945b41cf2 100644 --- a/source/isaaclab/config/extension.toml +++ b/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" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 6fd86207d43..53d86045336 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/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) ~~~~~~~~~~~~~~~~~~~ From 2691b1573337f8bca032983321a8a906e9f8c0e4 Mon Sep 17 00:00:00 2001 From: Bikram Pandit Date: Sat, 13 Dec 2025 00:18:49 -0800 Subject: [PATCH 4/8] Cleanup regex --- .../frame_transformer/frame_transformer.py | 45 ++++++++++++------- 1 file changed, 30 insertions(+), 15 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py index f8b6c8d69a3..d3430a7cfbc 100644 --- a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py +++ b/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. @@ -203,9 +229,8 @@ def _initialize_impl(self): # Get relative prim path as unique body identifier (e.g., "Robot/left_hand" instead of just "left_hand") # This allows tracking bodies with the same name at different hierarchy levels - env_match = re.search(r"env_\d+/(.*)", matching_prim_path) - body_name = env_match.group(1) if env_match else matching_prim_path.rsplit("/", 1)[-1] - # Use leaf name if frame isn't specified by user + body_name = _get_relative_body_path(matching_prim_path) + # Use leaf name if frame isn't specified by user (for backwards compatibility) 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 @@ -294,21 +319,11 @@ def extract_env_num_and_prim_path(item: str) -> tuple[int, str]: sorted_prim_paths = [all_prim_paths[index] for index in self._per_env_indices] # -- target frames: use relative prim path for unique identification - def _get_rel_path(p): - m = re.search(r"env_\d+/(.*)", p) - return m.group(1) if m else p.rsplit("/", 1)[-1] - - self._target_frame_body_names = [_get_rel_path(prim_path) for prim_path in sorted_prim_paths] + self._target_frame_body_names = [_get_relative_body_path(prim_path) for prim_path in sorted_prim_paths] # -- source frame: extract relative path from config pattern # Handle patterns like "{ENV_REGEX_NS}/Robot/torso" or "/World/envs/env_.*/Robot/torso" -> "Robot/torso" - source_path = self.cfg.prim_path - # Try to match {PATTERN}/, env_.*/, or env_X/ - pattern_match = re.search(r"(?:\{[^}]+\}|env_[^/]+)/(.*)", source_path) - if pattern_match: - self._source_frame_body_name = pattern_match.group(1) - else: - self._source_frame_body_name = _get_rel_path(source_path) + 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 From 527edfdea93ce7674c1c4c82500fd64bbb9c81c2 Mon Sep 17 00:00:00 2001 From: Bikram Pandit Date: Sat, 13 Dec 2025 00:20:22 -0800 Subject: [PATCH 5/8] Add more test coverages for duplicate body frame --- .../frame_transformer/frame_transformer.py | 8 +- .../test/sensors/test_frame_transformer.py | 123 ++++++++++++------ 2 files changed, 83 insertions(+), 48 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py index d3430a7cfbc..b1392467d7b 100644 --- a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py +++ b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py @@ -227,10 +227,9 @@ def _initialize_impl(self): " rigid body. The class only supports transformations between rigid bodies." ) - # Get relative prim path as unique body identifier (e.g., "Robot/left_hand" instead of just "left_hand") - # This allows tracking bodies with the same name at different hierarchy levels + # 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 if frame isn't specified by user (for backwards compatibility) + # 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 @@ -321,8 +320,7 @@ def extract_env_num_and_prim_path(item: str) -> tuple[int, str]: # -- 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: extract relative path from config pattern - # Handle patterns like "{ENV_REGEX_NS}/Robot/torso" or "/World/envs/env_.*/Robot/torso" -> "Robot/torso" + # -- 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) diff --git a/source/isaaclab/test/sensors/test_frame_transformer.py b/source/isaaclab/test/sensors/test_frame_transformer.py index b78f511c6c7..2c19c527906 100644 --- a/source/isaaclab/test/sensors/test_frame_transformer.py +++ b/source/isaaclab/test/sensors/test_frame_transformer.py @@ -593,10 +593,25 @@ def test_sensor_print(sim): @pytest.mark.isaacsim_ci def test_frame_transformer_duplicate_body_names(sim): - """Test that bodies with same name but different paths are tracked separately. + """Test tracking bodies with same leaf name at different hierarchy levels. - This test verifies the fix for body name collision when multiple bodies - share the same name but exist 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. """ # Create a custom scene config with two robots @@ -619,23 +634,21 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): frame_transformer: FrameTransformerCfg = FrameTransformerCfg( prim_path="{ENV_REGEX_NS}/Robot/base", target_frames=[ - # Robot's LF_FOOT + # Explicit frame names (recommended when bodies share the same leaf name) FrameTransformerCfg.FrameCfg( - name="LF_FOOT", + name="Robot_LF_SHANK", prim_path="{ENV_REGEX_NS}/Robot/LF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, -math.pi / 2), - ), ), - # Robot_1's LF_FOOT (same body name, different robot) FrameTransformerCfg.FrameCfg( - name="LF_FOOT_1", + name="Robot_1_LF_SHANK", prim_path="{ENV_REGEX_NS}/Robot_1/LF_SHANK", - offset=OffsetCfg( - pos=euler_rpy_apply(rpy=(0, 0, -math.pi / 2), xyz=(0.08795, 0.01305, -0.33797)), - rot=quat_from_euler_rpy(0, 0, -math.pi / 2), - ), + ), + # 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", ), ], ) @@ -649,15 +662,29 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): # Get target frame names target_frame_names = scene.sensors["frame_transformer"].data.target_frame_names - assert "LF_FOOT" in target_frame_names, "LF_FOOT should be in target frames" - assert "LF_FOOT_1" in target_frame_names, "LF_FOOT_1 should be in target frames" - lf_foot_idx = target_frame_names.index("LF_FOOT") - lf_foot_1_idx = target_frame_names.index("LF_FOOT_1") + # 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_lf_shank_idx = scene.articulations["robot"].find_bodies("LF_SHANK")[0][0] - robot_1_lf_shank_idx = scene.articulations["robot_1"].find_bodies("LF_SHANK")[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] # Define simulation stepping sim_dt = sim.get_physics_dt() @@ -697,30 +724,40 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): target_pos_w = scene.sensors["frame_transformer"].data.target_pos_w # Get ground truth positions - robot_lf_pos_w = scene.articulations["robot"].data.body_pos_w[:, robot_lf_shank_idx] - robot_1_lf_pos_w = scene.articulations["robot_1"].data.body_pos_w[:, robot_1_lf_shank_idx] - - # The two positions should be DIFFERENT (robots are at different locations) - # If the bug exists, both would return the same position - pos_difference = torch.norm(target_pos_w[:, lf_foot_idx] - target_pos_w[:, lf_foot_1_idx], dim=-1) - - # The robots are 2m apart, so the feet should also be roughly 2m apart - assert torch.all(pos_difference > 1.0), ( - f"LF_FOOT and LF_FOOT_1 should have different positions (got diff={pos_difference}). " + 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] + + # TEST 1: Explicit named frames (LF_SHANK) should have DIFFERENT 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 each frame transformer output matches the correct robot's body - # (with some tolerance for the offset transformation) - torch.testing.assert_close( - target_pos_w[:, lf_foot_idx], - robot_lf_pos_w, - atol=0.5, # Allow tolerance for offset - rtol=0.1, + # 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 2: Implicit named frames (RF_SHANK) should also have DIFFERENT 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 ) - torch.testing.assert_close( - target_pos_w[:, lf_foot_1_idx], - robot_1_lf_pos_w, - atol=0.5, # Allow tolerance for offset - rtol=0.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" From b382b4d6a4d0b22c105e0f6c0bf21f2a54fabafd Mon Sep 17 00:00:00 2001 From: Bikram Pandit Date: Wed, 17 Dec 2025 22:45:42 -0800 Subject: [PATCH 6/8] Add coverage for frame transformer source with duplicate body names --- .../test/sensors/test_frame_transformer.py | 86 +++++++++++++------ 1 file changed, 59 insertions(+), 27 deletions(-) diff --git a/source/isaaclab/test/sensors/test_frame_transformer.py b/source/isaaclab/test/sensors/test_frame_transformer.py index 2c19c527906..882f42641f7 100644 --- a/source/isaaclab/test/sensors/test_frame_transformer.py +++ b/source/isaaclab/test/sensors/test_frame_transformer.py @@ -592,7 +592,8 @@ def test_sensor_print(sim): @pytest.mark.isaacsim_ci -def test_frame_transformer_duplicate_body_names(sim): +@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 @@ -612,6 +613,11 @@ def test_frame_transformer_duplicate_body_names(sim): `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 @@ -630,31 +636,35 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): init_state=ANYMAL_C_CFG.init_state.replace(pos=(2.0, 0.0, 0.6)), ) - # Frame transformer tracking same-named bodies from both robots - frame_transformer: FrameTransformerCfg = FrameTransformerCfg( - prim_path="{ENV_REGEX_NS}/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", - ), - ], - ) + # 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 @@ -681,11 +691,17 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): 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() @@ -721,15 +737,31 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): scene.update(sim_dt) # Get frame transformer data - target_pos_w = scene.sensors["frame_transformer"].data.target_pos_w + 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 + # 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] - # TEST 1: Explicit named frames (LF_SHANK) should have DIFFERENT positions + # 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}). " @@ -740,7 +772,7 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): 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 2: Implicit named frames (RF_SHANK) should also have DIFFERENT positions + # 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 From c8b60da27791e65d19882ea0a8615abc5982549d Mon Sep 17 00:00:00 2001 From: Bikram Pandit Date: Thu, 18 Dec 2025 12:15:21 -0800 Subject: [PATCH 7/8] Update source/isaaclab/test/sensors/test_frame_transformer.py Co-authored-by: James Tigue <166445701+jtigue-bdai@users.noreply.github.com> Signed-off-by: Bikram Pandit --- source/isaaclab/test/sensors/test_frame_transformer.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/test/sensors/test_frame_transformer.py b/source/isaaclab/test/sensors/test_frame_transformer.py index 882f42641f7..e45443e3c59 100644 --- a/source/isaaclab/test/sensors/test_frame_transformer.py +++ b/source/isaaclab/test/sensors/test_frame_transformer.py @@ -706,9 +706,9 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): sim_dt = sim.get_physics_dt() # Simulate physics - for count in range(50): + for count in range(20): # Reset periodically - if count % 25 == 0: + if count % 10 == 0: # Reset robot root_state = scene.articulations["robot"].data.default_root_state.clone() root_state[:, :3] += scene.env_origins From f0d82b006e8b4ac621f3836da86983111822e773 Mon Sep 17 00:00:00 2001 From: Bikram Pandit Date: Thu, 18 Dec 2025 12:20:24 -0800 Subject: [PATCH 8/8] format changelog.rst --- source/isaaclab/docs/CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 53d86045336..05d3862a64a 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -10,7 +10,7 @@ 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) ~~~~~~~~~~~~~~~~~~~