Drake: Implement option to non-continuous hydroelastic query.

[amcastro-tri]

The discrete approximation introduced in #14574 is not bound by continuity requirements. This allow us to sacrifice continuity in exchange to simplifications to the query in order to reduce the triangle count (smaller contact problem).
One of the simplest simplifications we explored consists in not splitting a three sided polygon (aka triangle) into a fan of triangles around the centroid. This showed performance improvements between 20% to 30% in simulation of robotic systems with grasping.

A much better solution

I just realize that there is yet a better way to do this. While not splitting a three sided polygons only applies to triangles that are fully immersed inside a tetrahedron, we can reduce polygon counts by "directly" reporting polygons instead.
Discrete hydro does not need much, just a point and the area of the polygon. Therefore I propose that the discrete query actually reports the centroid of each clipped polygon and its area. Notice that this solution is equivalent to the one above for triangles. However, it does also apply for 4 to 7 sided polygons. Close examination of a few examples reveals this would reduce triangle count drastically for most cases (by a factor equal or larger to 3, while it was only about 20-30% with the solution in the original post).

Visualization

Notice we could still send triangles to the visualizer. So the query would need to somehow support this "double representation" so that "area+centroid" is used by the contact solver and "triangles" is used to fill in the contact results for viz.

This small patch implements the simplification though a proper design of updated APIs, if needed, might be in order.

[amcastro-tri]

I just realize that there yet a better way to do this. While not splitting a three sided polygons only applies to triangles that are fully immersed inside a tetrahedron, we can reduce polygon counts by "directly" reporting polygons instead.
Discrete hydro does not need much, just a point and the area of the polygon. Therefore I propose that the discrete query actually reports the centroid of each clipped polygon and its area. Notice that this solution is equivalent to the one above for triangles. However, it does also apply for 4 to 7 sided polygons. Close examination of a few examples reveals this would reduce triangle count drastically for most cases (by a factor equal or larger to 3, while it was only about 20-30% with the solution in the original post).

[SeanCurtis-TRI]

This alternate formulation is far more complex to implement. Currently the mesh for a ContactSurface is represented exclusively as a triangle mesh. To report a mesh with polygons would require changes in types. The best you can hope for in the short-term is a minimum triangulation of n-gons.

[amcastro-tri]

We can talk f2f. I wasn't suggesting to report a mesh with polygons. What I was suggesting is to report (probably in a different query with different return data) new data as point+area+pressure+gradient. Probably we could augment the hydro query to report this instead. That'd have the advantage of still being able to visualize it as a triangle mesh.

[SeanCurtis-TRI]

I wasn't suggesting you were suggesting mutating the current API. You and I agree, to accomplish what you want requires a new API. Hence my comment, "this alternate formulation is far more complex to implement". I didn't spell out the complexity comes from a new API, I thought that was implied by the fact that the current API is incompatible.

[DamrongGuoy]

(Update the plan: change the name from ComputeDiscreteContactSurfaces() to ComputePolygonalContactSurfaces()).

For this CY2021 Q2, our solution is to provide a new query called QueryObject::ComputePolygonalContactSurfaces() suitable for non-continuous hydroelastic contact solver (NCH for brevity); however, both this new query and the existing query for continuous hydroelastic contact solver (CH for brevity) will have the same return type (std::vector<ContactSurface>) with different semantics. For Q2, we decided to use the same return type so that we can continue to use existing infrastructures and deliver solution quickly. Unlike the query for CH, the new query for NCH will return one representative triangle for each contact polygon. The triangle will have the same centroid, area, pressure(at centroid), and pressure gradient(per face) as the contact polygon. Theoretically NCH can switch between the two queries and compute the same contact force and moment. The new query for NCH will speed up the simulation by returning fewer than 1/3 triangles of the existing query for CH.

This movie is from my feasibility study in an experimental code. The choice of how to draw a representative triangle is arbitrary at the moment (and will become immaterial in the post-Q2 future):

https://user-images.githubusercontent.com/42557859/116607176-860ee680-a8e6-11eb-85d7-285c3ff6ad35.mp4

After Q2, we will switch to a new return type tailored to NCH, as we improve infrastructure and supporting code.

The implementation plan for CY2021 Q2 is:

1. New public API QueryObject::ComputePolygonalContactSurfaces().
2. New internal utility function AddPolygonToMeshDataAsOneTriangle().
3. Introducing an internal boolean at GeometryState level that pushes all the way down, so we can choose between the existing AddPolygonToMeshData() and the new AddPolygonToMeshDataAsOneTriangle().
4. Exercise the boolean in mesh_intersection.
5. Exercise the boolean in mesh_plane_intersection.
6. Exercise the boolean in mesh_half_space_intersection.
7. Modify multibody_plant to make the call into the new API when it's doing NCH.
8. New public API QueryObject::ComputePolygonalContactSurfacesWithFallback() to support the hybrid point contact + hydroelastics contact.

Thanks to @SeanCurtis-TRI for helping me with the implementation plan.

[sherm1]

Great plan -- thanks!

