Drake: Distance to point reports partial information vis a vis non-unique gradients

[SeanCurtis-TRI]

Problem

The method QueryObject::ComputeSignedDistanceToPoint returns results in type SignedDistanceToPoint. Part of that declared struct is the field SignedDistanceToPoint::is_grad_W_unique, which, if true, indicates that the stored gradient vector is unique for the signed distance field at the query point.

In support of that, we have code like this, to compute signed distance and gradient in a box. Particular care is used to determine if the query point lies at one of the box's vertices or on its edges. In these cases, the signed distance does not have a unique gradient and the returned result reflects that.

However, those are not the only cases where the gradient is not unique. If the point lies inside the box anywhere on its medial axis, the gradient is likewise not unique. The same is true for a cylinder (and has been explicitly called out) and all shapes, generally. The signed distance field has a discontinuity in its spatial gradient on the medial axis. The problem is that the code in distance_to_point_callback.h is ignoring the internal medial axis.

• For spheres, we detect distance to the medial axis (the sphere center) based on a tolerance.
• Half spaces have no such problems; the gradient is always unique.
• The capsule makes use of the sphere's logic in whether near the sphere or near the barrel.
• Box and cylinder ignore medial axis as discussed above.

Essentially, the flag is an inconsistent lie.

Solution

Naively, the solution is to directly correct this so that the flag is always meaningful. Detect the points proximity to all shapes' medial axes with comparable tolerance.

There's a problem with this solution:

1. Reporting that a gradient is not unique has very little utility. We currently don't have any algorithms that make use of this knowledge. But, even if we did, there's very little meaningful action that could be taken given such limited information.
2. It would be far better if in addition to the knowledge that a gradient wasn't unique, that we could communicate the space of valid gradient vectors. For spheres, it would be the unit sphere. For cylinders, it could be a pair of vectors (near the circular edge) or a ring (on the central axis of the cylinder). For capsules, either a hemisphere or ring. For discrete surfaces (e.g., boxes), this would usually be a finite set of vectors (the normal of each equidistant face) or, if on the surface, a contiguous portion of the unit sphere (at the box's vertex) or unit circle (on the box's edge). Etc. To fully communicate the space of gradient vectors will require some careful design. However, as with the previous item, we currently have no code that would exercise it.

So, we are left with several options:

1. Leave the code alone (albeit with a sprinkling of TODOs referencing this issue).
2. Clean out the flag reporting uniqueness entirely (as no one can actually use it at this point). Cull it entirely (via deprecation) until we have a pressing need to distinguish non-unique gradients. Just so long as the gradient always be drawn from the set of valid gradients.
3. Fill in the code so that the flag is correct in all cases (the "naive" solution indicated above). This would arguably be pushing the code toward a full solution where we also define the space of non-unique gradient vectors, but, however such a thing would be implemented, it feels like that putative implementation plus this bool field would be redundant.
4. Devise the means for communicating the valid space of gradient vectors and simply implement that, replacing the current mechanism. At this point, we still don't have any users of this functionality, and it's not clear what the API should be -- how would the end user want to interact with the space of valid gradient vectors considering it can take so many different forms?

Conclusions

The code, as is, lies. That's not a good thing. The logical step to take is to make it stop lying. That can be done by adding or removing code. Given the presumption that no one cares about the current promise, the code would be simpler if we go with option 2 and clean it out.

[SeanCurtis-TRI]

@hongkai-dai Anything to add?

[hongkai-dai]

Sorry I made a mistake in our video chat about computing the subgradient

For discrete surfaces (e.g., boxes), this would usually be a space consisting of the convex combination of a finite enumerated set of vectors

Since the term grad_W always has a unit length, and a convex combination of unit length vectors doesn't have a unit length, I believe we only need to report the finite set of vectors, instead of its convex combinations.

Apart from this minor issue, I agree with everything you mentioned above.

[sherm1]

Option 2 sounds great to me! As I understand it: always return a valid gradient or subgradient, nuke the flag.
Leaving the flag would be marginally better, I think, if it can be made correct since it could be used as an error condition or a hint as to why some computation failed ("penetration was too deep").

[SeanCurtis-TRI]

@hongkai-dai Yeah. I know what you mean. I cringed when I wrote it. Even with what I had in mind it wasn't truly a "convex combination" in the lerp sense. At best I meant it in the slerp sense. But even that's not true. I've edited the text above.

@sherm1 Two thoughts on leaving the flag:

1. It can be made correct.
2. It doesn't necessarily communicate "penetration was too deep". In the case of things like spheres and capsules, it is true that the medial axis is buried deep inside the shape. But in cases like boxes and cylinders, the medial axis connects to the surface, so you can get non-unique gradients at any depth.

[DamrongGuoy]

I thought we talk about this before, long time ago. Unfortunately we didn't document that (or I couldn't find it). I'm not even sure that I remember it correctly or not. This may not be 100% true. At that time, we were worried that there was no perturbation scheme that was always consistent. There was one version that I wrote (I'm not sure whether it got into master or not) that always return one fixed vector, say Gx, as the gradient, i.e. grad_W = X_WG * Gx. Then, someone was worried that an arbitrary X_WG will change grad_W arbitrarily, or something like that (which I did not completely understand). In the end, @SeanCurtis-TRI (or @hongkai-dai ) said that we will return that flag to warn users to be careful.

I think this is a good lesson in software engineering that we should engineer the solution according to users needs, and document it, instead of speculating. Gather users requirement first.

[hongkai-dai]

I believe when we discussed computing grad_W, I raised the concern that sometimes grad_W is not unique, so if we report a single grad_W, the user might be fooled to think that is the unique gradient, hence we add the flag is_grad_W_unique. I feel I didn't think through this carefully, that the flag is_grad_W_unique is not very useful, as it only provides partial information.

[DamrongGuoy]

Thank you @hongkai-dai .

It think option 2 (Clean out the flag reporting uniqueness entirely) is the best for now. It will probably simplify the code and the unit tests too.

[SeanCurtis-TRI]

I've got an implementation that does number 3 - leave the flag and make it so it doesn't lie. The testing is incomplete because we don't actually test the point-to-cylinder case (which is derived from the box case). So, I'd need to flesh that out.

The branch is here: https://github.com/SeanCurtis-TRI/drake/tree/PR_point_box_distance_fix

So, it comes down to voting. Do we cull or patch? If you want me to finish the branch and leave the flag vote thumbs up, if you want me to deprecate the field, vote thumbs down.

[DamrongGuoy]

:-1: I vote to deprecate the field. Wise men said Code is Liability.

[sherm1]

I still like the _idea_ of that flag but @DamrongGuoy does make a strong point.

[DamrongGuoy]

And thanks to @SeanCurtis-TRI for discovering this. That code has been with us for so long that I forgot how it worked.

[SeanCurtis-TRI]

So, it seems like there's no strong vote in favor of keeping the flag and enough negative sentiment to suggest that it's going to get cut. I'll do it.

[SeanCurtis-TRI]

BTW The SignedDistancePair has the same kind of flag. I'll be ~removing~ deprecating it for the same reason.