Drake: MultibodyPlant::DoCalcTimeDerivatives() throws (i.e., running in continuous mode)

[edrumwri]

An error message is output that the articulated body inertia is not valid. No existing inertial or kinematic parameter checks have triggered any problems before the change to the O(n) algorithm.

The issue appears to be that the tolerances are set to tight, as this comment indicates:

https://github.com/RobotLocomotion/drake/blob/161b467072cb6df305b58e19b341cd6403d70c79/multibody/tree/articulated_body_inertia.h#L170

Changing the tolerance, just slightly, from -1e-14 to -7.5e-13 keeps the exception from being thrown. We can provide the SDF file we are using if it would be helpful for debugging (my initial thought is that it would not be).

Marking this medium priority because we are able to get around this by disabling that check but others are likely to hit this landmine, particularly when the ABA becomes the default. Consider increasing to high priority if it is not fixed before the ABA algorithm becomes the default.

[amcastro-tri]

Ideally I think we'd like having a reference scale to perform the check. Something like the trace?
@sherm1, @edrumwri, @mitiguy, do you have any recommendations on how to improve this check and/or obtain a reference scale?

[sherm1]

Ideally I think you would want to use a matrix norm as the scaling factor -- 2 norm or Frobenius norm would be good. I'm not sure the trace would be a good substitute. It would be interesting though to investigate the specifics here to see what makes this particular matrix so bad -- I would have expected real ABIs to be solidly posdef.

[amcastro-tri]

I agree. @edrumwri, do you have zero masses in your file? or some badly set configuration? I've actually noticed recently that some integrators seem to leave the system in some bad intermediate state (I believe RK5 is one of them), though I haven't been able to trace down the cause. Does this problem appear with all integrators (then we'd most likely blame MBP) or with a specific subset of integrators (in which case we could think there's some bad state being set).

[mitiguy]

Hmmmm. This does seem odd -- perhaps some programming error or modeling errors to track down.
As for a check, perhaps we should consider the checks that were used for a single rigid body and see which are plausible for an articulated rigid body inertia.

[edrumwri]

No zero masses. In fact, the masses of the five links are all identical and well scaled (2.7kg). I didn't investigate much since we had no difficulty with this model when using the old cubic-time CRB algorithm (an inertial problem should also show up in the mass matrix factorization there, right?), since a small tolerance change caused this to pass, and since the comment in the code indicated that the tolerance might be too tight.

FYI @amcastro-tri we're using the default RK3 integrator.

[amcastro-tri]

Thanks @edrumwri. Any chance you could have a simplified case that reproduces this failure?

[edrumwri]

Yep- will work on this later today.

[edrumwri]

Status update: just sent @amcastro-tri a minimal example that reproduces this bug.

[amcastro-tri]

Thank you @edrumwri. I didn't look at your example yet though I gave this problem some thought. Articulated Body Inertias P are positive-definite. However ABIs projected across a mobilizer P⁺ are only positive semi-definite. Therefore round-off errors could trigger your assertion when computing P⁺.

In particular, this very single line of code (which already has a TODO) comes to mind. I'll investigate.

[sherm1]

The analogous Simbody code symmetrizes this computation, allegedly to avoid roundoff errors. That _might_ be better but I don't know that I ever determined that, and Simbody doesn't have the positive-semidefinite check that we have in Drake.

[amcastro-tri]

Update:

I found out a similar error recently with an Atlas simulation. The same invariant check is triggered, _only in Debug builds._

Further investigation reveals that:

1. As suspected, the propagation of round off errors leads to slightly negative eigenvalues in g*U.
2. Making the product g*U symmetric does not help much, with corrections towards positive much smaller than machine epsilon (at least for the case I tried).
3. What does help is changing the tolerance from a hard-coded value (which by the way notice that it has units) to a dimensional tolerance that does scale with the magnitude of g*U.

g*U is a 6x6 matrix. Its 6 eigenvalues are computed into eigvals here. It was found that a much better tolerance is tolerance = eigvals.norm() * std::numeric_limits<double>::epsilon() * kEta, with kEta a factor between 2.0 (tight), 10.0 (works ok) to 100.0 (loose).

I propose that to be the fix.

[sherm1]

@amcastro-tri is this on your radar? It only blows up in Debug but I'm thinking that someone in Debug already has enough troubles! (Or could be a warm-up project for a new contributor.)