Drake: Speed continuous MultibodyPlant with contact and friction

[edrumwri]

MultibodyPlant requires the ability to rapidly simulate contact with Coulomb friction as a continuous system (i.e., simulation through our initial value problem solvers, aka "integrators"). Whether the contact model is hydroelastic (in development) or the present point-based contact model, we require this capability.

We know this problem to be computationally stiff when the compliance is low and the dissipation is significant (i.e., any elastic waves attenuate rapidly). However, Drake's only implicit integrator (ImplicitEulerIntegrator) is not simulating these multibody contact scenarios efficiently at the moment. Preliminary investigation indicates that the IVP solver is suffering from Newton-Raphson convergence failures (which are indicative of a discontinuity in the ODEs).

[edrumwri]

Update: I modified the manipulation station to use MBP in continuous time mode with the implicit Euler integrator (accuracy=1e-1, max h=1e-2). I then teleoperated the manipulator. As I teleoperated the manipulator in free-space, integration was very fast (on the order of 10 NR iterations and 10 ODE evaluations), performance comparable to the discrete-time stepper. I then maneuvered the manipulator such that it was contacting the ground, at which point performance on implicit Euler dropped by approximately two orders of magnitude. Investigation indicated that the problem was convergence failures (i.e., error control was not responsible). Removing the frictional forces from MBP would regain the prior (good) performance, so clearly friction is responsible. A clue: the performance did not "hiccup" and then improve but stayed poor as long as the manipulator remained in contact with the ground. _We would expect stiction to play an insignificant role in this problem_ (and many others), as transitions from sliding to sticking states occur rarely in most scenarios. In other words, it is unclear how a clever step limiting Newton-Raphson strategy like Alejandro uses in the discrete stepper could benefit the continuous time plant.

I hypothesized that the low accuracy of implicit Euler was causing spurious transitions into and out of stiction and increased the accuracy requested of implicit Euler to 1e-4. Performance improved significantly (albeit not to where we want it to be). Going to implement a low-order implicit RK method next and see how that performs on this problem.

[edrumwri]

Performance is still too slow using the Radau3 integrator in review (still many convergence failures). My hypothesis appears to be incorrect, barring any issues uncovered in review. Ryan Elandt has also used a regularized friction model on similar multibody contact problems and is getting acceptable performance with his Radau5 implementation. One key difference: his Jacobian matrices come from automatic differentiation.

My next investigation: integrate the manipulation station (using the continuous time model) to high accuracy and examine whether there are (unexpected) high frequency transitions into/out of stiction and into/out of contact.

[edrumwri]

Will compile a detailed report on this issue this week.

[edrumwri]

Pausing work on this to resume work on the hydroelastic model as described in the project plan.

[amcastro-tri]

Working on this issue now as I work tawards forward dynamics in TAMSI, which would allow us to make apples to apples comparisons. I am trying to understand why TAMSI performs significantly better.

[amcastro-tri]

Update:
Running simple_gripper with full NR is encouraging. For "soft" point contact things are ok. As soon as I stiffen the contact parameters (the interesting case since we expect point contact to be the fallback for rigid-rigid contact) the integrators have a real hard time.

Full Newton partially solves this meaning that the number of time steps now does make sense. That is, the "average" time step over two oscillations is about ~500 time steps per period. A reasonable number that I, as a human, I would've set. However, the cost of full Newton still is prohibitive.

I believe a reasonable solution is to make MaybeFreshenMatrices() fallback to full Newton as a last resource. My initial hacked trials did not go well. Investigating....