QueryObject::ComputeSignedDistancePairwiseClosestPoints can return NaN valued gradients

[SeanCurtis-TRI]

Problem

The query QueryObject::ComputeSignedDistancePairwiseClosestPoints evaluates the distance between two geometries. It reports the distance between them, "witness points" to that distance, and a unit normal indicating direction of fastest increase in the distance.

If the two objects are touching, the normal will be filled with NaNs.

Ultimately, it comes down to the computation of that normal: the normalized displacement vector between the two witness points. When distance is functionally zero, that normalization doesn't work.

This also touches related code (with various TODOs).

Solution

Ultimately, the way to resolve this is to use the witness points to find normals on the corresponding geometries. Given those normals we can infer a contact normal. It would be the normal of any plane that can serve as the tangent plane at that point of contact such that all of the geometry of one shape lies on one side of the plane, and the other geometry lies on the other side -- for convex geometry, it's very much "all of the geometry", for non-convex geometry, it just needs to be the geometry local to the contact point.

For continuous surfaces (e.g., sphere, capsule, cylinder barrel), the normals at the witness points should already popint in anti-parallel directions and the contact plane must be perpendicular to both simultaneously. In the case of discrete geometry (convex polytopes, cylinder edges, cone edges, etc.) if the witness point is on a crease (vertex or edge), the normal is not defined and can actually be drawn from a "cone" of directions (bound by the normals of the adjacent features). How do we pick one of those normals? Ideally, this plane would be selected to maximize continuity of the normal direction when we perturb the two geometries slightly.

A note on signed distance and AutoDiffXd

If we have this ability to meaningfully select these surface normals, we can use it to hoist FCL results and compute AutoDiffXd-valued query results. See #14571.

[sherm1]

For edge/edge (non-parallel) contact a unique normal is the cross product between the two edges (each witness is on an edge). For vertex/face or edge/face the face normal is the winner. Parallel edges, vertex/edge and vertex/vertex are measure-zero occurrences. Probably should return something non-NaN though.

[RussTedrake]

I just ran into this (i had to dig through many layers of turtles to get to this root cause). Here is a small reproduction:

  // Three boxes.  Two on the outside are fixed.  One in the middle on a prismatic
  // joint.  The configuration space is a (convex) line segment q ∈ (−1,1).
  const char urdf[] = R"""(
  <robot name="boxes">
    <link name="fixed">
      <collision name="right">
        <origin rpy="0 0 0" xyz="2 0 0"/>
        <geometry><box size="1 1 1"/></geometry>
      </collision>
      <collision name="left">
        <origin rpy="0 0 0" xyz="-2 0 0"/>
        <geometry><box size="1 1 1"/></geometry>
      </collision>
    </link>
    <joint name="fixed_link_weld" type="fixed">
      <parent link="world"/>
      <child link="fixed"/>
    </joint>
    <link name="movable">
      <collision name="center">
        <geometry><box size="1 1 1"/></geometry>
      </collision>
    </link>
    <joint name="movable" type="prismatic">
      <axis xyz="1 0 0"/>
      <limit lower="-2" upper="2"/>
      <parent link="world"/>
      <child link="movable"/>
    </joint>
  </robot>
  )""";
  systems::DiagramBuilder<double> builder;
  auto [plant, scene_graph] =
      multibody::AddMultibodyPlantSceneGraph(&builder, 0.0);
  multibody::Parser parser(&plant);
  parser.AddModelsFromString(urdf, "urdf");
  plant.Finalize();
  auto diagram = builder.Build();
  auto context = diagram->CreateDefaultContext();
  systems::Context<double>& plant_context =
      plant.GetMyMutableContextFromRoot(context.get());
  systems::Context<double>& scene_graph_context =
      scene_graph.GetMyMutableContextFromRoot(context.get());

  // Set the movable box to the boundary of collision.
  plant.SetPositions(&plant_context, Vector1d(1.0));

  const double kInfluenceDistance{5.0};
  auto query_object = scene_graph.get_query_output_port().Eval<
      geometry::QueryObject<double>>(scene_graph_context);
  const std::vector<geometry::SignedDistancePair<double>>
      signed_distance_pairs =
          query_object.ComputeSignedDistancePairwiseClosestPoints(
              kInfluenceDistance);
  for (const auto& pair : signed_distance_pairs) {
    log()->info("nhat_BA_W = {}", fmt_eigen(pair.nhat_BA_W.transpose()));
  }

