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..05d3862a64a 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) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py b/source/isaaclab/isaaclab/sensors/frame_transformer/frame_transformer.py index b6d2e766302..b1392467d7b 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. @@ -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 diff --git a/source/isaaclab/test/sensors/test_frame_transformer.py b/source/isaaclab/test/sensors/test_frame_transformer.py index 5257e732420..e45443e3c59 100644 --- a/source/isaaclab/test/sensors/test_frame_transformer.py +++ b/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"