FCL Error in ComputeSignedDistancePairwisePoints

[cohnt]

What happened?

I'm getting the following error message from somewhere deep in my code:

RuntimeError: Error with configuration
  Original error message: bazel-out/k8-opt/bin/external/fcl_internal/drake_hdr/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h:(1720): validateNearestFeatureOfPolytopeBeingEdge(): The origin is outside of the polytope. This should already have been identified as separating.
  Shape 1:
Cylinder<double>(0.041000000000000001721, 0.17000000000000001221);
  X_FS1
0.62286449762754547699, 0.71651535568044355529, -0.31407891153229777759, 0.20238033346825459735,
-0.20605566270224237591, -0.23703718387133412837, -0.94939688083050233214, 0.027845683594011010065,
-0.75470582445316081177, 0.65606334948423605802, 4.0172294049188053423e-17, 0.3114233939246693339,
0, 0, 0, 1;
  Shape 2:
Box<double>(0.2999999999999999889, 0.5999999999999999778, 0.016000000000000000333);
  X_FS2
1, 0, 0, 0.4000000000000000222,
0, 1, 0, 0,
0, 0, 1, 0.2688500000000000334,
0, 0, 0, 1;
  Solver: GjkSolver_libccd
    collision_tolerance:      2.0097183471152322134e-14
    max collision iterations: 500
    distance tolerance:       9.9999999999999995475e-07
    max distance iterations:  1000

Could be connected to #18704, or something within FCL? I have no idea what's going on in there.

Version

No response

What operating system are you using?

No response

What installation option are you using?

No response

Relevant log output

No response

[jwnimmer-tri]

Unless we have a reproducible demo of the problem (even a large demo, it doesn't need to be minimal), there's not much we can do about this.

If you're planning to create such a demonstration @cohnt, we can keep this open awaiting more information. Otherwise, we'll close it as unactionable until someone can put together such a demo.

[cohnt]

Fair enough! Fortunately, the output of the error message provided enough information to create a small reproduction (that doesn't involve sharing my entire codebase). Here's that reproduction:

import time
import numpy as np
from pydrake.all import (
    StartMeshcat,
    DiagramBuilder,
    AddMultibodyPlantSceneGraph,
    MeshcatVisualizerParams,
    Role,
    MeshcatVisualizer,
    Cylinder,
    GeometryInstance,
    RigidTransform,
    ProximityProperties,
    MakePhongIllustrationProperties,
    Box,
    GeometrySet
)

meshcat = StartMeshcat()

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)

meshcat_collision_params = MeshcatVisualizerParams()
meshcat_collision_params.delete_on_initialization_event = False
meshcat_collision_params.role = Role.kProximity
meshcat_collision_params.prefix = "collision"
meshcat_collision_params.visible_by_default = True
meshcat_collision = MeshcatVisualizer.AddToBuilder(
    builder, scene_graph, meshcat, meshcat_collision_params)

geom_id = scene_graph.RegisterSource()

cylinder = Cylinder(0.041000000000000001721, 0.17000000000000001221)
X_cylinder = np.array([
    [   0.62286449762754547699, 0.71651535568044355529, -0.31407891153229777759, 0.20238033346825459735],
    [-0.20605566270224237591, -0.23703718387133412837, -0.94939688083050233214, 0.027845683594011010065],
    [-0.75470582445316081177, 0.65606334948423605802, 4.0172294049188053423e-17, 0.3114233939246693339],
    [0, 0, 0, 1]
])
geometry_cylinder = GeometryInstance(RigidTransform(X_cylinder), cylinder, "cylinder")
geometry_cylinder.set_proximity_properties(ProximityProperties())

scene_graph.RegisterGeometry(geom_id, scene_graph.world_frame_id(), geometry_cylinder)

box = Box(0.2999999999999999889, 0.5999999999999999778, 0.016000000000000000333)
X_box = np.array([
    [1, 0, 0, 0.4000000000000000222],
    [0, 1, 0, 0],
    [0, 0, 1, 0.2688500000000000334],
    [0, 0, 0, 1]
])
geometry_box = GeometryInstance(RigidTransform(X_box), box, "box")
geometry_box.set_proximity_properties(ProximityProperties())

scene_graph.RegisterGeometry(geom_id, scene_graph.world_frame_id(), geometry_box)

plant.Finalize()

diagram = builder.Build()

context = diagram.CreateDefaultContext()
diagram.ForcedPublish(context)

plant_context = plant.GetMyContextFromRoot(context)
query_object = plant.get_geometry_query_input_port().Eval(plant_context);
inspector = query_object.inspector()
all_geom_ids = list(inspector.GetGeometryIds(GeometrySet(inspector.GetAllGeometryIds()), Role.kProximity))

query_object.ComputeSignedDistancePairClosestPoints(all_geom_ids[0], all_geom_ids[1])

Here's an image of what the configuration actually looks like:

[jwnimmer-tri]

Excellent! I'll re-assign to the component owner now for triage.

[DamrongGuoy]

Since it's in FCL, @SeanCurtis-TRI would know better than me.

[jwnimmer-tri]

To be clear -- just because a bug is assigned to you, doesn't mean you need to try to fix it, or even find the root cause. The only thing the component lead needs to do is decide how important (relatively speaking) the bug is, to assign a "priority" label. I guess we can let Sean decide, in any case.

[cohnt]

Just to give a little more context on how I encountered this bug, I'm currently working on some improvements/extensions to IrisInConfigurationSpace. This algorithm is effectively searching for configurations where to collision geometries are just slightly touching (i.e. signed distance is zero), so it seems to have a knack for finding these odd edge cases. #21192 was encountered in the same way.

I'm not currently blocking on this for my research -- since I'm working in the C++ right now, I've wrapped the query in a try-catch block to handle this with some custom logic. So it's not urgent (for me, at least) that we get a fix.

[DamrongGuoy]

@cohnt Thank you for the context.

FYI, for zero signed distance, this table describes the current limitation. I hope there are supported cases that are useful for you.

https://drake.mit.edu/doxygen_cxx/classdrake_1_1geometry_1_1_query_object.html#:~:text=Characterizing%20the%20returned%20gradients