[CENIC] Support distance constraints

[joemasterjohn]

Implements distance constraints in the ICF solver for CENIC support.

Adapted from the SAP distance constraint and given the same treatment as the other holonomic constraints in ICF (#24769).

Closes #23762

CC: @xuchen-han

---

This change is 

[joemasterjohn]

@joemasterjohn made 1 comment.
Reviewable status: 1 unresolved discussion, 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).

---

a discussion (no related file):
Blocking work on this until #24769 merges.

[joemasterjohn]

+a:@sherm1 for feature review, please.

This one still depends on the refactor in #24769, so everything except the last commit can be ignored. No rush on this, I just wanted to get your eyes on it as soon as I could. This is the last in the saga of holonomic constraint support.

@joemasterjohn made 1 comment.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on sherm1).

[joemasterjohn]

@joemasterjohn resolved 1 discussion.
Reviewable status: LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, labeled "do not merge" (waiting on sherm1).

[sherm1]

Feature with one issue regarding handling of a bad initial condition, PTAL.

@sherm1 reviewed 30 files and all commit messages, and made 10 comments.
Reviewable status: 9 unresolved discussions, needs at least two assigned reviewers, labeled "do not merge" (waiting on joemasterjohn).

---

multibody/contact_solvers/icf/distance_constraints_pool.h line 30 at r2 (raw file):

this is a compliant (spring-damper) constraint: with p̂ the unit vector from P
to Q, stiffness k and damping c, the scalar impulse is
  γ = −k⋅(d − ℓ) − c⋅ḋ ∈ ℝ

BTW this looks like a force to me -- shouldn't there be a δt in there somewhere to make it an impulse?

---

multibody/contact_solvers/icf/distance_constraints_pool.cc line 29 at r2 (raw file):

template <typename T>
Vector1<T> DistanceConstraintsPool<T>::CalcConstraintVelocity(
    int k, const Vector6<T>& V_WB, const Vector6<T>* V_WA) const {

BTW is there a reason these have to be Vector6's rather than SpatialVelocities? The latter would be more "Drake like" and shorten some of the code below (I think).

---

multibody/contact_solvers/icf/distance_constraints_pool.cc line 30 at r2 (raw file):

Vector1<T> DistanceConstraintsPool<T>::CalcConstraintVelocity(
    int k, const Vector6<T>& V_WB, const Vector6<T>* V_WA) const {
  // vc = ḋ = p̂ᵀ⋅(v_W_Bq − v_W_Ap), the rate of change of distance.

minor: to check this I had to imagine what d might be -- it didn't seem to be defined anywhere. This makes sense with d = ∥p_WBq − p_WAp∥₂. That should be said somewhere (even here would be ok).

nit: the monogram notation for the time derivative of p_WBq is v_WBq without the extra underscore.

---

multibody/contact_solvers/icf/distance_constraints_pool.cc line 34 at r2 (raw file):

  const Vector3<T>& w_WB = V_WB.template head<3>();
  const Vector3<T>& v_WBo = V_WB.template tail<3>();
  const Vector3<T> v_W_Bq = v_WBo + w_WB.cross(p_BQ_W_[k]);

nit: v_W_Bq -> v_WBq

---

multibody/contact_solvers/icf/distance_constraints_pool.cc line 53 at r2 (raw file):

  // at Q on B and −γ⋅p̂ at P on A. Shift each to the body origin.
  const Vector3<T>& p_hat_W = p_hat_W_[k];
  const Vector3<T> f_B = gamma(0) * p_hat_W;

BTW Consider using a different symbol for an impulse to distinguish it from a force. Maybe j_B?

---

multibody/contact_solvers/icf/icf_builder.cc line 672 at r2 (raw file):

          "between bodies '{}' and '{}' is {}, which is nonphysically small "
          "compared to the constraint's free length, {}.",
          body_A.name(), body_B.name(), ExtractDoubleOrThrow(d0), length));

