SAP Solver PR train

[amcastro-tri]

This issue provides a sketch of the PR train needed to complete the SAP solve project. Refer to the project's description for more information.

Find SapSolver project here. Full working prototype in https://github.com/RobotLocomotion/drake/pull/16543.

• [x] SapContactProblem (230 LOCs)
  • Without SapContraint
  • Without ContactProblemGraph
  • Define cliques
• [x] SapConstraint (386 LOCs)
  • Reference SapContactProblem's docs on cliques
• [x] ContactProblemGraph (340 LOCs)
• [x] SapContactProblem::AddConstraint() (224 LOCs)
• [x] SapConstraintBundle (621 LOCs)
• [x] PartialPermutation (354 LOCs)
• [x] Contact graph provides participating cliques (86 LOCs)
• [x] FrictionConeConstraint (655 LOCs)
  • Use MP to get a numerical solution to the projection.
• [x] Update MultibodyTreeTopology to provide tree information needed by the manager. (230 LOCs)
  • num_treevelocities: Needed to compute per-tree A and J blocks.
  • body_to_treeindex: Needed to determine clique given a body in contact.
  • tree_velocitiesstart: needed to extract per tree Jacobian blocks.
  • dof_to_treeindex: Thus far only used when adding distance constraints. I'll just add all these in a single PR.
• [x] SapModel (724 LOCs)
• [x] Complete SapModel features + unit tests:
  • Regularizer cost
  • Gradient of the cost.
  • Hessian of the cost
  • United with 2D boxes with contact and AutoDiff for gradients.
• [x] CompliantContactManager makes SapContactProblem
  • Eval contact Jacobian as needed.
  • Eval? SapContactProblem?, a function of state after all.
• [x] SapContactSolver is no longer a ContactSolver
• [x] SapSolver makes use of SapModel
• [x] SapSolverResults
  • Introduce results compatible with a given SapContactProblem
  • Extract ContactResults utility for contact reporting.
• [x] Incorporate SapSolver into CompliantContactManager
  • Testing strategy?
• [x] Limit constraint.
• [x] CompliantContactManager adds limit constraint to the SapContactProblem.
• [x] SapSolver uses supernodal algebra.
• [x] Implicit treatment of joint dissipation.
• [x] Manager computes acceleration kinematics.
• [x] Exact line search
• [x] Parsing of dissipation time scale for SAP dissipation model
• [x] MultibodyPlant option to use SapSolver
  • MBP API.
  • MultibodyPlantConfig
  • solver option
  • regularization params option?
  • Add gflag to opt-in for SAP in some of our examples:
  • Falling cylinder
  • Acrobot
  • Atlas
  • ManipulationStation
• [ ] SapSolver and CompliantContactManager log timing statistics.
• [x] ~CouplerConstraint~. SapHolonomicConstraint
• [x] Plant API to add coupler constraints.
• [x] Plant API to add distance constraints.
• [ ] Plant APIs for PD controller constraint
  • Ports for desired state.
  • Actuation and generalized forces input ports are additive. (this enables things like gravity compensation, slow, plus PD controllers, fast)
• [ ] (stretch) SapSolver unit tests
  • Push box
  • Falling cylinder
  • Painleve rod
  • Coupler and distance constraints.
• [x] Support for joint locking.
  • [x] Include locking in ForwardDynamics.
  • [x] Reduce SapContactProblem.
  • [x] Expand ContactSolverResults

[sherm1]

Should we add "support joint locking" to this list?

[DamrongGuoy]

It is so great to see the plan. Thank you!

[edrumwri]

Could I make two requests?

1. Could you bump the priority for joint locking?
2. Would you cause SAP solver to fail fast if joint locking is enabled? We spent three hours of engineering time tracking down why our locked bodies were no longer stationary.

[chen-tianjian]

May I ask what the status is for joint locking and free body locking with SAP?