[multibody] CENIC constraints handle small time steps

[rpoyner-tri]

Closes: #24403.
Closes: #24404.
Closes: #24228.

---

This change is 

[rpoyner-tri]

@joemasterjohn Here's a branch that turns up line search exceptions with joint limits.

Notes:

• contains a copy of #24634, pending merge/rebase.
• the patches so far to limit constraints bypass the "step too small" exceptions usually seen with small dt, but now instead throw for NaNs in line search.
• no problems found so far with patch to coupler constraints.

@rpoyner-tri made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits).

[rpoyner-tri]

Looking ahead to patch constraints, the hunt-crossley antiderivative math looks vulnerable (to my eye); see

drake/multibody/contact_solvers/icf/patch_constraints_pool.cc

Line 183 in f4d8b00

const T vx = fe0 / dt / (k + 1.0e-20);

@rpoyner-tri made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits).

[rpoyner-tri]

From f2f, the above patch constraint code is not vulnerable. H&C's broader computation can allow vx to become large; a competing term from elsewhere will limit it's effects.

@rpoyner-tri made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits).

[rpoyner-tri]

Joe has promised some algebra fixes he noticed while re-reading the constraints implementations. When those get added here, I'll open it up for review.

@rpoyner-tri made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits).

[joemasterjohn]

@joemasterjohn made 2 comments.
Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on rpoyner-tri).

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 69 at r2 (raw file):

  // Near-rigid regularization [Castro et al., 2022].
  constexpr double beta = 0.1;
  constexpr double eps = beta * beta / (4 * M_PI * M_PI) * (1 + beta / M_PI);

BTW Another algebra bug in the original implementation.

Suggestion:

 constexpr double eps = beta * beta / ((4 * M_PI * M_PI) * (1 + beta / M_PI));

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 86 at r2 (raw file):

  // CalcData().
  gl_hat_[index](dof) = (ql - q0) / (1.0 + beta);
  gu_hat_[index](dof) = (q0 - qu) / (1.0 + beta);

BTW This is the existing algebra bug that was tripping me up a bit. Otherwise the fix is straightforward, same treatment as the weld constraints. R and g_hat cannot be computed until the model's time step is established. For limit constraints that means at CalcData(). We can precompute gl_0 and gu_0 and the part of R that doesn't depend on time step. I'll send a patch.

Suggestion:

  gl_hat_[index](dof) = (ql - q0) / (1.0 + beta / M_PI);
  gu_hat_[index](dof) = (q0 - qu) / (1.0 + beta / M_PI);

[rpoyner-tri]

Grabbing the semaphore for a rebase to remove copies of other PRs.

[rpoyner-tri]

@rpoyner-tri made 1 comment.
Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 69 at r2 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

BTW Another algebra bug in the original implementation.

This error turns up in at least two places.

[rpoyner-tri]

@rpoyner-tri made 1 comment.
Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 69 at r2 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

This error turns up in at least two places.

👁️ Nope, actually one place. coupler_constraints_pool has the almost-identical spelling:

  // Near-rigid regularization: this constraint acts as a very stiff
  // critically-damped spring with time scale β⋅δt [Castro et al., 2022].
  const double beta = 0.1;
  const double eps = beta * beta / (4 * M_PI * M_PI) / (1 + beta / M_PI);

See the difference? coupler uses the amcastro spelling (chained division), and the limit file swaps a multiply for one of the divs.

I'm tempted to factor this in to IcfModel, but I'm not yet sure what to call it.

[rpoyner-tri]

rebase is done.

@rpoyner-tri made 1 comment.
Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

[joemasterjohn]

Ok, I made a commit that gives the fix for small dt with large initial violations. Similar treatment as the one for weld constraints.
I think it makes sense to let beta be a model-wide parameter. We always use beta = 0.1 anyway.

@joemasterjohn reviewed 1 file and made 1 comment.
Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on rpoyner-tri).

[rpoyner-tri]

@rpoyner-tri resolved 2 discussions.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

[rpoyner-tri]

rebased again. I think we are ready for review.
-(status: do not merge) -(status: do not review)
+a:@jwnimmer-tri for feature review please.

@rpoyner-tri made 1 comment.
Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn and jwnimmer-tri).

[jwnimmer-tri]

feature, so far as it goes. I'm not the best feature reviewer for this, but I guess I'm the next-best one who didn't author this and isn't ooo.

