Feature Request: Heightmaps as collision geometry for point contact

[Brian-Acosta]

Per discussion (on slack)[https://drakedevelopers.slack.com/archives/C2WBPQDB7/p1647448357773929], it would be useful to have a height map, interpolated as a triangular mesh, available as a collision geometry for point contact. We currently have an implementation where the robot has hydroelastic compliant feet, but it would be nice to be able to use a faster contact model. cc @sherm1 @amcastro-tri.

An example heightmap (being an open mesh is not required - just that the ground is able to be represented by the surface):

[rpoyner-tri]

Assigning to @joemasterjohn for initial discussion.

[sherm1]

FWIW my first thought is that we could implement as a geometry type similar to the existing halfspace where it could be rigid or compliant with an implicit pressure field since it has an unambiguous depth at every interior point.

[rpoyner-tri]

\cc @DamrongGuoy

[DamrongGuoy]

There seem to be two features requested here:

Feature 1. How to specify height maps in Drake (programmatically, SDF, URDF).

Feature 2. Once we have the class geometry::HeightMap final : public Shape {...}, which queries for point contacts to support first?

Feature 1

Feature 1 is infrastructure and software engineering and asset management. For now, it can disguise indirectly as geometry::Mesh. This feature can share between hydroelastic contact and point contact (and perhaps deformable contact in the future).

Perhaps Feature 1 should have its own issue independent of this issue #16809?

• Add HeightMap to shape_specification.h

Feature 2

Feature 2 is, hmm..., we might have to go beyond FCL (or extend our knowledge of FCL).

Right now even with geometry::Mesh, ComputePointPairPenetration(), ComputeSignedDistancePairwiseClosestPoints(), ComputeSignedDistancePairClosestPoints(), and HasCollisions() take the convex hull of the mesh, according to their doxygen like this:

Meshes are represented by the convex hull of the mesh, therefore the results for Mesh are the same as for Convex.

and ComputeSignedDistanceToPoint quietly ignores the mesh, I think.

For FindCollisionCandidates(), I am assuming it works with Mesh by taking its bounding box (I'll need to look at the code to be sure).

Follow up question

@amcastro-tri when we talk about point contacts, is ComputePointPairPenetration() the most relevant?

[amcastro-tri]

Yes, I'd say today the relevant query for point contact is ComputePointPairPenetration().

For Feature 2, I believe FCL does support what we need, though we might need to unit test properly since I recall Sean complaining about the accuracy of mesh queries. Assuming the functionality actually does work, it'd seem the only necessary chance would be to replace the fcl::Convexd FCL object in ProximityEngine::Impl::Reify(const Mesh&) with an fcl::OBBRSS (or fcl::BVHModel). The documentation for FCL provides examples on how to do this, see here.

[sherm1]

Alejandro pointed out that the simple pressure field I outlined above would only work for heightfields with gentle height variations, like Brian's picture here. That's because the simplification I proposed would yield only vertical pressure gradients like a halfspace has. If there were steep slopes in the heightfield, any interactions with the sloped parts ought to encounter gradients perpendicular to those slopes, which gradients would be substantially different than vertical.

It is possible that the all-vertical pressure field approximation would still be useful if its limitations were well documented. It has the advantage of being really easy to implement and compute! More general heightfields seem like they would inherit most of pressure field problems of a general concave mesh.

Thoughts?

[jwnimmer-tri]

@sherm1 I got lost why you're talking about hydroelastic contact. The initial report says they are using hydro and it's too slow. The request was to add point contact height fields. Are you trying to say that using different shapes with hydro would be sufficiently fast, on par with point contact?

[sherm1]

No, sorry -- I was thinking about adding the new geometry type for general use. A rigid height map is sufficient to address the request here. I thought we'd get the compliant version almost for free, but now I don't think so. Let's just start with a rigid height map that works with point contact (and as the rigid side of a rigid/compliant contact).

[SeanCurtis-TRI]

I'd stay away from using FCL's contact with a mesh.

1. The mesh has no volume, no inside/outside. If the foot contacting geometry is small enough, and velocities are high enough, you can easily have the the foot penetrate so deeply that you either miss the contact, or, more likely, the foot gets sucked in. Heightfields implicitly have volume and if you treat it as such, you'll never miss contact.
2. Heightmaps are generally created on a regular grid. This makes spatial queries very efficient. If you simply turned it into a triangle soup, you'd be back to an O(lg n) look up instead of an O(1) look up.
3. Ultimately, Drake is looking to wean itself off of fcl, bringing this shape type into Drake and supporting it with Drake-specific code, and backed by Drake-authored tests would be advantageous.

[amcastro-tri]

Thank you for the input @SeanCurtis-TRI! love to see you are still around!

[jcarpent]

Hi Drake team,

Here is an implementation of HField with volume within hpp-fcl (a fork of FCL): https://github.com/humanoid-path-planner/hpp-fcl/blob/devel/include/hpp/fcl/hfield.h. Feel free to integrate it within the original FCL.

[RussTedrake]

I still consider this to be a very reasonable feature request.

But I just wanted to leave a bread crumb saying that using a nonconvex mesh with a hydroelastic contact model (user guide) is a potential work-around that we have available today.

Comments in #15744 also suggest that you can make many terrains via the union of other primitives.