[amcastro-tri]

Thank you @DamrongGuoy! all this makes sense to me.
Only one question. @SeanCurtis-TRI made me realize that this query actually IS continuous also! the only reason we split polygons into triangles really is because we know how to integrate numerically (using high orders quadrature rules) for triangles, but not for general polygons. That is why, when using a first order quadrature, we can simply use a polygon's centroid and area without having to split into triangles.

Long story short, should we have a better name instead of ComputeDiscreteContactSurfaces()? the result really is a set of disjoint polygons (even if represented by their area only) that cover the entire surface. Would that be the distinct property that should show in the name? say, ComputePolygonalContactSurfaceDiscretization()

[SeanCurtis-TRI]

I have and will continue to advocate for the name ComputePolygonalContactSurface. We will be shipping the return value with an actual mesh consisting of polygons -- the polygons are a byproduct of producing the data that dynamics cares about and are necessary for visualization.

[DamrongGuoy]

Ok. Thanks. I changed the name to ComputePolygonalContactSurface and updated the plan above.

[SeanCurtis-TRI]

Just a reminder: there have been recent changes to hydroelastic code to enable auto-diffable contact. I'm not sure where you've branched from, but you should make sure that your changes are compatible with current state of master. (And, I believe I have one more PR that might create merge difficulties for you. I'll get that submitted today.)

[DamrongGuoy]

Thanks @SeanCurtis-TRI . Indeed I have been rebasing on master and resolving merge conflicts several times.

[SeanCurtis-TRI]

Excellent! Seems you're well on top of things. The last PR (at least that touches geometry) is in flight: #15040. That is the last one that might interfere with your efforts.

[DamrongGuoy]

As mentioned above, rather than going straight to the final PolygonalContactSurface, we are providing an intermediate solution in CY2021 Q2. This will allow us to provide immediate performance improvements even without the fully designed system. Below, I’ve referred to the intermediate solution as the representative triangle of a contact polygon. A contact polygon is an intersection between a rigid triangle and a compliant tetrahedron. For each contact polygon, we will have one representative triangle with the same centroid, area, pressure value, and pressure gradient as the contact polygon. This is more economical than the existing representation that triangulates a contact polygon around its centroid.

Note:

1. The code for the representative triangles will have a short lifespan as we move from representative triangles to fully implemented contact polygons, and I will put TODO to remind that they will have a short lifespan.

2. The representative triangle will have the same relevant properties as the contact polygon for the first-order numerical integrations. The representative triangles and the contact polygons should give the same force and moment for discrete hydroelastics. Therefore, users of discrete hydroelastics should see the same simulated physics between the existing triangulated contact surfaces, the intermediate solution using representative triangles, and the final solution using contact polygons directly.

This is the plan for the sequence of PRs.

Part I. Code with short lifespan to support representative triangles. For quick development, the code is slightly duplicated and less than optimal.

• [x] PR 15034: Calculate centroid and area of a convex polygon.
• [x] PR 15035: Utility to add one representative triangle for each contact polygon.

Part II. Code with long lifespan applicable to both the representative triangles and the final solution using polygons in the future.

• [ ] PRs 15131,...: Switch between two representations of output from three geometric intersection codes (mesh_intersection, mesh_plane_intersection, and mesh_half_space_intersection).
• [ ] Enhance hydroelastic_callback to have a switch between two representations of contact surfaces (involve DispatchRigidSoftCalculation(), CallbackData, MaybeCalcContactSurface()).
• [ ] New public API of QueryObject: ComputePolygonalContactSurfacesWithFallback() and ComputePolygonalContactSurfaces().
  
  
  
  • ProximityEngine supports the API by setting the switch in hydroelastic_callback.
  
  
  • GeometryState supports the API.
  
  
  • QueryObject supports the API with appropriate documentation.
    
    
    
    
    
    • Inform users that the return type in this implementation is ContactSurface that uses the representative triangles, and, in the future, the return type will change to PolygonalContactSurface using the contact polygons.
    
    
    
  
  
• [ ] Multibody callsite.
  
  
  
  • MBP calls the new API when using discrete hydroelastic.
    
    
    
    
    
    • This engagement will change when the API returns PolygonalContactSurfaces instead of ContactSurfaces.
    
    
    
  
  
  • Provide an option to use discrete hydroelastics with the original triangulated contact surfaces, so users can produce movies showing clean contact surfaces.
  
  

Part III. (Not a part of this CY2021 Q2 development). Final solution using PolygonalContactSurface. Implementation and PR details have not been worked out yet. I speculate that these are the tasks, and they may change.

• Introduce PolygonalContactSurface and PolygonalSurfaceMesh classes.
• Refactor geometric intersection code to maximize re-use for the two different return types (ContactSurface for triangles, and PolygonalContactSurface for polygons).
  
  
  
  • It largely differs in the last stage of how a contact polygon is added to the mesh. This may be done via a callback function.
  
  
• Add visualization of contact polygons.
• Update QueryObject API and MBP for the new return type.

[sherm1]

Great plan -- very clear!