Reached Unreachable Code in BoxBoxGradient

[cohnt]

What happened?

While using SceneGraphCollisionChecker, I encountered the following error:

abort: Failure at geometry/proximity/distance_to_shape_touching.cc:228 in BoxBoxGradient(): condition 'Unreachable code was reached?!' failed.

I can provide code to replicate this, although it's a rather large chunk. I did print out the arguments to the function, in case that might help debug. But if that's insufficient, I can try to distill it down to a small runnable example.

box_A.side: 0.05, 0.55, 0.3
box_B.side: 0.25, 0.2, 0.15
X_WA    rpy = 0 -0 1.570000000000001 xyz = 1.2500000000000009 -2.9878953394393027e-16 0.15
X_WB    rpy = 3.141592653589793 -0 0 xyz = 1.25 0 0.15
p_ACa   1.71304e-17, 6.66134e-16, 8.32667e-17
p_BCb   2.22045e-16, 6.93889e-17, 8.32667e-17

Version

4fdee8082686b878b97ab33d1605fddae6fc611d

What operating system are you using?

Unknown / Other, Ubuntu 22.04

What installation option are you using?

compiled from source code using CMake

Relevant log output

No response

[jwnimmer-tri]

It's possible that those numbers are enough to reproduce (or at least make a good guess), but ideally we would have the exact numbers. If you can re-run your repro and print all of the doubles without any truncation (i.e., full precision) that would help. Also if you could print the rotations as the native 3x3 matrix (not rpy) that would also remove conversion noise.

In fact, once you have that, I imagine a C++ unit test call to BoxBoxGradient with your exact numbers would probably immediately reproduce the problem. It's just a namespace function with no state, so a full-precision argument dump should be enough to reproduce.

[DamrongGuoy]

There's something fishy. I can reproduce the problem with a unit test of BoxBoxGradient. However, each of the given witness points p_ACa and p_BCb is almost at the center (0,0,0) of its box, far from the box's surface. Witness points are supposed to be on the surface of the box.

The bug is likely at a higher level. If @cohnt could give more data, I can check further. For example, what went into CalcDistanceFallback here.

template <>
void CalcDistanceFallback<double>(const fcl::CollisionObjectd& a,
                                  const math::RigidTransformd& X_WA,
                                  const fcl::CollisionObjectd& b,
                                  const math::RigidTransformd& X_WB,
                                  const fcl::DistanceRequestd& request,
                                  SignedDistancePair<double>* pair_data) {

From what already provided, I just need one more argument: the const fcl::DistanceRequestd& request.

Alternatively, distilling down to a small runnable example would be the best.

[DamrongGuoy]

I reproduced the problem in this commit. I'll continue on Monday.

[cohnt]

Here are all of the values of the fields of request:

request.abs_err                 0
request.distance_tolerance      1e-06
request.enable_nearest_points   1
request.enable_signed_distance  1
request.gjk_solver_type         0
request.rel_err                 0

Let me know if there are other values I can print for you! If you need a runnable example, I probably won't have time to make one until after October 3.