[WIP] Implements margin for hydroelastics

[amcastro-tri]

This is motivated by issue #21514, to resolve a pretty accute problem when stacking boxes. I'm sure we solve other similar problems as well. Please refer to that issue for analysis and simulation results, with nice gifs that do a good job at explaining the problem and also the effect of this proposed solution.

This introduced the concept of a "margin" for hydroelastics. Margin is an old concept for point contact. For instance is used by Bullet so that they can still use GJK throuout and avoid the more expensive EPA computation, see Chapter 4 of their user guide.

Here I introduce a Drake version of margin for hydroelastics. I'll copy/paste the documentation I wrote for it in DefaultProximityProperties, that I think does an okay job at explaining the idea:

/** Specifies a thin layer of thickness "margin" (in meters) around each
   geometry.

   @note In Drake, margin effectively "shrinks" the geometry of an object
   inwards by a magnitude equal to the margin. This is as if we had "shaved-off" a
   thin layer of thickness `margin` all around the surface of the object. Since
   the margin is meant to be small, this is usually negligible in most cases.
   Recall you can still overwrite this default value of margin for each geometry
   through its ProximityProperties. The margin can be zero, effectively removing
   its effect.

   If the thin margin layer of two objects overlaps, there will be no contact
   nor contact forces. However, contact constraints will be added allowing our
   discrete contact solvers to predict if a contact "will" occur at the next
   time step. This leads to additional time coherence and stability.
   Typically, a thin layer of 0.1 mm to 1.0 mm is more than enough for
   most robotics applications. This is not an "action-at-a-distance" trick,
   there is no contact when the thin margin layers of two objects overlap. The
   margin is simply a "cheap" mechanism to avoid significantly more complex and
   costly strategies such as Continuous Collision Detection. **/

Note:

I only implemented box and ellipoid as proofs of concept. I implemented ellipsoid as an example of a pressure field with no unique reference length ("foundation depth" in the Elastic Foundation Model) and with non-uniform pressure gradient at the surface of the geometry.

Implementation:

I implemented this by allowing a (small) negative pressure field at the surface of hydro meshes. The "true" geometry of an object is then defined by the "zero-level set" of this pressure field.

Our contact solvers already know how to deal with negative pressure values and do interpret them as producing no-forces. However, this allows to add constraints before a contact stablishes, increasing time-coherence and stability.

---

This change is 

[SeanCurtis-TRI]

our contact response is a function of the pressure gradient and we have set up our pressure fields such that the pressure gradient is discontinuous across the boundary of the collision geometry

I didn't completely follow this.

• "Collision geometry" refers to "contact surface"?
• I thought we were integrating the pressure. So, even if pressure is discontinuous, its integral is continuous.
• I sincerely hope that a quadratic pressure field is not a required solution -- it will absolutely kill our intersection code that benefits from linear pressure fields.

[xuchenhan-tri]

"Collision geometry" refers to "contact surface"?

No, by collision geometry I mean the tetrahedral hydroelastic mesh.

I thought we were integrating the pressure. So, even if pressure is discontinuous, its integral is continuous.

Yes, we are integrating the pressure and the integral is indeed continuous. But that is only part of the contact force. We model the normal contact force as k (phi + tau vn) with k defined in equation 16 of https://arxiv.org/pdf/2110.04157. phi is also defined there as phi0 + dt vn. The integral you mentioned is the k phi0 term and that's nice and continuous, but the k*vn term is not as contact emerges.

I sincerely hope that a quadratic pressure field is not a required solution

I'm not suggesting that it is required. I think this PR is already showing empirical evidence that there are other solutions to the problem (although I'd love to understand it more on the theoretical level). I'm just wondering whether a quadratic pressure field would also solve the instability problem without resorting to using margins. If that's the case, it might be worth trying in Alejandro's "new hydro model" that doesn't require intersecting pressure fields.

[SeanCurtis-TRI]

Previously, xuchenhan-tri wrote…

> "Collision geometry" refers to "contact surface"? No, by collision geometry I mean the tetrahedral hydroelastic mesh. > I thought we were integrating the pressure. So, even if pressure is discontinuous, its integral is continuous. Yes, we are integrating the pressure and the integral is indeed continuous. But that is only part of the contact force. We model the normal contact force as k * (phi + tau * vn) with k defined in equation 16 of https://arxiv.org/pdf/2110.04157. phi is also defined there as phi0 + dt * vn. The integral you mentioned is the k * phi0 term and that's nice and continuous, but the k*vn term is not as contact emerges. > I sincerely hope that a quadratic pressure field is not a required solution I'm not suggesting that it is *required*. I think this PR is already showing empirical evidence that there are other solutions to the problem (although I'd love to understand it more on the theoretical level). I'm just wondering whether a quadratic pressure field would *also* solve the instability problem *without* resorting to using margins. If that's the case, it might be worth trying in Alejandro's "new hydro model" that doesn't require intersecting pressure fields.

So, the boxes are one of Drake's tessellated primitives. That means there are two triangles on each box face. Would you also think stabilization might be improved if there were more faces? (more elements approximately equal to higher order approximations, right?)

[xuchenhan-tri]

Would you also think stabilization might be improved if there were more faces?

I don't think that's going to help. Like I mentioned in the previous reply, I suspect the instability comes from the k*vn term with k defined in equation 16 of https://arxiv.org/pdf/2110.04157. Neither of those terms is related to the density of the tessellation.

But of course, everything I have said in this thread is hypothesis/speculation.

[SeanCurtis-TRI]

geometry/scene_graph_config.h line 120 at r3 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Good point Jeremy. Let me try this on those cases.

I have a proposal where the whole thing gets simpler.

If we relax the promise that after inflating the mesh's zero level set is the same as before inflation, then we can inflate general meshes. In this case, it would be approximate. But as this only applies to compliant meshes anyways, the idea of "where contact happens" is fuzzy at best.

If we do this, then:

(a) We don't have to distinguish between proximity geometry types. (b) LBM is far more likely to benefit from this as all geometries will have the margin and for the huge amount of compliant mesh-compliant mesh contact they have, that may be essential for this approach to provide value (at least to them).

@amcastro-tri Let's have a face to face to see if I can persuade you as to my idea.