Adds automatic transform discovery for imu sensors to a viable parent body. #3864
Conversation
…d file. This PR is based on work by @GiulioRomualdi here: isaac-sim#3094
bmccann-bdai
requested review from
jtigue-bdai and
pascal-roth
as code owners
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
There was a problem hiding this comment.
Greptile Overview
Greptile Summary
This PR enables IMU sensors to be attached to arbitrary USD transform primitives (xForms) rather than requiring direct attachment to rigid bodies with RigidBodyAPI. The implementation adds automatic parent body discovery by traversing up the USD prim hierarchy to find the nearest rigid body ancestor, then mathematically composes the fixed transform from that ancestor to the target prim with any user-configured offset. This addresses issue #3088 where users needed to attach IMUs to frames defined in robot URDFs that don't have RigidBodyAPI (e.g., /ergoCub/root_link/torso_1/waist_imu_0). The solution is IMU-specific because different sensor types handle pose information too differently to benefit from a generalized approach, as noted in the PR description. Key files modified include imu.py for transform discovery and composition logic, utils.py for USD hierarchy traversal utilities, and comprehensive test coverage validating equivalence between xForm-attached and offset-attached IMUs.
PR Description Notes:
- Checklist items for changelog update and CONTRIBUTORS.md are not completed
Important Files Changed
| Filename | Score | Overview |
|---|---|---|
| source/isaaclab/isaaclab/sensors/imu/imu.py | 4/5 | Implements automatic rigid body ancestor discovery and fixed transform composition for IMU attachment to xForm primitives |
| source/isaaclab/isaaclab/sim/utils.py | 2/5 | Adds utility functions for USD API checking and parent transform resolution with critical type hint errors and missing null checks |
| source/isaaclab/test/sensors/test_imu.py | 4/5 | Adds comprehensive test coverage for xForm attachment feature, validating transform equivalence and invalid attachment rejection |
Confidence score: 3/5
- This PR introduces useful functionality but has several implementation issues that require attention before merging
- Score reflects critical bugs in
utils.pyincluding incorrect type hint syntax (Usd.Stage()vsUsd.Stage), missing null check forfind_first_matching_primthat can cause crashes, and inadequate type checking usingtype(apis) is not list; additionally, the feature relies on the correctness of external utilities whose edge cases are not fully validated - Pay close attention to
source/isaaclab/isaaclab/sim/utils.pyfor the type hint error on line 913, missing null safety on line 931, and type checking pattern on line 897; also verify the transform composition logic inimu.pyhandles all USD hierarchy edge cases correctly
Sequence Diagram
sequenceDiagram
participant User
participant Imu
participant resolve_pose_relative_to_physx_parent
participant find_first_matching_prim
participant USD Stage
participant PhysX View
User->>Imu: __init__(cfg)
Imu->>Imu: _initialize_impl()
Imu->>find_first_matching_prim: find_first_matching_prim(cfg.prim_path)
find_first_matching_prim->>USD Stage: search for matching prim
USD Stage-->>find_first_matching_prim: target prim
find_first_matching_prim-->>Imu: prim
Imu->>resolve_pose_relative_to_physx_parent: resolve_pose_relative_to_physx_parent(cfg.prim_path)
resolve_pose_relative_to_physx_parent->>find_first_matching_prim: find_first_matching_prim(prim_path_regex)
find_first_matching_prim->>USD Stage: get target_prim
USD Stage-->>find_first_matching_prim: target_prim
find_first_matching_prim-->>resolve_pose_relative_to_physx_parent: target_prim
resolve_pose_relative_to_physx_parent->>resolve_pose_relative_to_physx_parent: check_prim_implements_apis(target_prim, UsdPhysics.RigidBodyAPI)
alt target_prim implements required APIs
resolve_pose_relative_to_physx_parent-->>Imu: (prim_path_regex, None, None)
else target_prim does not implement APIs
resolve_pose_relative_to_physx_parent->>USD Stage: walk up hierarchy to find ancestor
USD Stage-->>resolve_pose_relative_to_physx_parent: ancestor prim with RigidBodyAPI
resolve_pose_relative_to_physx_parent->>USD Stage: compute relative transform (ancestor->target)
USD Stage-->>resolve_pose_relative_to_physx_parent: XformCache with relative transform
resolve_pose_relative_to_physx_parent->>resolve_pose_relative_to_physx_parent: extract position and quaternion
resolve_pose_relative_to_physx_parent-->>Imu: (ancestor_path, fixed_pos_b, fixed_quat_b)
end
Imu->>PhysX View: create_rigid_body_view(_rigid_parent_expr)
PhysX View-->>Imu: _view
Imu->>Imu: _initialize_buffers_impl()
alt fixed transform exists
Imu->>Imu: compose cfg.offset with fixed transform
Note over Imu: composed_p = fixed_p + R_fixed * cfg_p<br/>composed_q = fixed_q * cfg_q
Imu->>Imu: update _offset_pos_b and _offset_quat_b
end
Imu-->>User: initialized IMU sensor
3 files reviewed, 3 comments
| prim_path_regex: str, | ||
| implements_apis: list[Usd.APISchemaBase] | Usd.APISchemaBase = UsdPhysics.RigidBodyAPI, | ||
| *, | ||
| stage: Usd.Stage() | None = None, |
There was a problem hiding this comment.
syntax: typo in type hint: Usd.Stage() should be Usd.Stage (remove the call operator)
| stage: Usd.Stage() | None = None, | |
| stage: Usd.Stage | None = None, |
| target_prim = find_first_matching_prim(prim_path_regex, stage) | ||
| # If target prim itself implements all required APIs, we can use it directly. | ||
| if check_prim_implements_apis(target_prim, implements_apis): | ||
| return prim_path_regex.replace(".*", "*"), None, None |
There was a problem hiding this comment.
logic: missing null check: find_first_matching_prim can return None, but code assumes valid prim on line 933
| def check_prim_implements_apis( | ||
| prim: Usd.Prim, apis: list[Usd.APISchemaBase] | Usd.APISchemaBase = UsdPhysics.RigidBodyAPI | ||
| ) -> bool: | ||
| """Return true if prim implements all apis, False otherwise.""" | ||
|
|
||
| if type(apis) is not list: | ||
| return prim.HasAPI(apis) | ||
| else: | ||
| return all(prim.HasAPI(api) for api in apis) |
There was a problem hiding this comment.
style: using type(apis) is not list won't catch subclasses or list-like types - consider isinstance(apis, list) instead
There was a problem hiding this comment.
Greptile Overview
Greptile Summary
This review covers only the changes made since the last review, not the entire PR. The most recent changes address the final checklist items: updating the changelog, bumping the version number, and adding the contributor name. Specifically, version 0.48.2is documented in CHANGELOG.rst with an entry describing the IMU sensor xform attachment capability, the extension version is incremented from "0.47.2" to "0.48.2" in config/extension.toml, and Brian McCann is added to the alphabetized contributors list. These changes complete the documentation and administrative requirements for the PR that implements automatic transform discovery for IMU sensors.
Important Files Changed
| Filename | Score | Overview |
|---|---|---|
| CONTRIBUTORS.md | 5/5 | Adds Brian McCann to the alphabetized contributors list |
| source/isaaclab/docs/CHANGELOG.rst | 5/5 | Documents the IMU xform attachment feature for version 0.48.2 release |
| source/isaaclab/config/extension.toml | 5/5 | Increments extension version from "0.47.2" to "0.48.2" |
Confidence score: 5/5
- This PR is safe to merge with minimal risk
- Score reflects completion of straightforward administrative/documentation tasks with no code logic changes in this update
- No files require special attention - these are standard changelog, version bump, and contributor acknowledgment updates
3 files reviewed, no comments
There was a problem hiding this comment.
Greptile Overview
Greptile Summary
This review covers only the changes made since the last review, not the entire PR. The developer has completed the checklist items that were previously pending: the CHANGELOG.rst has been updated with version 0.48.2 documenting the new feature, the extension.toml version has been bumped from 0.47.2 to 0.48.2, and the contributor name "Brian McCann" has been added to CONTRIBUTORS.md. These are standard maintenance changes that bring the PR into compliance with the project's contribution guidelines. The core implementation (automatic transform discovery for IMU sensors to attach to xForm primitives) remains unchanged from previous reviews.
Important Files Changed
| Filename | Score | Overview |
|---|---|---|
| CONTRIBUTORS.md | 5/5 | Added "Brian McCann" to the alphabetically sorted contributors list |
| source/isaaclab/docs/CHANGELOG.rst | 5/5 | Added version 0.48.2 changelog entry documenting the IMU sensor xform attachment feature |
| source/isaaclab/config/extension.toml | 5/5 | Bumped extension version from 0.47.2 to 0.48.2 following semantic versioning |
| source/isaaclab/isaaclab/sim/utils.py | 3/5 | No changes since last review - still has critical null-check issue on line 931 |
| source/isaaclab/isaaclab/sensors/imu/imu.py | 3/5 | No changes since last review - still has potential null-check and indexing concerns |
| source/isaaclab/test/sensors/test_imu.py | 4/5 | No changes since last review - comprehensive test coverage for the new feature |
Confidence score: 3/5
- This PR requires attention to critical issues in core implementation files before merging
- Score reflects unresolved critical null-check issue in utils.py and potential runtime errors in imu.py that were identified in previous reviews but remain unaddressed
- Pay close attention to source/isaaclab/isaaclab/sim/utils.py (line 931) and source/isaaclab/isaaclab/sensors/imu/imu.py (lines 144-147) where missing null checks could cause runtime crashes
6 files reviewed, 3 comments
| raise RuntimeError(f"Failed to find a RigidBodyAPI for the prim paths: {self.cfg.prim_path}") | ||
|
|
||
| # Determine rigid source prim and (if needed) the fixed transform from that rigid prim to target prim | ||
| self._rigid_parent_expr, fixed_pos_b, fixed_quat_b = resolve_pose_relative_to_physx_parent(self.cfg.prim_path) |
There was a problem hiding this comment.
logic: if resolve_pose_relative_to_physx_parent returns None for rigid_parent_expr, line 147 will fail when creating the rigid body view. Does resolve_pose_relative_to_physx_parent guarantee non-None rigid_parent_expr, or should we add validation?
|
|
||
| # get the offset from COM to link origin | ||
| # COM of rigid source (body frame) | ||
| com_pos_b = self._view.get_coms().to(self.device).split([3, 4], dim=-1)[0] |
There was a problem hiding this comment.
logic: when env_ids is a slice (line 180), indexing com_pos_b[env_ids] requires com_pos_b to be indexed first - should be self._view.get_coms()[env_ids]
| def test_attachment_validity(setup_sim): | ||
| """Test invalid imu attachment. An imu cannot be attached directly to the world. It must be somehow attached to | ||
| something implementing physics.""" | ||
| sim, scene = setup_sim | ||
| imu_world_cfg = ImuCfg( | ||
| prim_path="/World/envs/env_0", | ||
| gravity_bias=(0.0, 0.0, 0.0), | ||
| ) | ||
| with pytest.raises(RuntimeError) as exc_info: | ||
| imu_world = Imu(imu_world_cfg) | ||
| imu_world._initialize_impl() | ||
| assert exc_info.type is RuntimeError |
There was a problem hiding this comment.
logic: test may pass even if initialization succeeds - should call _initialize_impl() before entering the context manager, or verify the exception message to ensure it's the expected error. Should this test verify the exception message to ensure it's catching the correct error about invalid attachment?
…i/IsaacLab into bcm/sensor_transform
There was a problem hiding this comment.
Greptile Overview
Greptile Summary
This review covers only the changes made since the last review. The most recent change updates the critical file source/isaaclab/isaaclab/sensors/imu/imu.py, which implements the core IMU sensor attachment logic. The file now includes automatic transform discovery that walks up the USD hierarchy to find a viable PhysX parent (a prim with RigidBodyAPI) when the target prim lacks the required physics APIs. This change addresses issue #3088 by allowing IMU sensors to attach to arbitrary transform frames (xform primitives) rather than only rigid bodies. The implementation calls resolve_pose_relative_to_physx_parent() during initialization to compute the relative transform from the target prim to its closest physics-enabled ancestor, then creates a RigidPrimView on that ancestor while storing the computed offset for sensor data transformation.
Important Files Changed
| Filename | Score | Overview |
|---|---|---|
| source/isaaclab/isaaclab/sensors/imu/imu.py | 3/5 | Implements automatic transform discovery for IMU sensors to find viable PhysX parents and compute relative transforms |
| source/isaaclab/isaaclab/sim/utils.py | 3/5 | Already reviewed - adds utility functions for API checking and transform resolution with regex pattern handling concerns |
| source/isaaclab/test/sensors/test_imu.py | 4/5 | Already reviewed - comprehensive test coverage for indirect attachment feature across various USD hierarchy configurations |
| source/isaaclab/docs/CHANGELOG.rst | 5/5 | Already reviewed - documents the new xform primitive attachment capability in version 0.48.2 |
| source/isaaclab/config/extension.toml | 5/5 | Already reviewed - version bump from 0.47.2 to 0.48.2 reflecting the new feature |
| CONTRIBUTORS.md | 5/5 | Already reviewed - adds author name to contributors list |
Confidence score: 3/5
- This PR requires careful review due to complex transform resolution logic and potential edge cases in USD hierarchy traversal
- Score reflects concerns about: (1) error handling when
resolve_pose_relative_to_physx_parent()returnsNonefor transforms but the code path may still try to use them, (2) the regex pattern reconstruction logic insim/utils.pythat assumes simple wildcard patterns, (3) potential type mismatches withGetPrimPath()returningSdf.Pathinstead of string, and (4) unreviewed implementation details in the criticalimu.pyfile that orchestrates the entire feature - Pay close attention to
source/isaaclab/isaaclab/sensors/imu/imu.py(the newly changed file not fully reviewed in context),source/isaaclab/isaaclab/sim/utils.py(transform resolution and regex handling), and the interaction between_initialize_impl()and the new transform discovery code paths
6 files reviewed, 8 comments
| if fixed_pos_b is not None and fixed_quat_b is not None: | ||
| # Broadcast fixed transform across instances | ||
| fixed_p = torch.tensor(fixed_pos_b, device=self._device).repeat(self._view.count, 1) | ||
| fixed_q = torch.tensor(fixed_quat_b, device=self._device).repeat(self._view.count, 1) | ||
|
|
||
| cfg_p = self._offset_pos_b.clone() | ||
| cfg_q = self._offset_quat_b.clone() | ||
|
|
||
| composed_p = fixed_p + math_utils.quat_apply(fixed_q, cfg_p) | ||
| composed_q = math_utils.quat_mul(fixed_q, cfg_q) | ||
|
|
||
| self._offset_pos_b = composed_p | ||
| self._offset_quat_b = composed_q |
There was a problem hiding this comment.
style: offset composition assumes both fixed transforms are always present together, but the logic doesn't validate that fixed_pos_b and fixed_quat_b are both non-None or both None. Does resolve_pose_relative_to_physx_parent guarantee both are None or both are non-None?
| Changed | ||
| ^^^^^^^ | ||
|
|
||
| * Implement ability to attach an imu sensor to xform primitives in a usd file. This PR is based on work by @GiulioRomualdi here: #3094 Addressing issue #3088. |
There was a problem hiding this comment.
syntax: typo: 'Implement' should be 'Implemented' to match past tense convention used throughout the CHANGELOG
| Changed | ||
| ^^^^^^^ | ||
|
|
||
| * Implement ability to attach an imu sensor to xform primitives in a usd file. This PR is based on work by @GiulioRomualdi here: #3094 Addressing issue #3088. |
There was a problem hiding this comment.
style: consider using the @username syntax in backticks to prevent GitHub mentions: @GiulioRomualdi
| def test_indirect_attachment(setup_sim): | ||
| """Test attaching the imu through an xForm primitive configuration argument.""" |
There was a problem hiding this comment.
style: test name says 'indirect_attachment' but docstring says 'xForm primitive' - more precise to say 'IMU attached to xForm child of rigid body link'
| def test_attachment_validity(setup_sim): | ||
| """Test invalid imu attachment. An imu cannot be attached directly to the world. It must be somehow attached to | ||
| something implementing physics.""" | ||
| sim, scene = setup_sim | ||
| imu_world_cfg = ImuCfg( | ||
| prim_path="/World/envs/env_0", | ||
| gravity_bias=(0.0, 0.0, 0.0), | ||
| ) | ||
| with pytest.raises(RuntimeError) as exc_info: | ||
| imu_world = Imu(imu_world_cfg) | ||
| imu_world._initialize_impl() | ||
| assert exc_info.type is RuntimeError |
There was a problem hiding this comment.
style: test only verifies RuntimeError is raised, not the specific error message about invalid attachment - should assert on message to ensure it's the expected failure mode
|
|
||
| # This restores regex patterns from the original PathPattern in the path to return. | ||
| # ( Omnikit 18+ may provide a cleaner approach without relying on strings ) | ||
| child_path = target_prim.GetPrimPath() |
There was a problem hiding this comment.
syntax: GetPrimPath() returns Sdf.Path, not string. Call .pathString if string is needed, or handle Sdf.Path directly
| if rel and prim_path_regex.endswith(rel): | ||
| # Remove "/<rel>" or "<rel>" at end | ||
| cut_len = len(rel) | ||
| trimmed = prim_path_regex | ||
| if trimmed.endswith("/" + rel): | ||
| trimmed = trimmed[: -(cut_len + 1)] | ||
| else: | ||
| trimmed = trimmed[:-cut_len] | ||
| ancestor_path = trimmed |
There was a problem hiding this comment.
logic: string manipulation assumes prim_path_regex format matches child_path suffix structure - if regex contains complex patterns (e.g., character classes, quantifiers beyond .*), this substring logic may produce incorrect results. What regex patterns are expected in prim_path_regex beyond simple wildcards? Complex patterns like [0-9]+ or (a|b) could break the string trimming logic.
| raise RuntimeError(f"Path: {prim_path_regex} does not match any existing primitives.") | ||
| # If target prim itself implements all required APIs, we can use it directly. | ||
| if check_prim_implements_apis(target_prim, implements_apis): | ||
| return prim_path_regex.replace(".*", "*"), None, None |
There was a problem hiding this comment.
logic: when target prim directly implements required APIs, returning None for transforms may break callers expecting valid tuples - should document this behavior or return identity transform ((0, 0, 0), (1, 0, 0, 0)). Should this return identity transform instead of None when no offset is needed, to maintain consistent return type handling?
1 participant
Description
Implement ability to attach an imu sensor to xform primitives in a usd file. This PR is based on work by @GiulioRomualdi here: #3094 Addressing issue #3088.
We considered more general implementations to account for all sensor types, but found they all handle pose information too differently to gain any benefit from a more general solution.
Fixes # (3088)
Type of change
Checklist
pre-commitchecks with./isaaclab.sh --formatconfig/extension.tomlfileCONTRIBUTORS.mdor my name already exists there