SAP failure case with Newton line search

[amcastro-tri]

What happened?

@chen-tianjian reported this problem in the Drake developers Slack channel.

In one of their simulations SAP fails with the message:

Failure at multibody/contact_solvers/newton_with_bisection.cc:21 in DoNewtonWithBisectionFallback(): condition 'bracket.inside(x_guess)' failed.

@mshalm analyzed the problem and found out that our initial guess can sometimes be outside the solver's bracket, thus causing the exception.

The solution is to provide a guess inside the bracket. For as long the guess is inside the bracket, the solver should always succeed. Options are:

• Midpoint in the bracket
• Assume a quadratic form of the cost at either alpha=0 or alpha=alpha_max. We'd choose the side with the largest gradient. For quadratics it'd correspond to the side "closer" to the root, and probably true for non-linear costs as well.

Related to #19322, it's in the critical path.

[amcastro-tri]

Progress note: As @mshalm pointed out, it is true that we are ussing d²ℓ/dα²(αₘₐₓ) instead of the (correct) d²ℓ/dα²(α=0) to estimate the initial guess of the line search, in this line.

However, at that point we already verified that dℓ/dα(α=0) < 0 and therefore that line (which takes the maximum with αₘₐₓ) effectively computes a guess that is guaranteed to be within the line search bracket [0, αₘₐₓ]. Therefore, unless there is a floiting point exception, the check for the guess to be inside the bracket from within DoNewtonWithBisectionFallback() should always succeed.

My guess is that somehow alpha_guess is becoming nan, which would explain the failure. Now, this does not explain the original cause of the problem, which might be very far from SAP's source.

@chen-tianjian, @mshalm, could you confirm?

[chen-tianjian]

Yes, I checked in initial guess and it is nan