Can you say more about why this error condition? I would have expected that as long as ℓ is a reasonable non-zero length then it doesn't matter what the current value of d is -- we should be able to assemble the distance constraint as we can for a weld or ball that are initially unsatisfied. This seems to be setting some requirement on the initial unassembled model specification. Is that also the case in SAP, and is this documented somewhere?

I can see why you might want to require d0 to be non-zero to facilitate normalization of p_PQ_W below, but for that purpose almost anything non-zero would work, even 1e-15. You could even allow d0==0 if you provide an arbitrary p_hat just to help get the distance constraint assembled.

Also, I don't see a unit test for this condition. If possible, I'd suggest removing the restriction on d0 altogether to avoid annoying failures when resolving initial conditions. There ought to be an error condition for ℓ too small though if there isn't already. You would still need a unit test for the d0=0 initial condition since there would be a special case using an arbitrary p_hat for the first step.

---

multibody/contact_solvers/icf/test/distance_constraint_init_and_sim_test.cc line 150 at r2 (raw file):

  EXPECT_NEAR(distance, expected_distance, 2e-3);
}

BTW should have an initial conditions test where the points P and Q are initially coincident (d0=0) to see that we can escape from that trap.

---

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 61 at r2 (raw file):

}

/* Checks that pool.ReduceInto does not incur any heap allocations on a

nit: space after period missing

---

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 85 at r2 (raw file):

/* Verifies that distance constraints produce correct data. Uses a rigid and a
compliant constraint (see AddDistanceConstraints), exercising both R branches.

BTW not sure what "R branches" means?

[joemasterjohn]

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

---

multibody/contact_solvers/icf/distance_constraints_pool.h line 30 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW this looks like a force to me -- shouldn't there be a δt in there somewhere to make it an impulse?

Good catch. Added here and in the spot I copied it from.

---

multibody/contact_solvers/icf/distance_constraints_pool.cc line 29 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW is there a reason these have to be Vector6's rather than SpatialVelocities? The latter would be more "Drake like" and shorten some of the code below (I think).

I don't have a compelling reason. Vector6 has just been the convention cenic/icf has used since the beginning.

---

multibody/contact_solvers/icf/distance_constraints_pool.cc line 30 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: to check this I had to imagine what d might be -- it didn't seem to be defined anywhere. This makes sense with d = ∥p_WBq − p_WAp∥₂. That should be said somewhere (even here would be ok).

nit: the monogram notation for the time derivative of p_WBq is v_WBq without the extra underscore.

Done. d was introduced in the Class docs, but I'll add it here for clarity.

---

multibody/contact_solvers/icf/icf_builder.cc line 672 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Can you say more about why this error condition? I would have expected that as long as ℓ is a reasonable non-zero length then it doesn't matter what the current value of d is -- we should be able to assemble the distance constraint as we can for a weld or ball that are initially unsatisfied. This seems to be setting some requirement on the initial unassembled model specification. Is that also the case in SAP, and is this documented somewhere?

I can see why you might want to require d0 to be non-zero to facilitate normalization of p_PQ_W below, but for that purpose almost anything non-zero would work, even 1e-15. You could even allow d0==0 if you provide an arbitrary p_hat just to help get the distance constraint assembled.

Also, I don't see a unit test for this condition. If possible, I'd suggest removing the restriction on d0 altogether to avoid annoying failures when resolving initial conditions. There ought to be an error condition for ℓ too small though if there isn't already. You would still need a unit test for the d0=0 initial condition since there would be a special case using an arbitrary p_hat for the first step.

This was also a remnant from the SAP constraint. But after f2f we decided that this isn't strictly an error condition. I've refactored to be able to handle the coincident initial condition and added test coverage, PTAL.

---

multibody/contact_solvers/icf/test/distance_constraint_init_and_sim_test.cc line 150 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW should have an initial conditions test where the points P and Q are initially coincident (d0=0) to see that we can escape from that trap.

Done. Covered by the new test in icf_builder_test.

---

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 61 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: space after period missing

Cargo culted from the other constraints. This is talking about the method pool.ReduceInto().

---

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 85 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW not sure what "R branches" means?

"R" is referring to the regularization; I added more comments to clarify. The base class chooses between the near-rigid regularization and that computed by the user provided stiffness (whichever is softer). That's the branching this is referring to.

[sherm1]

Thanks Joe. All good, one suggestion.
+a:@jwnimmer-tri for Thu platform review per rotation, please

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

---

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 61 at r2 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Cargo culted from the other constraints. This is talking about the method pool.ReduceInto().

Oh, sorry -- I misread that!

---

multibody/contact_solvers/icf/test/icf_builder_test.cc line 466 at r3 (raw file):

  const auto& pool = model.distance_constraints_pool();
  EXPECT_NEAR(pool.p_hat_W()[0].norm(), 1.0, 1e-14);
  EXPECT_NEAR(pool.g0()[0](0), -kFreeLength, 1e-14);

BTW consider adding a short simulation to verify that it actually ends up near the free length

[jwnimmer-tri]

Checkpoint about halfway through. I'll need to circle back on the rest later today.

@jwnimmer-tri reviewed 27 files and all commit messages, and made 8 comments.
Reviewable status: 8 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

---

multibody/contact_solvers/icf/icf_builder.cc line 618 at r3 (raw file):

  int index = 0;
  for (const auto& [id, params] : params_map) {

nit We can make it more clear that id is unused.

Suggestion:

for (const auto& [_, params] : params_map) {

---

multibody/contact_solvers/icf/icf_builder.cc line 631 at r3 (raw file):

    //  happen is in MbP::Finalize() after the topology has been finalized.
    if (A_anchored && B_anchored) {
      const std::string msg = fmt::format(

nit msg is not a form of abbreviation allowed by our style guide.

(The easiest solution here is to put the fmt::format directly into the logic_error constructor; naming it doesn't help with clarity at all.)

---

multibody/contact_solvers/icf/icf_builder.cc line 664 at r3 (raw file):

    // When P and Q are (nearly) coincident, p̂ is undefined. The free length ℓ
    // is strictly positive (enforced by DistanceConstraintParams), so there

nit Something here is grammatically incorrect near "there".

---

multibody/contact_solvers/icf/icf_builder.cc line 671 at r3 (raw file):

    // is purely a divide-by-zero guard for the normalization below.
    constexpr double kMinimumDistance = 1.0e-14;
    const Vector3<T> p_hat_W = ExtractDoubleOrThrow(d0) < kMinimumDistance

nit Is the "extract double" really necessary? AutoDiff should already have a comparison operator that does this implicitly:

/** Standard comparison operator. Discards the derivatives. */
inline bool operator<(const AutoDiff& a, double b) {
  return a.value() < b;
}