@jwnimmer-tri reviewed 10 files and all commit messages, and made 9 comments.
Reviewable status: 8 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on rpoyner-tri).

---

multibody/contact_solvers/icf/icf_model.h line 118 at r5 (raw file):

All such variables with static storage duration should be named [with a leading k] ...

-- https://drake.mit.edu/styleguide/cppguide.html#Constant_Names

---

multibody/contact_solvers/icf/icf_model.h line 466 at r5 (raw file):

... the formula ...

Which formula?

---

multibody/contact_solvers/icf/coupler_constraints_pool.cc line 56 at r5 (raw file):

  // critically-damped spring with time scale β⋅δt [Castro et al., 2022].
  constexpr double beta = IcfModel<T>::beta;
  constexpr double eps = beta * beta / (4 * M_PI * M_PI) / (1 + beta / M_PI);

The two operator/ in a row is confusing -- requires the reader to have memorized C's operator grouping rules in order to understand the code. This needs clarifying parens. (I believe the spelling was also inconsistent within this subdirectory, noted elsewhere during the review, though maybe those other instances are gone now.)

Ditto throughout any other eps like it.

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 69 at r5 (raw file):

  // Near-rigid regularization [Castro et al., 2022].
  // Eventually we will use:
  //  R⁻¹ = K·dt·(dt + taud)

BTW Do we want to call this taud in the math comments? We could use some τ unicode thing if there's one that fits, or even τd might be easier to parse.

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 74 at r5 (raw file):

  //  taud = β·dt_eff/π, so
  // Therefore:
  //  R = w / (K·dt·(dt + taud)) = β²·dt_eff²·w / (4π²·dt·(dt + taud)).

typo

Suggestion:

1 / (K·dt·(dt + taud))

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 106 at r5 (raw file):

  DRAKE_ASSERT(limit_data != nullptr);

  const T dt_eff = model().effective_time_step();

nit Other code computes dt first and df_eff second. That makes more sense to me, but in any case we should have a consistent approach across all constraints.

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 125 at r5 (raw file):

      // R_[k](i) only stores the non-time-step-dependent part:
      //  R_[k](i) = β²·w / (4π²)
      const T R = (R_[k](i) * dt_eff * dt_eff) / (dt * (dt + taud));

BTW Is the compiler smart enough to trigger loop-invariant code hoisting for the constant part of this product ((dt_eff * dt_eff) / (dt * (dt + taud)))? I wonder if its worth manually factoring that outside of the loop.

Code quote:

(R_[k](i) * dt_eff * dt_eff) / (dt * (dt + taud))

---

multibody/contact_solvers/icf/weld_constraints_pool.cc line 34 at r5 (raw file):

// Near-rigid parameter β.
constexpr double kBeta = 0.1;

I presume this value should come from the IcfModel constant now? (Though perhaps still aliased here, for brevity.)

Please also grep for other places that might need repair.

[rpoyner-tri]

@rpoyner-tri made 3 comments and resolved 7 discussions.
Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn and jwnimmer-tri).

---

multibody/contact_solvers/icf/coupler_constraints_pool.cc line 56 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The two operator/ in a row is confusing -- requires the reader to have memorized C's operator grouping rules in order to understand the code. This needs clarifying parens. (I believe the spelling was also inconsistent within this subdirectory, noted elsewhere during the review, though maybe those other instances are gone now.)

Ditto throughout any other eps like it.

The other occurrence got a different fix. The last term was rewritten and applied at a different stage.

---

multibody/contact_solvers/icf/icf_model.h line 466 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

... the formula ...

Which formula?

It wasn't even clear in the original place it was copied from. PTAL

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 74 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

typo

Do you agree, @joemasterjohn ?

[jwnimmer-tri]

Let's add +a:@joemasterjohn as another feature review approver, just to be sure we get this right.

@jwnimmer-tri reviewed 6 files and all commit messages, and made 3 comments.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee joemasterjohn, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn and jwnimmer-tri).

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 74 at r5 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

Do you agree, @joemasterjohn ?

(This is the only way to make the rest of the algebra consistent. If the typo isn't this one, then there is something larger gone wrong.)

---

a discussion (no related file):
Needs platform reviewer assigned.

[joemasterjohn]

@joemasterjohn made 2 comments and resolved 1 discussion.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee joemasterjohn, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on jwnimmer-tri).

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 69 at r2 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

