SceneGraphCollisionChecker Segfaults when Given NaNs

[cohnt]

What happened?

Not sure how this happened, but I was getting segfaults in some of my code, and eventually discovered it was due to passing nans into a SceneGraphCollisionChecker.

import numpy as np
from pydrake.all import (
    RobotDiagramBuilder,
    Parser,
    CollisionCheckerParams,
    SceneGraphCollisionChecker,
)

builder = RobotDiagramBuilder()

parser = Parser(builder.plant(), builder.scene_graph())
foo = parser.AddModelsFromUrl("package://drake_models/dishes/evo_bowl.sdf")
bar = parser.AddModelsFromUrl("package://drake_models/dishes/plate_8in.sdf")

diagram = builder.Build()

params = dict()
params["edge_step_size"] = 0.01
params["model"] = diagram
params["robot_model_instances"] = foo
cc = SceneGraphCollisionChecker(**params)

q = np.random.rand(14)
q[0] = np.nan
cc.CheckConfigCollisionFree(q)

Obviously, adding a quick check to complain if it gets nans should help the user. But notably, commenting out the bar = parser.AddModelsFromUrl... line makes it not segfault, so I'm suspicious that there's something going on in the scene graph behind the scenes that is segfaulting. And a backtrace shows something going on in the proximity engine as well.

#11 0x00007fffbeb69673 in drake::geometry::internal::ProximityEngine<double>::ComputeSignedDistancePairwiseClosestPoints(std::unordered_map<drake::geometry::GeometryId, drake::math::RigidTransform<double>, std::hash<drake::geometry::GeometryId>, std::equal_to<drake::geometry::GeometryId>, std::allocator<std::pair<drake::geometry::GeometryId const, drake::math::RigidTransform<double> > > > const&, double) const ()
   from /home/tommy/opt/rlg/drake-build/install/lib/python3.10/site-packages/pydrake/common/../../../../libdrake.so
#12 0x00007fffbef80c9c in drake::geometry::QueryObject<double>::ComputeSignedDistancePairwiseClosestPoints(double) const ()
   from /home/tommy/opt/rlg/drake-build/install/lib/python3.10/site-packages/pydrake/common/../../../../libdrake.so
#13 0x00007fffbffd418e in drake::planning::SceneGraphCollisionChecker::DoCheckContextConfigCollisionFree(drake::planning::CollisionCheckerContext const&) const () from /home/tommy/opt/rlg/drake-build/install/lib/python3.10/site-packages/pydrake/common/../../../../libdrake.so

Version

31989f3

What operating system are you using?

Ubuntu 22.04, Other

What installation option are you using?

compiled from source code using Bazel

Relevant log output

[jwnimmer-tri]

Even though the reproducer is SceneGraphCollisionChecker, I imagine that similar to #21219 (comment) the fundamental problem is that SceneGraph fails to check that the kinematics it's been given are finite, and hands the non-finite values down to FCL which explodes. I imagine the solution is to have SceneGraph fail-fast with an exception in this case, instead of segfaulting.

[SeanCurtis-TRI]

My solution is to put NaN guards at various locations:

• SceneGraph protects itself from Nan on its inputs #22592.
• Collision checker protects itself from user-provided NaN #22593.
• MultibodyPlant hinders writing user NaN to context #22594.