Assuming this gets removed, also remove the #include statement for it.

Code quote:

ExtractDoubleOrThrow(d0) < kMinimumDistance

---

multibody/contact_solvers/icf/icf_data.h line 122 at r3 (raw file):

  @param patch_sizes Number of contact pairs for each patch constraint, of size
                     equal to the number of patches. */
  // TODO(sherm1) This argument list will get out of hand as we add more

BTW We're at the point of being out-of-hand. Either Joe or Sherm should tackle this in a follow-up PR.

It's aggravated by the fact that here (only) we've sorted the span<const int> arguments to the end of the list, whereas everywhere else we list all constraints in a row they are sorted differently (e.g., ball, coupler, distance, gain, limit, patch, weld -- in that order).

---

multibody/contact_solvers/icf/test/icf_builder_test.cc line 387 at r3 (raw file):

}

GTEST_TEST(IcfBuilder, DistanceConstraint) {

At least from the comments describing the new tests, I can't convince myself that the "swap bodies A and B in case B is anchored" logic in the builder is exercised.

---

multibody/contact_solvers/icf/test_utilities/icf_model_test_helpers.cc line 440 at r3 (raw file):

     &AddCouplerConstraint<T>, &AddDistanceConstraints<T>,
     &AddGainConstraints<T>, &AddLimitConstraints<T>, &AddPatchConstraints<T>,
     &AddWeldConstraints<T>));