👁️ Nope, actually one place. coupler_constraints_pool has the almost-identical spelling:

  // Near-rigid regularization: this constraint acts as a very stiff
  // critically-damped spring with time scale β⋅δt [Castro et al., 2022].
  const double beta = 0.1;
  const double eps = beta * beta / (4 * M_PI * M_PI) / (1 + beta / M_PI);

See the difference? coupler uses the amcastro spelling (chained division), and the limit file swaps a multiply for one of the divs.

I'm tempted to factor this in to IcfModel, but I'm not yet sure what to call it.

Yeah, super easy to miss.

I don't think we have an official name for this quantity. Maybe kNearRigidEps.

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 74 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

(This is the only way to make the rest of the algebra consistent. If the typo isn't this one, then there is something larger gone wrong.)

Yes, that's a typo.

[rpoyner-tri]

+a:@SeanCurtis-TRI for platform review, please.

@rpoyner-tri made 1 comment.
Reviewable status: 2 unresolved discussions, LGTM missing from assignees joemasterjohn,SeanCurtis-TRI(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on jwnimmer-tri, rpoyner-tri, and SeanCurtis-TRI).

[rpoyner-tri]

@rpoyner-tri resolved 1 discussion and dismissed @jwnimmer-tri from a discussion.
Reviewable status: 1 unresolved discussion, LGTM missing from assignees SeanCurtis-TRI(platform),joemasterjohn, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on SeanCurtis-TRI).

[rpoyner-tri]

@rpoyner-tri made 1 comment.
Reviewable status: 1 unresolved discussion, LGTM missing from assignees SeanCurtis-TRI(platform),joemasterjohn, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on jwnimmer-tri and SeanCurtis-TRI).

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 69 at r2 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Yeah, super easy to miss.

I don't think we have an official name for this quantity. Maybe kNearRigidEps.

I punted the refactoring, since the two(?) places I noticed have already diverged.

[rpoyner-tri]

@rpoyner-tri resolved 1 discussion.
Reviewable status: LGTM missing from assignees SeanCurtis-TRI(platform),joemasterjohn, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on jwnimmer-tri and SeanCurtis-TRI).

[jwnimmer-tri]

@jwnimmer-tri reviewed 1 file and all commit messages.
Reviewable status: LGTM missing from assignees SeanCurtis-TRI(platform),joemasterjohn, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on SeanCurtis-TRI).

[joemasterjohn]

Feature pass done. PTAL.

@joemasterjohn reviewed 11 files and all commit messages, and made 6 comments.
Reviewable status: 4 unresolved discussions, LGTM missing from assignees SeanCurtis-TRI(platform),joemasterjohn, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on rpoyner-tri and SeanCurtis-TRI).

---

multibody/contact_solvers/icf/limit_constraints_pool.cc line 69 at r2 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

I punted the refactoring, since the two(?) places I noticed have already diverged.

👍

---

multibody/contact_solvers/icf/coupler_constraints_pool.cc line 66 at r6 (raw file):

  // actually solve the problem, we precompute ĝ = v̂⋅δt = −g₀/(1 + β/π), so that
  // we can compute v̂ = ĝ/δt from the current time step in calls to CalcData().
  g_hat_[index] = -g0 / (1.0 + kBeta / M_PI);

I assume coupler constraints will have the same fundamental issue as the other constraints r.e. the near rigid approximation. Might be worth it to fix the coupler constraints as well. Same exact treatment as limit constraints. Only store -g0 in g_hat_[index] here in Set(), then finish the job in CalcData(). Ditto for R_[index], drop the (1 + kBeta / M_PI) here in Set() and add the factor when we know dt and taud in CalcData().

Code quote:

  g_hat_[index] = -g0 / (1.0 + kBeta / M_PI);

---

multibody/contact_solvers/icf/coupler_constraints_pool.cc line 91 at r6 (raw file):

    const int j = dofs_[k].second;
    const T& rho = gear_ratio_[k];
    const T v_hat = g_hat_[k] / model().effective_time_step();

This is incorrect. We should be doing what is done in limit constraints.

  const T& dt = model().time_step();
  const T dt_eff = model().effective_time_step();
  constexpr double kBeta = IcfModel<T>::kBeta;
  const T taud = kBeta * dt_eff / M_PI;

