[CENIC] Support distance constraints

bindings/generated_docstrings/multibody_contact_solvers_icf.h

@@ -16,6 +16,7 @@
 // #include "drake/multibody/contact_solvers/icf/ball_constraints_pool.h"
 // #include "drake/multibody/contact_solvers/icf/coupler_constraints_data_pool.h"
 // #include "drake/multibody/contact_solvers/icf/coupler_constraints_pool.h"
+// #include "drake/multibody/contact_solvers/icf/distance_constraints_pool.h"
 // #include "drake/multibody/contact_solvers/icf/eigen_pool.h"
 // #include "drake/multibody/contact_solvers/icf/gain_constraints_data_pool.h"
 // #include "drake/multibody/contact_solvers/icf/gain_constraints_pool.h"

multibody/contact_solvers/icf/BUILD.bazel

@@ -183,6 +183,7 @@ drake_cc_library(
     srcs = [
         "ball_constraints_pool.cc",
         "coupler_constraints_pool.cc",
+        "distance_constraints_pool.cc",
         "gain_constraints_pool.cc",
         "holonomic_constraints_pool.cc",
         "icf_model.cc",
@@ -194,6 +195,7 @@ drake_cc_library(
         "abstract_constraints_pool.h",
         "ball_constraints_pool.h",
         "coupler_constraints_pool.h",
+        "distance_constraints_pool.h",
         "gain_constraints_pool.h",
         "holonomic_constraints_pool.h",
         "icf_model.h",
@@ -266,6 +268,31 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "distance_constraint_init_and_sim_test",
+    deps = [
+        "//multibody/cenic:cenic_integrator",
+        "//multibody/parsing",
+        "//multibody/plant",
+        "//multibody/plant:multibody_plant_config_functions",
+        "//systems/analysis:simulator",
+        "//systems/framework:diagram_builder",
+    ],
+)
+
+drake_cc_googletest(
+    name = "distance_constraints_pool_test",
+    deps = [
+        ":icf_data",
+        ":icf_model",
+        ":icf_search_direction_data",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:limit_malloc",
+        "//math:gradient",
+        "//multibody/contact_solvers/icf/test_utilities:icf_model_test_helpers",
+    ],
+)
+
 drake_cc_googletest(
     name = "eigen_pool_test",
     deps = [

multibody/contact_solvers/icf/distance_constraints_pool.cc

@@ -0,0 +1,128 @@
+#include "drake/multibody/contact_solvers/icf/distance_constraints_pool.h"
+
+#include "drake/math/cross_product.h"
+#include "drake/multibody/contact_solvers/icf/icf_model.h"
+
+namespace drake {
+namespace multibody {
+namespace contact_solvers {
+namespace icf {
+namespace internal {
+
+using math::VectorToSkewSymmetric;
+
+template <typename T>
+void DistanceConstraintsPool<T>::Set(int index, int bodyA, int bodyB,
+                                     const Vector3<T>& p_AP_W,
+                                     const Vector3<T>& p_BQ_W,
+                                     const Vector3<T>& p_hat_W, const T& g0,
+                                     const T& stiffness, const T& damping) {
+  p_AP_W_[index] = p_AP_W;
+  p_BQ_W_[index] = p_BQ_W;
+  p_hat_W_[index] = p_hat_W;
+  // Constraint function g₀ = d₀ − ℓ ∈ ℝ.
+  this->SetCommon(index, bodyA, bodyB, Vector1<T>(g0), stiffness, damping);
+}
+
+template <typename T>
+Vector1<T> DistanceConstraintsPool<T>::CalcConstraintVelocity(
+    int k, const Vector6<T>& V_WB, const Vector6<T>* V_WA) const {
+  // With d = ‖p_WBq − p_WAp‖ the distance between P and Q, the constraint
+  // velocity is vc = ḋ = p̂ᵀ⋅(v_WBq − v_WAp), the rate of change of distance.
+  const Vector3<T>& p_hat_W = p_hat_W_[k];
+  const Vector3<T>& w_WB = V_WB.template head<3>();
+  const Vector3<T>& v_WBo = V_WB.template tail<3>();
+  const Vector3<T> v_WBq = v_WBo + w_WB.cross(p_BQ_W_[k]);
+
+  T vc = p_hat_W.dot(v_WBq);
+  if (V_WA != nullptr) {
+    const Vector3<T>& w_WA = V_WA->template head<3>();
+    const Vector3<T>& v_WAo = V_WA->template tail<3>();
+    const Vector3<T> v_WAp = v_WAo + w_WA.cross(p_AP_W_[k]);
+    vc -= p_hat_W.dot(v_WAp);
+  }
+  return Vector1<T>(vc);
+}
+
+template <typename T>
+void DistanceConstraintsPool<T>::CalcSpatialImpulses(
+    int k, const Vector1<T>& gamma, Vector6<T>* Gamma_Bo,
+    Vector6<T>* Gamma_Ao) const {
+  // The scalar impulse γ acts as γ⋅p̂ along the line PQ, applied
+  // 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> j_B = gamma(0) * p_hat_W;  // Impulse on B at Q, along p̂.
+  const Vector6<T> spatial_gamma_Bq =
+      (Vector6<T>() << Vector3<T>::Zero(), j_B).finished();
+  *Gamma_Bo = ShiftSpatialImpulse<T>(spatial_gamma_Bq, p_BQ_W_[k]);
+  if (Gamma_Ao != nullptr) {
+    const Vector6<T> minus_spatial_gamma_Ap =
+        (Vector6<T>() << Vector3<T>::Zero(), Vector3<T>(-j_B)).finished();
+    *Gamma_Ao = ShiftSpatialImpulse<T>(minus_spatial_gamma_Ap, p_AP_W_[k]);
+  }
+}
+
+template <typename T>
+void DistanceConstraintsPool<T>::CalcHessianBlocks(int k, const T& R_inv,
+                                                   Matrix6<T>* G_Bp,
+                                                   Matrix6<T>* G_Ap,
+                                                   Matrix6<T>* G_cross) const {
+  // G = diag(0, Gt) with Gt = R⁻¹⋅p̂⋅p̂ᵀ.
+  const Vector3<T>& p_hat_W = p_hat_W_[k];
+  const Matrix3<T> Gt = R_inv * (p_hat_W * p_hat_W.transpose());
+  const Matrix3<T> px_B = VectorToSkewSymmetric(p_BQ_W_[k]);
+  // Compute G_Bp = Φ(p_BoBm)ᵀ⋅G⋅Φ(p_BoBm) where Φ(p) = [𝕀₃, 0; -pₓ, 𝕀₃] and
+  // G = diag(0, Gt).
+  G_Bp->template topLeftCorner<3, 3>() = -px_B * Gt * px_B;
+  G_Bp->template topRightCorner<3, 3>() = px_B * Gt;
+  G_Bp->template bottomLeftCorner<3, 3>() = -Gt * px_B;
+  G_Bp->template bottomRightCorner<3, 3>() = Gt;
+
+  if (G_Ap != nullptr) {
+    const Matrix3<T> px_A = VectorToSkewSymmetric(p_AP_W_[k]);
+    // G_Ap = Φ(p_AoAm)ᵀ⋅G⋅Φ(p_AoAm).
+    G_Ap->template topLeftCorner<3, 3>() = -px_A * Gt * px_A;
+    G_Ap->template topRightCorner<3, 3>() = px_A * Gt;
+    G_Ap->template bottomLeftCorner<3, 3>() = -Gt * px_A;
+    G_Ap->template bottomRightCorner<3, 3>() = Gt;
+
+    // G_cross = −Φ(p_BQ)ᵀ⋅G⋅Φ(p_AP).
+    G_cross->template topLeftCorner<3, 3>() = px_B * Gt * px_A;
+    G_cross->template topRightCorner<3, 3>() = -px_B * Gt;
+    G_cross->template bottomLeftCorner<3, 3>() = Gt * px_A;
+    G_cross->template bottomRightCorner<3, 3>() = -Gt;
+  }
+}
+
+template <typename T>
+void DistanceConstraintsPool<T>::ResizeGeometry(int num_constraints) {
+  p_AP_W_.Resize(num_constraints, 3, 1);
+  p_BQ_W_.Resize(num_constraints, 3, 1);
+  p_hat_W_.Resize(num_constraints, 3, 1);
+}
+
+template <typename T>
+void DistanceConstraintsPool<T>::ReduceGeometryInto(
+    DistanceConstraintsPool<T>* reduced, int k, bool flip) const {
+  // g₀ = d − ℓ is symmetric under swapping P and Q, but the unit direction p̂
+  // (from P to Q) negates and the anchor points swap.
+  if (flip) {
+    reduced->p_AP_W_.Add(3, 1) = p_BQ_W_[k];
+    reduced->p_BQ_W_.Add(3, 1) = p_AP_W_[k];
+    reduced->p_hat_W_.Add(3, 1) = -p_hat_W_[k];
+  } else {
+    reduced->p_AP_W_.Add(3, 1) = p_AP_W_[k];
+    reduced->p_BQ_W_.Add(3, 1) = p_BQ_W_[k];
+    reduced->p_hat_W_.Add(3, 1) = p_hat_W_[k];
+  }
+}
+
+}  // namespace internal
+}  // namespace icf
+}  // namespace contact_solvers
+}  // namespace multibody
+}  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    class ::drake::multibody::contact_solvers::icf::internal::
+        DistanceConstraintsPool);

multibody/contact_solvers/icf/distance_constraints_pool.h

@@ -0,0 +1,107 @@
+#pragma once
+
+#include "drake/common/drake_copyable.h"
+#include "drake/common/eigen_types.h"
+#include "drake/multibody/contact_solvers/icf/abstract_constraints_pool.h"
+#include "drake/multibody/contact_solvers/icf/eigen_pool.h"
+#include "drake/multibody/contact_solvers/icf/holonomic_constraints_data_pool.h"
+#include "drake/multibody/contact_solvers/icf/holonomic_constraints_pool.h"
+#include "drake/multibody/contact_solvers/icf/icf_data.h"
+
+namespace drake {
+namespace multibody {
+namespace contact_solvers {
+namespace icf {
+namespace internal {
+
+// Forward declaration to break circular dependencies.
+template <typename T>
+class IcfModel;
+
+/* A pool of distance constraints between pairs of bodies.
+
+Each distance constraint connects distinct bodies A and B, constraining the
+Euclidean distance d between a point P on A and a point Q on B to a free length
+ℓ. This is a single (scalar) holonomic constraint equation:
+  g = d − ℓ = 0 ∈ ℝ.
+Adapted from the SAP distance constraint (see sap_distance_constraint.h),
+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
+     γ = δt⋅(−k⋅(d−ℓ) − c⋅ḋ) ∈ ℝ
+applied as γ⋅p̂ on B at Q and −γ⋅p̂ on A at P.
+
+Unlike other holonomic constraints (e.g. weld, ball), the distance constraint
+exposes stiffness and damping parameters to the user, allowing the modeling of
+a linear spring-damper between two points. If the stiffness is set to +∞, the
+constraint uses the near-rigid approximation to approximate a rigid distance
+constraint.
+
+@tparam_nonsymbolic_scalar */
+template <typename T>
+class DistanceConstraintsPool
+    : public HolonomicConstraintsPool<T, 1, DistanceConstraintsPool<T>> {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(DistanceConstraintsPool);
+
+  explicit DistanceConstraintsPool(const IcfModel<T>* parent_model)
+      : HolonomicConstraintsPool<T, 1, DistanceConstraintsPool<T>>(
+            parent_model) {}
+
+  /* The scalar constraint function g₀ = d − ℓ is symmetric under swapping the
+  points P and Q, so (unlike the weld/ball) it does not negate on an A/B flip
+  during model reduction. See HolonomicConstraintsPool::kFlipNegatesG0(). */
+  static constexpr bool FlipNegatesG0() { return false; }
+
+  /* Sets the k-th distance constraint.
+  @param index The index of the constraint within the pool.
+  @param bodyA The index of body A. May be anchored.
+  @param bodyB The index of body B. Must not be anchored.
+  @param p_AP_W Position of constraint point P in body A, expressed in world.
+  @param p_BQ_W Position of constraint point Q in body B, expressed in world.
+  @param p_hat_W Unit vector from P to Q, expressed in world.
+  @param g0 The constraint function at q₀, g₀ = d₀ − ℓ.
+  @param stiffness The constraint stiffness k in N/m (may be +∞ for near-rigid).
+  @param damping The constraint damping c in N⋅s/m.
+  @pre indices in range, bodyA ≠ bodyB, bodyB not anchored, stiffness > 0,
+       damping ≥ 0. */
+  void Set(int index, int bodyA, int bodyB, const Vector3<T>& p_AP_W,
+           const Vector3<T>& p_BQ_W, const Vector3<T>& p_hat_W, const T& g0,
+           const T& stiffness, const T& damping);
+
+  /* Hooks required by HolonomicConstraintsPool (CRTP). */
+  Vector1<T> CalcConstraintVelocity(int k, const Vector6<T>& V_WB,
+                                    const Vector6<T>* V_WA) const;
+  void CalcSpatialImpulses(int k, const Vector1<T>& gamma, Vector6<T>* Gamma_Bo,
+                           Vector6<T>* Gamma_Ao) const;
+  void CalcHessianBlocks(int k, const T& R_inv, Matrix6<T>* G_Bp,
+                         Matrix6<T>* G_Ap, Matrix6<T>* G_cross) const;
+  const DistanceConstraintsDataPool<T>& GetDataPool(
+      const IcfData<T>& data) const {
+    return data.distance_constraints_data();
+  }
+  void ResizeGeometry(int num_constraints);
+  void ReduceGeometryInto(DistanceConstraintsPool<T>* reduced, int k,
+                          bool flip) const;
+
+  /* Testing-only access. */
+  const EigenPool<Vector3<T>>& p_AP_W() const { return p_AP_W_; }
+  const EigenPool<Vector3<T>>& p_BQ_W() const { return p_BQ_W_; }
+  const EigenPool<Vector3<T>>& p_hat_W() const { return p_hat_W_; }
+
+ private:
+  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.
+};
+static_assert(IsAbstractConstraintsPool<DistanceConstraintsPool>);
+static_assert(IsHolonomicConstraintsDerived<DistanceConstraintsPool>);
+
+}  // namespace internal
+}  // namespace icf
+}  // namespace contact_solvers
+}  // namespace multibody
+}  // namespace drake
+
+DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    class ::drake::multibody::contact_solvers::icf::internal::
+        DistanceConstraintsPool);

multibody/contact_solvers/icf/eigen_pool.cc

@@ -168,6 +168,12 @@ template class EigenPoolDynamicSizeStorage<MatrixX<double>>;
 template class EigenPool<MatrixX<AutoDiffXd>>;
 template class EigenPoolDynamicSizeStorage<MatrixX<AutoDiffXd>>;
 
+// Vector1
+template class EigenPool<Vector1<double>>;
+template class EigenPoolFixedSizeStorage<Vector1<double>>;
+template class EigenPool<Vector1<AutoDiffXd>>;
+template class EigenPoolFixedSizeStorage<Vector1<AutoDiffXd>>;
+
 // Vector3
 template class EigenPool<Vector3<double>>;
 template class EigenPoolFixedSizeStorage<Vector3<double>>;

multibody/contact_solvers/icf/eigen_pool.h

@@ -118,6 +118,7 @@ scalars and/or sharing the allocation across multiple pools.
 - Matrix6<T>
 - Matrix6X<T>
 - MatrixX<T>
+- Vector1<T>
 - Vector3<T>
 - Vector6<T>
 - VectorX<T>

multibody/contact_solvers/icf/holonomic_constraints_data_pool.cc

@@ -21,6 +21,8 @@ template class HolonomicConstraintsDataPool<double, 6>;
 template class HolonomicConstraintsDataPool<AutoDiffXd, 6>;
 template class HolonomicConstraintsDataPool<double, 3>;
 template class HolonomicConstraintsDataPool<AutoDiffXd, 3>;
+template class HolonomicConstraintsDataPool<double, 1>;
+template class HolonomicConstraintsDataPool<AutoDiffXd, 1>;
 
 }  // namespace internal
 }  // namespace icf