nit The order of instantiations here seems inconsistent. It is neither alphabetical nor does it match the order of functions in the h/cc file.

[jwnimmer-tri]

platform.

@jwnimmer-tri reviewed 4 files and made 9 comments.
Reviewable status: 16 unresolved discussions, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on joemasterjohn).

---

multibody/contact_solvers/icf/distance_constraints_pool.h line 71 at r3 (raw file):

           const T& stiffness, const T& damping);

  /* Hooks required by HolonomicConstraintsPool (CRTP). */

nit In the HolnomicConstraintsPool specification for CalcSpatialImpulses and CalcHessianBlocks, it is not made clear whether the non-const pointer arguments are [out] or [in,out].

---

multibody/contact_solvers/icf/distance_constraints_pool.h line 94 at r3 (raw file):

  EigenPool<Vector3<T>> p_AP_W_;   // Position of P in A, expressed in W.
  EigenPool<Vector3<T>> p_BQ_W_;   // Position of Q in B, expressed in W.
  EigenPool<Vector3<T>> p_hat_W_;  // Unit vector P→Q, expressed in W.

BTW These comments seem a bit redundant, both with monogram itself and with the constructor documentation.

Code quote:

  EigenPool<Vector3<T>> p_AP_W_;   // Position of P in A, expressed in W.
  EigenPool<Vector3<T>> p_BQ_W_;   // Position of Q in B, expressed in W.
  EigenPool<Vector3<T>> p_hat_W_;  // Unit vector P→Q, expressed in W.

---

multibody/contact_solvers/icf/distance_constraints_pool.cc line 23 at r3 (raw file):

  p_BQ_W_[index] = p_BQ_W;
  p_hat_W_[index] = p_hat_W;
  // Constraint function g₀ = d₀ − ℓ ∈ ℝ.

BTW I'm not sure copy-pasting the comment from the header file documentation of this method into this spot is really adding any useful information.

---

multibody/contact_solvers/icf/distance_constraints_pool.cc line 89 at r3 (raw file):

    G_Ap->template bottomRightCorner<3, 3>() = Gt;

    // G_cross = −Φ(p_BQ)ᵀ⋅G⋅Φ(p_AP).

The specification of CalcHessianBlocks does not promise that G_Ap and G_cross are either both null or both non-null, yet we seem to be relying on that here.

---

multibody/contact_solvers/icf/test/distance_constraint_init_and_sim_test.cc line 148 at r3 (raw file):

  const double distance = SimulateAndGetFinalDistance(kStiffness, kDamping);
  const double expected_distance = kFreeLength + kMass * kGravity / kStiffness;
  EXPECT_NEAR(distance, expected_distance, 2e-3);

nit The 2 here seems a bit magical. Locally for me, 1mm passes. If allowing the extra factor of 2 is important, we should comment why.

---

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 98 at r3 (raw file):

  model.ResizeData(&data);
  EXPECT_EQ(data.num_velocities(), model.num_velocities());
  EXPECT_EQ(data.distance_constraints_data().num_constraints(), 0);

BTW This feels weird to be here, when the very next sentence is "should be no distance constraints". Seems like this should be part of the next stanza.

---

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 192 at r3 (raw file):

dynamic with 6 DOFs each. */
template <typename T>
void MakeModelForDistance(IcfModel<T>* model, double time_step = 0.01) {

nit The time_step argument is never varied by any callers, so it shouldn't be an argument.

---

multibody/contact_solvers/icf/test/distance_constraints_pool_test.cc line 207 at r3 (raw file):

  MatrixX<T>& M0 = params->M0;
  M0 = MatrixX<T>::Identity(nv, nv);

BTW Do we need identity here? It seems like zero would be equally good, and possibly clearer?