[multibody] Implements SapCouplerConstraint and incorporates it into …

…SapDriver (#19902)

Also includes a fix for an existing bug in SapDriver::AddCouplerConstraint
where the velocity index of a joint was incorrectly used to index the joint's positions.

multibody/contact_solvers/sap/BUILD.bazel

@@ -20,6 +20,7 @@ drake_cc_package_library(
         ":sap_constraint_bundle",
         ":sap_constraint_jacobian",
         ":sap_contact_problem",
+        ":sap_coupler_constraint",
         ":sap_distance_constraint",
         ":sap_friction_cone_constraint",
         ":sap_holonomic_constraint",
@@ -106,6 +107,19 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "sap_coupler_constraint",
+    srcs = ["sap_coupler_constraint.cc"],
+    hdrs = ["sap_coupler_constraint.h"],
+    deps = [
+        ":sap_constraint",
+        ":sap_constraint_jacobian",
+        ":sap_holonomic_constraint",
+        "//common:default_scalars",
+        "//common:essential",
+    ],
+)
+
 drake_cc_library(
     name = "sap_distance_constraint",
     srcs = ["sap_distance_constraint.cc"],
@@ -251,6 +265,16 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "sap_coupler_constraint_test",
+    deps = [
+        ":sap_coupler_constraint",
+        ":validate_constraint_gradients",
+        "//common:pointer_cast",
+        "//common/test_utilities:eigen_matrix_compare",
+    ],
+)
+
 drake_cc_googletest(
     name = "sap_distance_constraint_test",
     deps = [

multibody/contact_solvers/sap/sap_coupler_constraint.cc

@@ -0,0 +1,95 @@
+#include "drake/multibody/contact_solvers/sap/sap_coupler_constraint.h"
+
+#include <limits>
+#include <utility>
+
+#include "drake/common/default_scalars.h"
+#include "drake/common/eigen_types.h"
+
+namespace drake {
+namespace multibody {
+namespace contact_solvers {
+namespace internal {
+
+template <typename T>
+SapCouplerConstraint<T>::SapCouplerConstraint(Kinematics kinematics)
+    : SapHolonomicConstraint<T>(
+          MakeSapHolonomicConstraintKinematics(kinematics),
+          MakeSapHolonomicConstraintParameters(), {}),
+      kinematics_(std::move(kinematics)) {}
+
+template <typename T>
+typename SapHolonomicConstraint<T>::Parameters
+SapCouplerConstraint<T>::MakeSapHolonomicConstraintParameters() {
+  // "Near-rigid" regime parameter, see [Castro et al., 2022].
+  // TODO(amcastro-tri): consider exposing this parameter.
+  constexpr double kBeta = 0.1;
+
+  // Coupler constraints do not have impulse limits, they are bi-lateral
+  // constraints. Each coupler constraint introduces a single constraint
+  // equation.
+  //
+  // N.B. `stiffness` is set to infinity to model this constraint in the
+  // "near-rigid" regime. `relaxation_time` is set arbitrarily to 0.0.
+  // SapHolonomicConstraint will use the time step as the relaxation time when
+  // it detects this constraint is "near-rigid". See
+  // SapHolonomicConstraint::DoMakeData().
+  constexpr double kInf = std::numeric_limits<double>::infinity();
+  return typename SapHolonomicConstraint<T>::Parameters{
+      Vector1<T>(-kInf), Vector1<T>(kInf), Vector1<T>(kInf), Vector1<T>(0.0),
+      kBeta};
+}
+
+template <typename T>
+typename SapHolonomicConstraint<T>::Kinematics
+SapCouplerConstraint<T>::MakeSapHolonomicConstraintKinematics(
+    const Kinematics& kinematics) {
+  Vector1<T> g(kinematics.q0 - kinematics.gear_ratio * kinematics.q1 -
+               kinematics.offset);    // Constraint function.
+  Vector1<T> b = Vector1<T>::Zero();  // Bias term.
+
+  // Determine the single clique or two clique case.
+  if (kinematics.clique0 == kinematics.clique1) {
+    MatrixX<T> J0 = MatrixX<T>::Zero(1, kinematics.clique_nv0);
+    J0(0, kinematics.clique_dof0) = 1.0;
+    J0(0, kinematics.clique_dof1) = -kinematics.gear_ratio;
+
+    return typename SapHolonomicConstraint<T>::Kinematics(
+        std::move(g),
+        SapConstraintJacobian<T>(kinematics.clique0, std::move(J0)),
+        std::move(b));
+  } else {
+    MatrixX<T> J0 = MatrixX<T>::Zero(1, kinematics.clique_nv0);
+    MatrixX<T> J1 = MatrixX<T>::Zero(1, kinematics.clique_nv1);
+    J0(0, kinematics.clique_dof0) = 1.0;
+    J1(0, kinematics.clique_dof1) = -kinematics.gear_ratio;
+
+    return typename SapHolonomicConstraint<T>::Kinematics(
+        std::move(g),
+        SapConstraintJacobian<T>(kinematics.clique0, std::move(J0),
+                                 kinematics.clique1, std::move(J1)),
+        std::move(b));
+  }
+}
+
+template <typename T>
+void SapCouplerConstraint<T>::DoAccumulateGeneralizedImpulses(
+    int c, const Eigen::Ref<const VectorX<T>>& gamma,
+    EigenPtr<VectorX<T>> tau) const {
+  // τ = Jᵀ⋅γ
+  if (c == 0) {
+    (*tau)(kinematics().clique_dof0) += gamma(0);
+  }
+
+  if (this->num_cliques() == 1 || c == 1) {
+    (*tau)(kinematics().clique_dof1) -= kinematics().gear_ratio * gamma(0);
+  }
+}
+
+}  // namespace internal
+}  // namespace contact_solvers
+}  // namespace multibody
+}  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    class ::drake::multibody::contact_solvers::internal::SapCouplerConstraint)

multibody/contact_solvers/sap/sap_coupler_constraint.h

@@ -0,0 +1,117 @@
+#pragma once
+
+#include <memory>
+#include <utility>
+
+#include "drake/common/drake_copyable.h"
+#include "drake/common/eigen_types.h"
+#include "drake/multibody/contact_solvers/sap/sap_holonomic_constraint.h"
+
+namespace drake {
+namespace multibody {
+namespace contact_solvers {
+namespace internal {
+
+/* Implements a SAP (compliant) coupler constraint between two dofs.
+ Given positions of dof 0 and 1 as q₀ and q₁, respectively, this constraint
+ enforces:
+   q₀ = ρ⋅q₁ + Δq
+ where ρ is the gear ratio and Δq is a fixed offset.
+
+ More precisely, this constraint enforces a single holonomic constraint
+ equation:
+   g = q₀ - ρ⋅q₁ - Δq = 0
+ which produces a constraint impulse γ ∈ ℝ.
+
+ The resulting generalized impulse on dof 0 is:
+   j₀ = γ
+ And the generalized impulse on dof 1 is:
+   j₁ = −ργ
+
+ @tparam_nonsymbolic_scalar */
+template <typename T>
+class SapCouplerConstraint final : public SapHolonomicConstraint<T> {
+ public:
+  /* We do not allow copy, move, or assignment generally to avoid slicing. */
+  //@{
+  SapCouplerConstraint& operator=(const SapCouplerConstraint&) = delete;
+  SapCouplerConstraint(SapCouplerConstraint&&) = delete;
+  SapCouplerConstraint& operator=(SapCouplerConstraint&&) = delete;
+  //@}
+
+  /* Struct to store the kinematics of the the constraint in its current
+   configuration, when it gets constructed. */
+  struct Kinematics {
+    /* Index of clique 0. */
+    int clique0;
+    /* Clique local index of dof 0. */
+    int clique_dof0;
+    /* Clique 0 number of velocities. */
+    int clique_nv0;
+    /* Position of dof 0. */
+    T q0;
+    /* Index of clique 1. */
+    int clique1;
+    /* Clique local index of dof 1. */
+    int clique_dof1;
+    /* Clique 1 number of velocities. */
+    int clique_nv1;
+    /* Position of dof 1. */
+    T q1;
+    /* Gear ratio ρ. */
+    T gear_ratio;
+    /* Constraint function bias Δq. */
+    T offset;
+  };
+
+  /* Constructs a coupler constraint given its kinematics in a particular
+   configuration. */
+  explicit SapCouplerConstraint(Kinematics kinematics);
+
+  const Kinematics& kinematics() const { return kinematics_; }
+
+ private:
+  /* Private copy construction is enabled to use in the implementation of
+     DoClone(). */
+  SapCouplerConstraint(const SapCouplerConstraint&) = default;
+
+  /* Accumulates generalized impulses applied by this constraint on the c-th
+   clique.
+   @param[in] c The c-th clique of index clique(c).
+   @param[in] gamma Impulses for this constraint, of size
+   num_constraint_equations().
+   @param[out] tau On output this function will accumulate the generalized
+   impulses applied by this constraint on the c-th clique.
+  */
+  void DoAccumulateGeneralizedImpulses(
+      int c, const Eigen::Ref<const VectorX<T>>& gamma,
+      EigenPtr<VectorX<T>> tau) const final;
+
+  /* No-op for this constraint. */
+  void DoAccumulateSpatialImpulses(int, const Eigen::Ref<const VectorX<T>>&,
+                                   SpatialForce<T>*) const final {}
+
+  /* Helper used at construction. This method makes the parameters needed by the
+   base class SapHolonomicConstraint. */
+  static typename SapHolonomicConstraint<T>::Parameters
+  MakeSapHolonomicConstraintParameters();
+
+  /* Helper used at construction. Makes the constraint function and Jacobian
+   needed to initialize the base class SapHolonomicConstraint.
+   @returns Holonomic constraint kinematics needed at construction of the
+   parent SapHolonomicConstraint. */
+  static typename SapHolonomicConstraint<T>::Kinematics
+  MakeSapHolonomicConstraintKinematics(const Kinematics& kinematics);
+
+  std::unique_ptr<SapConstraint<T>> DoClone() const final {
+    return std::unique_ptr<SapCouplerConstraint<T>>(
+        new SapCouplerConstraint<T>(*this));
+  }
+
+  Kinematics kinematics_;
+};
+
+}  // namespace internal
+}  // namespace contact_solvers
+}  // namespace multibody
+}  // namespace drake