multibody/contact_solvers/icf/holonomic_constraints_data_pool.h

@@ -50,11 +50,14 @@ class HolonomicConstraintsDataPool {
 };
 
 /* Named data pools for the concrete holonomic constraints. Each is just the
-generic pool at the constraint's equation count (weld: 6, ball: 3). */
+generic pool at the constraint's equation count (weld: 6, ball: 3,
+distance: 1). */
 template <typename T>
 using WeldConstraintsDataPool = HolonomicConstraintsDataPool<T, 6>;
 template <typename T>
 using BallConstraintsDataPool = HolonomicConstraintsDataPool<T, 3>;
+template <typename T>
+using DistanceConstraintsDataPool = HolonomicConstraintsDataPool<T, 1>;
 
 }  // namespace internal
 }  // namespace icf

multibody/contact_solvers/icf/holonomic_constraints_pool.cc

@@ -7,6 +7,7 @@
 #include "drake/common/autodiff.h"
 #include "drake/common/extract_double.h"
 #include "drake/multibody/contact_solvers/icf/ball_constraints_pool.h"
+#include "drake/multibody/contact_solvers/icf/distance_constraints_pool.h"
 #include "drake/multibody/contact_solvers/icf/icf_model.h"
 #include "drake/multibody/contact_solvers/icf/weld_constraints_pool.h"
 
@@ -356,6 +357,10 @@ template class HolonomicConstraintsPool<AutoDiffXd, 6,
 template class HolonomicConstraintsPool<double, 3, BallConstraintsPool<double>>;
 template class HolonomicConstraintsPool<AutoDiffXd, 3,
                                         BallConstraintsPool<AutoDiffXd>>;
+template class HolonomicConstraintsPool<double, 1,
+                                        DistanceConstraintsPool<double>>;
+template class HolonomicConstraintsPool<AutoDiffXd, 1,
+                                        DistanceConstraintsPool<AutoDiffXd>>;
 
 }  // namespace internal
 }  // namespace icf