QueryObject::ComputeSignedDistancePairwiseClosestPoints is not deterministic

[hongkai-dai]

What happened?

The ordering in the returned std::vector<SignedDistancePair<T>> is not deterministic.

To reproduce it, check out the branch https://github.com/hongkai-dai/drake/tree/reproduce_compute_signed_distance_ordering_error, and the gist files, and take the following steps

1. Compile pydrake from source, as in https://drake.mit.edu/from_source.html
2. run python test.py

It will print out the result, you might need to run python test.py for several times before the print out message changes.

In my case, normally it prints

distances[0]=0.04804141069317556523, grad[25]=-0.53185542592967094411, grad[31]=-0.42261090948719659544
distances[1]=0.05217582339447376233, grad[25]=0.00000000000000056585, grad[31]=0.00000000000000044963
distances[2]=0.07077759825297903762, grad[25]=-0.35937297264284862042, grad[31]=-0.28555681000760274602

but sometimes it prints

distances[0]=0.05217582339447376233, grad[25]=0.00000000000000056585, grad[31]=0.00000000000000044963
distances[1]=0.07077759825297903762, grad[25]=-0.35937297264284862042, grad[31]=-0.28555681000760274602
distances[2]=0.04804141069317556523, grad[25]=-0.53185542592967094411, grad[31]=-0.42261090948719659544

The ordering of the reported distances is changed.

This is related to #21682

Version

No response

What operating system are you using?

No response

What installation option are you using?

No response

Relevant log output

No response

[hongkai-dai]

This seems to be a problem in FCL. The collision geometries are boxes. I can confirm that each call has the same frame ID, but the return from the FCL is different.

[jwnimmer-tri]

I think this is probably a duplicate of #13739. The solution to the related problem (for contact results) was to explicitly sort them at the end.

[hongkai-dai]

I think the documentation of ComputeSignedDistancePairwiseClosestPoints can be confusing. It says

For a geometry pair (A, B), the returned results will always be reported in a fixed order (e.g., always (A, B) and never (B, A)). The basis for the ordering is arbitrary (and therefore undocumented), but guaranteed to be fixed and repeatable.

as I understand it, this means that within a pair of geometry, the ordering is fixed. But we didn't mention about the ordering in the vector<SignedDistancePair<T>>. It is worth mentioning explicitly about the ordering of the vector, whether it is deterministic or not.

I personally would prefer the returned vector<SignedDistancePair<T>> to be sorted. Using std::vector instead of std::unordered_set suggests there is an ordering. Hence I would suggest to the sorting within ComputeSignedDistancePairwiseClosestPoints, rather than in the downstream code.