Suggestion:

    const T v_hat = g_hat_[k] / (dt + taud);

---

multibody/contact_solvers/icf/icf_model.h line 117 at r6 (raw file):

  /* The near-rigid contact stabilization parameter β.  See [Castro et al.,
  2022], section V.B. */

More accurate to call this the time scale factor.

Suggestion:

  /* The near-rigid time scale factor β.  See [Castro et al.,
  2022], section V.B. */

---

multibody/contact_solvers/icf/icf_model.h line 252 at r6 (raw file):

    using std::max;
    return max(time_step(), static_cast<T>(kHMin));
  }

nit I think it's important to mention that this is just a time step used for computing stiffness/dissipation parameters. So as not to introduce confusion about the time step actually being changed.

Suggestion:

  /* Returns the effective time step for computing near-rigid parameters for constraints.
  At very small actual time steps, the effective time step will be limited to some
  minimum value to avoid unreasonable stiffness and dissipation effects.
  */
  T effective_time_step() const {
    using std::max;
    return max(time_step(), static_cast<T>(kHMin));
  }

[SeanCurtis-TRI]

@rpoyner-tri looks like you have a few more feature comments to clean up.

@SeanCurtis-TRI reviewed 11 files and all commit messages, and made 4 comments.
Reviewable status: 7 unresolved discussions, LGTM missing from assignees SeanCurtis-TRI(platform),joemasterjohn, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on rpoyner-tri).

---

multibody/cenic/test/cenic_integrator_test.cc line 538 at r7 (raw file):

  EXPECT_EQ(error_estimate.norm(), 0);

  // Check handling of extreme initial conditions (#24403).

BTW The test is documented as "Checks that the integrator can enforce joint limits".

The test above is actually testing the integrator's result against limit values. That is a clear manifestation of "checking the enforcing of limits".

This newly added test (and the test just below -- check joint locking support) less obviously "check" in the same sense as the previous testing code.

Perhaps we can be more clear about what is being checked? The joint locking support seems to be validated simply as "it doesn't get angry". Should the check for extreme initial conditions have the same success criterion? If so, we should be clear about that. (A bit of justification why "not getting angry" is a sufficient proxy for "does the right thing" would be nice too.)

(All of this applies to the CoupledJoints test below.)

---

multibody/cenic/test/cenic_integrator_test.cc line 541 at r7 (raw file):

  const VectorXd q_bad = VectorXd::Zero(2);
  plant_->SetPositions(plant_context_, q_bad);
  simulator_->AdvanceTo(1.1);

nit: It seems this should be:

EXPECT_NO_THROW(simulator_->AdvanceTo(1.1));

---

multibody/cenic/test/cenic_integrator_test.cc line 622 at r7 (raw file):

  q_bad(1) += 0.2;
  plant_->SetPositions(plant_context_, q_bad);
  simulator_->AdvanceTo(1.1);

nit: Similarly, EXPECT_NO_THROW?

[rpoyner-tri]

@rpoyner-tri made 3 comments and resolved 4 discussions.
Reviewable status: 3 unresolved discussions, LGTM missing from assignees SeanCurtis-TRI(platform),joemasterjohn, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn, jwnimmer-tri, and SeanCurtis-TRI).

---

multibody/contact_solvers/icf/coupler_constraints_pool.cc line 66 at r6 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

I assume coupler constraints will have the same fundamental issue as the other constraints r.e. the near rigid approximation. Might be worth it to fix the coupler constraints as well. Same exact treatment as limit constraints. Only store -g0 in g_hat_[index] here in Set(), then finish the job in CalcData(). Ditto for R_[index], drop the (1 + kBeta / M_PI) here in Set() and add the factor when we know dt and taud in CalcData().

Done.

---

multibody/contact_solvers/icf/coupler_constraints_pool.cc line 91 at r6 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

This is incorrect. We should be doing what is done in limit constraints.

  const T& dt = model().time_step();
  const T dt_eff = model().effective_time_step();
  constexpr double kBeta = IcfModel<T>::kBeta;
  const T taud = kBeta * dt_eff / M_PI;

Done.

---

multibody/contact_solvers/icf/icf_model.h line 117 at r6 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

More accurate to call this the time scale factor.

Done.