Improve MinimumDistanceConstraint

[hongkai-dai]

TL;DR We think the current MinimumDistanceConstraint is NOT continuous or differentiable everywhere, and this discontinuity causes issue for numeric solvers. We will discuss where that discontinuity comes from, and possible solutions.

Diagnose

The minimum distance constraint is formulated as https://github.com/RobotLocomotion/drake/blob/b110da2f1df1f105e1aaec1a0526316b56efad8b/multibody/inverse_kinematics/minimum_distance_constraint.h#L56-L67

where φ(x) is the smoothed penalty function, for example smoothed linear hinge loss, that approximates the function max(-x, 0) with a smooth piecewise function.

At first glance this formulation looks perfect, it involves the distance function d(q), the smoothed penalty function φ(), and the SmoothMax function. Each function is smooth. So what is the problem?

Culprit

The issue is that when we compute the distance between all pairs of collision geometries, we only return the distance d(q) if d(q) <= d_influence, namely we discard the pair of geometry if their distance is larger than d_influence. This "discarding" operation is non-differentiable. Namely if we change the robot configuration q such that the distance for one pair of geometry changes from below d_influence to above d_influence (or vice versa), then the minimum distance constraint value is non-differentiable (and actually non-continuous).

You might wonder that if dᵢ(q) >= d_influence, then φ((dᵢ(q) - d_influence)/(d_influence - dₘᵢₙ)) / φ(-1) is just 0, why it matters to discard a zero-value entry in SmoothMax(x, 0) if x is always positive? If we used a true Max function instead of SmoothMax, then Max(x, 0) = Max(x) if x>=0, namely discarding a 0-value entry shouldn't change the value of Max function.

Well, the reason is that our SmoothMax formulation doesn't act as Max function. Our SmoothMax will compute the result using every entry, even if the entry is 0. Formally our SmoothMax is

SmoothMax(x₁, x₂, ..., xₙ)  = log(∑ᵢ exp(α xᵢ)) / α

This SmoothMax function approaches Max as α increases, but you can see its value depends on every entry xᵢ. So if we drop any of its value (xᵢ = 0 in particular in our case), then SmoothMax function is not continuous, hence non-differentiable.

Candidate solutions

@RussTedrake and I come up with two possible solutions

Solution 1

We don't discard the the distance even if it is larger than d_influence. After we call https://github.com/RobotLocomotion/drake/blob/b110da2f1df1f105e1aaec1a0526316b56efad8b/multibody/inverse_kinematics/minimum_distance_constraint.cc#L33-L35 which discard the pair of geometries >= d_influence (through broadphase checking or some other mechanism), we then add back value=d_influence in the return distance if a pair of geometry isn't included in signed_distance_pairs. So we still save computation when we compute the signed distance pairs that we can ignore the pairs with distance >= d_influence. This fix seems simple to implement.

Solution 2

The current issue is that our SmoothMax(x) function uses every value of xᵢ, even if xᵢ is very far away from the maximum. We could consider other formulation to approximate Max function, that only smooth the value of Max near its argmax. For example Max(x, y) can be written as Max(x, y) = ReLU(x-y) + y, hence we could consider NewSmoothMax(x, y) = SmoothHingeLoss(x - y) + y, where SmoothHingeLoss is a smooth approximation of ReLU.

The issue with this approach is that it is still possible to cause non-smoothness when the maximal value is very close to 0 (namely all distances are very close to d_influence). In this situation, the 0 value will still affect NewSmoothMax function (for example NewSmoothMax(1E-5, 0) ≠ NewSmoothMax(1E-5)), and discarding 0-value will still cause non-smoothness.

What do you think? @RussTedrake @calderpg-tri @SeanCurtis-TRI

[hongkai-dai]

After investigating further into the code, I don't believe the problem is discarding distance above d_influence. When the distance >= d_influence, we drop a 0-value entry in SmoothMax function. But in our minimum_value_constraint.cc, we actually first initialize the value of SmoothMax(x) with each entry of x being 0 https://github.com/RobotLocomotion/drake/blob/100464346c936eae97646075bc5fe2220217d838/solvers/minimum_value_constraint.cc#L180-L181 , and the number of entries in x being the maximal possible number, which is the num_collision_candidates, https://github.com/RobotLocomotion/drake/blob/100464346c936eae97646075bc5fe2220217d838/multibody/inverse_kinematics/minimum_distance_constraint.cc#L83-L85 hence we essentially have already added back the 0-value entry in SmoothMax function.

[RussTedrake]

related to #17406.