Return identity directly when resolving a frame against itself

[samirbhattarai135]

🎉 New feature

Part of #1692

Summary

resolvePose computes the pose of a frame relative to a resolve-to frame as poseR.Inverse() * _pose. When both are the same vertex, that composes an edge chain with its own inverse — the identity in real arithmetic, but not guaranteed in floating point. This returns the identity directly for that case instead of deriving it.

The first resolvePoseRelativeToRoot call is kept, so validation and error diagnostics for the frame vertex are unchanged.

What this changes

resolvePose leaves a sub-epsilon residual when a frame is resolved against itself, and this removes it. Confirmed by @scpeters on macOS arm64 by reverting the FrameSemantics.cc change and running the new test:

pose.Rot().X()  = -6.3357220245782474e-18   (wheel, wheel_joint)
xyz.Y()         =  1.2671444049156495e-17   (wheel_joint)

That xyz.Y() value is exactly what gazebosim/gz-sim#3602 prints for the joint axis.

Correction to my earlier claim in this PR and in #1692. I originally wrote that the test passed with and without the change. That was wrong, and the reason is worth recording: my CI runs used GitHub's macos-latest, which now resolves to the macos-26-arm64 image. So the architecture was right, but the toolchain was not the one that exhibits the residual — the newer Apple clang on macOS 26 apparently contracts the quaternion multiply such that the round trip cancels exactly. Same source, same architecture, different codegen, opposite result.

Worth knowing for anyone testing this area: reproducing it needs the right compiler, not just an arm64 machine, and CI on macos-latest will not show it.

Test

FrameSemantics.resolveAgainstOwnFrameIsExact checks the identity case at the resolvePose level and through JointAxis::ResolveXyz for an axis declared without xyz_expressed_in.

EXPECT_EQ cannot be used here: Pose3d and Vector3d compare through tolerance-based operator==, so a residual near 1e-17 compares equal. Components are checked directly.

The fixture uses -1.5707, mirroring gz-sim's static_diff_drive_vehicle.sdf. Note that model_frame_relative_to_joint.sdf uses only 0 and exact pi/2 rotations and round trips exactly on some toolchains, so a test written against that fixture can pass whether or not the behaviour is present.

Checklist

• Signed all commits for DCO
• Added tests
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (as needed)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

I have not run the full suite locally — I do not have a working sdformat build on this machine, so I have been relying on the fork's CI. Flagging that rather than ticking the box.

Note: Assisted by Claude (Anthropic), per the GenAI disclosure policy; commits carry an Assisted-by: trailer. All analysis and measurements above were verified against real CI runs rather than taken on trust.

[scpeters]

To be explicit: this test passes with and without the change.

I manually reverted the changes to FrameSemantics.cc from this branch and then build and ran the new test, and it fails locally for me on a macOS arm64 machine:

[ RUN      ] FrameSemantics.resolveAgainstOwnFrameIsExact
/Users/scpeters/ws/sdformat_math/src/sdformat/src/FrameSemantics_TEST.cc:338: Failure
Expected equality of these values:
  0.0
    Which is: 0
  pose.Pos().Y()
    Which is: 2.0816681711721685e-17
wheel
/Users/scpeters/ws/sdformat_math/src/sdformat/src/FrameSemantics_TEST.cc:342: Failure
Expected equality of these values:
  0.0
    Which is: 0
  pose.Rot().X()
    Which is: -6.3357220245782474e-18
wheel
/Users/scpeters/ws/sdformat_math/src/sdformat/src/FrameSemantics_TEST.cc:338: Failure
Expected equality of these values:
  0.0
    Which is: 0
  pose.Pos().Y()
    Which is: 2.0816681711721685e-17
wheel_joint
/Users/scpeters/ws/sdformat_math/src/sdformat/src/FrameSemantics_TEST.cc:342: Failure
Expected equality of these values:
  0.0
    Which is: 0
  pose.Rot().X()
    Which is: -6.3357220245782474e-18
wheel_joint
/Users/scpeters/ws/sdformat_math/src/sdformat/src/FrameSemantics_TEST.cc:360: Failure
Expected equality of these values:
  0.0
    Which is: 0
  xyz.Y()
    Which is: 1.2671444049156495e-17
wheel_joint
[  FAILED  ] FrameSemantics.resolveAgainstOwnFrameIsExact (23 ms)

so I don't know why you weren't able to reproduce the problem (maybe testing with amd64 instead of arm64?), but this looks good to me

[samirbhattarai135]

Thank you for reverting and running it — that resolves it, and it means my "could not reproduce" note in #1692 was wrong. I have corrected the PR description.

It was not amd64, which makes it more interesting. My runs used sdformat's existing macos.yml, which is runs-on: macos-latest, and that now resolves to the macos-26-arm64 image:

Image Release: .../runner-images/releases/tag/macos-26-arm64%2F20260728.0273

So same architecture as your machine, same source, opposite result. The remaining difference is the toolchain — the Apple clang on the macOS 26 image evidently contracts the quaternion multiply so the round trip cancels exactly, while yours does not. That also fits the original amd64-passes / arm64-fails split in gazebosim/gz-sim#3602 better than architecture alone does: it looks like FP contraction in codegen rather than anything about the ISA.

Practical consequence worth flagging: this will not reproduce on GitHub's macos-latest runners, so CI there cannot be used to verify it. Your local machine and the OSRF Jenkins arm64 builders can.

I will correct my earlier comments on #1692 and gazebosim/gz-sim#3602 accordingly — with the residual confirmed real in resolvePose, your original point about the trivial case stands, and the LCA change would indeed skip the calculation that induces it.

I have also pushed a benchmark on a separate branch comparing current resolvePose against the cost of a LowestCommonAncestor lookup across nesting depths, since LowestCommonAncestor allocates an unordered_set and a vector per call and I wanted the trade measured rather than assumed before attempting (2). I will open that separately once I have numbers.

[scpeters]

@osrf-jenkins run tests please

[samirbhattarai135]

@scpeters heads up on the red sdformat-ci-pr_any-resolute-amd64 check — it is not a test failure. All 1341 tests passed; the build went UNSTABLE on the compiler-warning quality gate:

[GNU C Compiler (gcc)] - [Total (any severity)]: <Unstable> - (Actual value: 2, Quality gate: 1.00)

Both warnings were mine, in the new test: I bound string literals to const std::string & in two range-for loops, so GCC 15 reported -Wrange-loop-construct for the per-iteration temporary. Clang did not warn, which is why homebrew-arm64 stayed green.

Fixed in 3081c36 by letting the loop variable deduce, matching the existing idiom at src/parser.cc:1770. No change to the test's behaviour or to FrameSemantics.cc.

[scpeters]

thanks for the contribution!

[scpeters]

https://github.com/Mergifyio backport sdf16

[mergify]

backport sdf16

✅ Backports have been created

Details

• #1695 Return identity directly when resolving a frame against itself (backport #1693) has been created for branch sdf16

[scpeters]

https://github.com/Mergifyio backport sdf15

[mergify]

backport sdf15

✅ Backports have been created

Details

• #1696 Return identity directly when resolving a frame against itself (backport #1693) has been created for branch sdf15

[scpeters]

https://github.com/Mergifyio backport sdf14

[mergify]

backport sdf14

✅ Backports have been created

Details

• #1697 Return identity directly when resolving a frame against itself (backport #1693) has been created for branch sdf14

[scpeters]

https://github.com/Mergifyio backport sdf12

[mergify]

backport sdf12

✅ Backports have been created

Details

• #1698 Return identity directly when resolving a frame against itself (backport #1693) has been created for branch sdf12