prints

[2023-07-10 21:52:49.726] [console] [info] nhat_BA_W = nan nan nan
[2023-07-10 21:52:49.726] [console] [info] nhat_BA_W = -1  0  0

This makes it impossible for me to use the proximity engine in IrisInConfigurationSpace -- which is very specifically trying to find configurations at the boundary. cc @hongkai-dai .

It is reasonable to increase the priority on resolving this?

[DamrongGuoy]

As @SeanCurtis-TRI explained in the problem description, except for pairs of sphere-{box, capsule, cylinder, half space, sphere}, we call FCL for a pair of geometries like this:

https://github.com/RobotLocomotion/drake/blob/b35fa8277c449aee51024adc5103c2bbaf330f08/geometry/proximity/distance_to_shape_callback.cc#L65-L70

Then, we calculate the gradient or set it to NaN depending on whether the two geometries almost touch each other:

https://github.com/RobotLocomotion/drake/blob/b35fa8277c449aee51024adc5103c2bbaf330f08/geometry/proximity/distance_to_shape_callback.cc#L93C1-L97C4

  if (std::abs(result.min_distance) < kEps) {
    pair_data->nhat_BA_W = Eigen::Vector3d(kNan, kNan, kNan);
  } else {
    pair_data->nhat_BA_W = (p_WCa - p_WCb) / result.min_distance;
  }

We will have to calculate nhat_BA_W ourselves when std::abs(result.min_distance) < kEps.

I'm still thinking about how to compute the solution that Sean explained. It will involve case analysis. Thank you for being patient. I'll try to update again soon.

[SeanCurtis-TRI]

The vague idea was something along the lines of, given a shape and a point (lying on its surface), compute the normal at that surface.

Generally, I think that's a tractable problem. Just a family of functions that take each shape in tern. Obviously, they're not all equally easy or equally cheap. But as this would only get triggered when the contact points are very close, it's probably worth paying. And for most of the primitives it'd be pretty cheap.

[SeanCurtis-TRI]

@DamrongGuoy It would be nice if we could include a list of all the geometry pairs and the state of their resolution. As we're taking pairs bit by bit, it would be good to have a clear view of the state of things.

[DamrongGuoy]

@SeanCurtis-TRI I'll create a table of the state of things after #20033 lands. Thanks.

[DamrongGuoy]

20033 has landed (thanks @joemasterjohn and @SeanCurtis-TRI). This table of gradient support is what we have now.

https://github.com/RobotLocomotion/drake/blob/master/geometry/proximity/distance_to_shape_touching.h#L34-L43

  |           | %Box | %Capsule | %Convex | %Cylinder | %Ellipsoid | %HalfSpace |  %Mesh  | %Sphere |
  | --------: | :--: | :------: | :-----: | :-------: | :--------: | :--------: | :-----: | :-----: |
  | Box       | Ok   |  ░░░░░░  |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
  | Capsule   | NaN  |  NaN     |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
  | Convex    | NaN  |  NaN     |  NaN    |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
  | Cylinder  | NaN  |  NaN     |  NaN    |  NaN      |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
  | Ellipsoid | NaN  |  NaN     |  NaN    |  NaN      |   NaN      |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
  | HalfSpace | NaN  |  NaN     |  NaN    |  NaN      |   NaN      |   NaN      |  ░░░░░  |  ░░░░░  |
  | Mesh      | NaN  |  NaN     |  NaN    |  NaN      |   NaN      |   NaN      |  NaN    |  ░░░░░  |
  | Sphere    | Okᵃ  |  Okᵃ     |  Okᵃ    |  Okᵃ      |   Okᵃ      |   Okᵃ      |  Okᵃ    |  Okᵇ    |

[DamrongGuoy]

I'll start updating QueryObject doc in:

• 20679

[DamrongGuoy]

@RussTedrake which pair should I do next?

[RussTedrake]

I think you've got it to a good place for now. I've worked around the problem in the short term, so am not immediately stuck on missing pairs in that documented support. But I'll call them out if and when I'm stuck again. Thanks.