Revisit the near-rigid regime for implicit PD constraints

[amcastro-tri]

We revisit how we compute near-rigid parameters to control numerical conditioning for implicit PD constraints.

For context, a user tried to do velocity control with a very large derivative gain and a zero position gain. This put the implicit PD gain into the near-rigid regime, but our logic in master sets both non-zero Kp and Kd gains to be in a critically damped regime for position control, which is not what the user wanted in the first place.

Therefore in this PR we do allow users to set either Kp or Kd to zero (but not both). The new near-rigid treatment ensures that we use effective gains that preserve the user provided ratio Kp/Kd.

---

This change is 

[amcastro-tri]

+@jwnimmer-tri for feature review please

Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @amcastro-tri)

[amcastro-tri]

I think you have enough context for this one Jeremy. Otherwise feel free to delegate.
Thanks! for the help with Dale finding this problem!

Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @amcastro-tri)

[jwnimmer-tri]

I'm unable to follow the math comments.

Reviewed 1 of 3 files at r1, all commit messages.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @amcastro-tri)

---

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc line 56 at r1 (raw file):

  // Estimate regularization based on near-rigid regime threshold.
  // Rigid approximation constant: Rₙ = β²/(4π²)⋅wᵢ when the contact frequency

nit I believe the R-sub-n here is the same thing as the new R-sub-n-r used below. If they are the same thing, use the same spelling throughout all comments? Or if there are different, explain how they are different.

---

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc line 75 at r1 (raw file):

  // the user. In particular, if Kp = 0, that means the user wants velocity
  // control only. Similarly, if Kd = 0, the user wants position control only.
  // We observe we can write Rₙᵣ in two different but equivalent ways:

How do we "observe" this? It seems to me that the next two formulae appear out of thin air. There is no δt until here. Are we setting ωₙ equal to some boundary condition?

---

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc line 78 at r1 (raw file):

  //  1) Rₙᵣ = 1/(δt⋅(δt +   Kd/Kp )⋅Kp),   when Kp > 0
  //  2) Rₙᵣ = 1/(δt⋅( 1 + δt⋅Kp/Kd)⋅Kd),   when Kd > 0
  // From where we see that:

typo

Code quote:

where

[amcastro-tri]

No worries, thou you mean the comments or the math, Jeremy? I can rework the comments of course, but let me know if you'd like me to reassign.

Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes

[jwnimmer-tri]

The comments about the math.

[amcastro-tri]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @amcastro-tri)

---

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc line 56 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit I believe the R-sub-n here is the same thing as the new R-sub-n-r used below. If they are the same thing, use the same spelling throughout all comments? Or if there are different, explain how they are different.

Good catch. I cleaned up the notation in code and comments to keep them consistent.

---

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc line 75 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

How do we "observe" this? It seems to me that the next two formulae appear out of thin air. There is no δt until here. Are we setting ωₙ equal to some boundary condition?

Updated text. PTAL.

---

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc line 78 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

typo

?

[jwnimmer-tri]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @amcastro-tri)

---

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc line 78 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

?

"From where" is not a correct phasing. I assumed it was a typo for maybe "from there" or "from here" or something like that.

[jwnimmer-tri]

Improved now (with a few nits), but still I'm somehow not connecting where the math for setting boundary condition comes from. See below.

Reviewed 1 of 1 files at r2, all commit messages.
Reviewable status: 4 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @amcastro-tri)

---

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc line 80 at r2 (raw file):

  // regularization R in two different but equivalent ways:
  //  1) R = 1/(δt⋅(δt +   Kd/Kp )⋅Kp),   when Kp > 0
  //  2) R = 1/(δt⋅( 1 + δt⋅Kp/Kd)⋅Kd),   when Kd > 0

BTW For me at least, these lines would have been more clear if we said "if" instead of "when". Saying "when" implied to me that when Kp > 0 we must select option (1) to the exclusion of option (2), but really all we're doing with these guards is conditioning the validity of the respelling to avoid divide-by-zero invalidation.

Code quote:

  //  1) R = 1/(δt⋅(δt +   Kd/Kp )⋅Kp),   when Kp > 0
  //  2) R = 1/(δt⋅( 1 + δt⋅Kp/Kd)⋅Kd),   when Kd > 0

---

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc line 87 at r2 (raw file):

  // From where we see that:
  //  1) δt⋅Kpₙᵣ = 1 / ( (δt +   Kd/Kp )⋅Rₙᵣ) > 1/(2⋅δt⋅Rₙᵣ), δt⋅Kp > Kd
  //  2)    Kdₙᵣ = 1/(δt⋅( 1 + δt⋅Kp/Kd)⋅Rₙᵣ) > 1/(2⋅δt⋅Rₙᵣ), δt⋅Kp < Kd

Okay. So the respelling from

  //  1) Rₙᵣ = 1/(δt⋅(δt +   Kd/Kp )⋅Kpₙᵣ),   when Kp > 0
  //  2) Rₙᵣ = 1/(δt⋅( 1 + δt⋅Kp/Kd)⋅Kdₙᵣ),   when Kd > 0

to

  //  1) δt⋅Kpₙᵣ = 1 / ( (δt +   Kd/Kp )⋅Rₙᵣ)
  //  2)    Kdₙᵣ = 1/(δt⋅( 1 + δt⋅Kp/Kd)⋅Rₙᵣ)

is clear to me. (It's not very easy to follow in this horizontal layout with / instead of a vertically separated numerator and denominator, but once I copied onto a sheet of paper it was quick.)

The part where I get lost is why we constrain those two Kd-like quantities to be > 1/(2⋅δt⋅Rₙᵣ), and where the δt⋅Kp <> Kd jiblet comes from at the end. Big picture I understand we're trying "... propose to use Rₙᵣ as a lower bound such that the time scales ..." and to choose which of the two gains is the active one at that limit, but somehow I don't see from there where 2⋅δt⋅Rₙᵣ came from.

---

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc line 99 at r2 (raw file):

    } else {
      const T tau_inv = Kp / Kd;
      Kd_eff = 1.0 / (dt * (dt * tau_inv + 1.0) * R_nr);

nit The comment math and code should use the same ordering of their terms. (Changing the comment instead of the code would also be fine.)

Suggestion:

1.0 + dt * tau_inv

[amcastro-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @amcastro-tri)

---

multibody/contact_solvers/sap/sap_pd_controller_constraint.cc line 87 at r2 (raw file):

but somehow I don't see from there where 2⋅δt⋅Rₙᵣ

It comes from replacing δt⋅Kp <> Kd into the respective equation (either (1) or (2)).
For instance, δt⋅Kp > Kd implies δt> Kd/Kp. Thefore (δt + Kd/Kp ) < 2δt and thus δt⋅Kpₙᵣ > 1/(2⋅δt⋅Rₙᵣ).

Not sure if worth diving inot that much detail?

Then using δt⋅Kp <> Kd as the switching condition is just a symmetry argument (or choice), so that once of these two Kd-like quantities dominate.

Happy to edit more the comment, not sure exactly what you'd like to see here though.