gazebosim / gazebosim/sdformat

resolvePose leaves a sub-epsilon residual when a frame is resolved to itself

Open
#1,692 5 comments 0 reactions 0 assignees View on GitHub
Dominant language
C++
Stars
216
Forks
125
Avg merge
1d 14h
Merged PRs (30d)
14

Description

## Environment

* OS Version: macOS 15 (arm64). The residual is observable on arm64; on amd64 the same computation appears to cancel exactly.
* Source or binary build? Source, via gz-sim10.

## Description

`resolvePose()` resolves both the frame and the resolve-to frame all the way to the graph root, then composes them:

https://github.com/gazebosim/sdformat/blob/cb1179326fab25ba4b44dfecf1f9f22bdd909346/src/FrameSemantics.cc#L1791-L1813

```cpp
Errors errors = resolvePoseRelativeToRoot(_pose, _graph, _frameVertexId);

if (_resolveToVertexId != gz::math::graph::kNullId)
{
gz::math::Pose3d poseR;
Errors errorsR = resolvePoseRelativeToRoot(poseR, _graph, _resolveToVertexId);
...
_pose = poseR.Inverse() * _pose;
}
```

When `_frameVertexId == _resolveToVertexId`, `poseR` and `_pose` are the product of the *same* edge chain, so the last line is an inverse-times-itself round trip. That is the identity in exact arithmetic, but in doubles the quaternion inverse and multiply leave a residual on the order of 2^-56.

This is reachable from ordinary SDF. A joint axis declared without `xyz_expressed_in`:

```xml
0 0 1
```

resolves the axis frame to itself, because `JointAxis::ResolveXyz` sets `axisExpressedIn` and `resolveTo` to the same `xmlParentName` when both are empty:

https://github.com/gazebosim/sdformat/blob/main/src/JointAxis.cc#L451-L493

## Steps to reproduce

Downstream in gz-sim, `gz model -m vehicle_blue --joint` against `test/worlds/static_diff_drive_vehicle.sdf` prints:

```
- Axis unit vector [ XYZ ]:
[0 1.26714e-17 1]
```

where `[0 0 1]` is declared in the SDF. Verified on a GitHub `macos-15` arm64 runner:
https://github.com/samirbhattarai135/gz-sim/actions/runs/31218579236

Every other value in that output — poses, inertial matrices, masses — matched the expected values byte for byte. The same magnitude of residual is present in the pose values too; it is simply invisible there because those are printed with fixed 6-decimal formatting.

Context and discussion: https://github.com/gazebosim/gz-sim/issues/3602

## Suggested fix

Two changes are possible, with quite different scope. @scpeters suggested the second in https://github.com/gazebosim/gz-sim/issues/3602#issuecomment-5222221359.

**1. Skip the round trip when the two vertices are identical.** Guard the `poseR.Inverse() * _pose` composition on `_resolveToVertexId != _frameVertexId`. This is a few lines, affects only the case that is identity by definition, and leaves every other numerical result untouched.

**2. Resolve to the lowest common ancestor rather than the root.** More efficient in general, and it makes case 1 fall out for free, since `LowestCommonAncestor(g, v, v) == v` and both walks become zero-length. `gz::math::graph::LowestCommonAncestor` was added by @caguero in https://github.com/gazebosim/gz-math/pull/759 and is present on gz-math7, gz-math8 and main.

A few notes on the second option from reading the code:

* `ScopedGraph::Graph()` already returns `const DirectedGraph&`, which is directly what `LowestCommonAncestor` accepts.
* Edge direction lines up: `PoseRelativeToGraph` documents edges "pointing to a given frame from its relative-to frame", and the LCA helper walks `AdjacentsTo`.
* There is currently no resolve-to-arbitrary-ancestor walk. `FindSourceVertex` walks to the scope root, so a stop-at-vertex variant would be needed.
* The existing `POSE_RELATIVE_TO_GRAPH_ERROR` diagnostics for disconnected frames would need to be preserved; `LowestCommonAncestor` returns `kNullId` for different-tree and cyclic inputs, and those cases should keep producing the current error messages.
* Because resolving via the LCA is mathematically equivalent but not bit-identical, hardcoded pose expectations elsewhere in the test suite may shift.

I am happy to implement either or both. Given the difference in blast radius, my suggestion would be to land 1 as a small backportable correctness fix and treat 2 as a separate performance change with its own benchmarks, but I am glad to do it whichever way you prefer.

Contributor guide

Open the contributing guide

Research direction

Start in src/FrameSemantics.cc at resolvePose and the resolvePoseRelativeToRoot calls linked in the issue; review src/JointAxis.cc at JointAxis::ResolveXyz to understand the self-resolving axis case. Reproduce the arm64 residual, then verify that the chosen fix removes the identity round-trip while preserving the existing disconnected-frame diagnostics and other pose results.

Written by the indexing model from the issue text.

Assessment

Tech stack
cpp
Domain
robotics
Issue type
Bug
Difficulty
3/5
Estimated time
1-2 days
Activity status
Quiet
Clarity
Mostly clear
Newbie friendliness
58/100

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.