Return identity directly when resolving a frame against itself (backport #1693)

[mergify]

🎉 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.

---

This is an automatic backport of pull request #1693 done by Mergify.