Implement capability to define coupler constraints.

This PR:
  - Introduces MultibodyPlant::AddCouplerConstraint() to declare coupler constraints.
  - Update the CompliantContactMnager to build a contact problem including these coupler constraints.
  - Update the simple_gripper.cc example to showcase this new capability.
    The SDF model is updated so that now finger positions are symmetric from the center of the gripper.

examples/simple_gripper/simple_gripper.cc

@@ -15,18 +15,19 @@
 #include "drake/multibody/plant/contact_results.h"
 #include "drake/multibody/plant/contact_results_to_lcm.h"
 #include "drake/multibody/plant/multibody_plant.h"
+#include "drake/multibody/plant/multibody_plant_config_functions.h"
 #include "drake/multibody/tree/prismatic_joint.h"
 #include "drake/systems/analysis/simulator.h"
 #include "drake/systems/analysis/simulator_gflags.h"
 #include "drake/systems/analysis/simulator_print_stats.h"
 #include "drake/systems/framework/diagram_builder.h"
 #include "drake/systems/primitives/sine.h"
-
 namespace drake {
 namespace examples {
 namespace simple_gripper {
 namespace {
 
+using Eigen::Vector2d;
 using Eigen::Vector3d;
 using geometry::SceneGraph;
 using geometry::Sphere;
@@ -38,6 +39,7 @@ using multibody::ConnectContactResultsToDrakeVisualizer;
 using multibody::CoulombFriction;
 using multibody::ModelInstanceIndex;
 using multibody::MultibodyPlant;
+using multibody::MultibodyPlantConfig;
 using multibody::Parser;
 using multibody::PrismaticJoint;
 using systems::Sine;
@@ -86,8 +88,8 @@ DEFINE_double(rz, 0,
               "The z-rotation of the mug around its origin - the center "
               "of its bottom. [degrees]. Extrinsic rotation order: X, Y, Z");
 
-// Gripping force.
-DEFINE_double(gripper_force, 10,
+// Grip force. The amount of force we want to apply on the manipuland.
+DEFINE_double(grip_force, 10,
               "The force to be applied by the gripper. [N]. "
               "A value of 0 indicates a fixed grip width as set with option "
               "grip_width.");
@@ -100,6 +102,16 @@ DEFINE_double(frequency, 2.0,
               "The frequency of the harmonic oscillations "
               "carried out by the gripper. [Hz].");
 
+DEFINE_string(discrete_solver, "sap",
+              "Discrete contact solver. Options are: 'tamsi', 'sap'.");
+DEFINE_double(
+    coupler_gear_ratio, -1.0,
+    "When using SAP, the left finger's position qₗ is constrainted to qₗ = "
+    "ρ⋅qᵣ, where qᵣ is the right finger's position and ρ is this "
+    "coupler_gear_ration parameter (dimensionless). If TAMSI used, "
+    "then the right finger is locked, only the left finger moves and this "
+    "parameter is ignored.");
+
 // The pad was measured as a torus with the following major and minor radii.
 const double kPadMajorRadius = 14e-3;  // 14 mm.
 const double kPadMinorRadius = 6e-3;   // 6 mm.
@@ -154,8 +166,11 @@ int do_main() {
   DRAKE_DEMAND(FLAGS_simulator_max_time_step > 0);
   DRAKE_DEMAND(FLAGS_mbp_discrete_update_period >= 0);
 
-  auto [plant, scene_graph] = multibody::AddMultibodyPlantSceneGraph(
-      &builder, FLAGS_mbp_discrete_update_period);
+  MultibodyPlantConfig plant_config;
+  plant_config.time_step = FLAGS_mbp_discrete_update_period;
+  plant_config.discrete_contact_solver = FLAGS_discrete_solver;
+  auto [plant, scene_graph] =
+      multibody::AddMultibodyPlant(plant_config, &builder);
 
   Parser parser(&plant);
   std::string full_name =
@@ -193,7 +208,7 @@ int do_main() {
   // Pads offset from the center of a finger. pad_offset = 0 means the center of
   // the spheres is located right at the center of the finger.
   const double pad_offset = 0.0046;
-  if (FLAGS_gripper_force == 0) {
+  if (FLAGS_grip_force == 0) {
     // We then fix everything to the right finger and leave the left finger
     // "free" with no applied forces (thus we see it not moving).
     const double finger_width = 0.007;  // From the visual in the SDF file.
@@ -205,6 +220,19 @@ int do_main() {
     AddGripperPads(&plant, +pad_offset, left_finger);
   }
 
+  // Get joints so that we can set initial conditions.
+  const PrismaticJoint<double>& left_slider =
+      plant.GetJointByName<PrismaticJoint>("left_slider");
+  const PrismaticJoint<double>& right_slider =
+      plant.GetJointByName<PrismaticJoint>("right_slider");
+
+  // TAMSI does not support general constraints. If using TAMSI, we simplify the
+  // model to have the right finger locked.
+  if (FLAGS_discrete_solver != "tamsi") {
+    plant.AddCouplerConstraint(left_slider, right_slider,
+                               FLAGS_coupler_gear_ratio);
+  }
+
   // Now the model is complete.
   plant.Finalize();
 
@@ -259,14 +287,40 @@ int do_main() {
   const double f0 = mass * a0;           // Force amplitude, Newton.
   fmt::print("Acceleration amplitude = {:8.4f} m/s²\n", a0);
 
+  // The actuation force that must be applied in order to attain a given grip
+  // force depends on how the gripper is modeled.
+  // When we model the gripper as having the right finger locked and only the
+  // actuated left finger moves, it is clear that the actuation force directly
+  // balances the grip force (as done when usint the TAMSI solver).
+  // When we model the gripper with two moving fingers coupled to move together
+  // with a constraint, the relationship is not as obvious. We can think of the
+  // constrained mechanism as a system of pulleys and therefore we'd need to
+  // consider a free-body diagram to find the relationship between forces. A
+  // much simpler approach however is to consider virtual displacements. When
+  // the object is in a steady grasp between the fingers, there is an equal and
+  // opposite force on each finger of magnitude equal to the grip force, G (or
+  // otherwise the mug would move). A small motion δqᵣ of the right finger
+  // then leads to a motion δqₗ = ρ⋅δqᵣ of the left finger. If U is the
+  // actuation on the right finger, the virtual work due to U is δWu = U⋅δqᵣ.
+  // The virtual work due to the contact forces (G on each finger) is δWg =
+  // -G⋅δqₗ + G⋅δqᵣ = G⋅(1-ρ)⋅δqᵣ. In equilibrium we must have δWu = δWg for
+  // arbitrary δqᵣ and therefore we find U = G⋅(1-ρ). For instance, when ρ = -1
+  // (fingers move in opposition) we find that U = 2 G and thus we must apply
+  // twice as much force as the grip force. This makes sense if we consider a
+  // system of pulleys for this mechanism.
+  const double grip_actuation_force =
+      FLAGS_discrete_solver == "tamsi"
+          ? FLAGS_grip_force
+          : (1.0 - FLAGS_coupler_gear_ratio) * FLAGS_grip_force;
+
   // Notice we are using the same Sine source to:
   //   1. Generate a harmonic forcing of the gripper with amplitude f0 and
   //      angular frequency omega.
   //   2. Impose a constant force to the left finger. That is, a harmonic
   //      forcing with "zero" frequency.
-  const Vector2<double> amplitudes(f0, FLAGS_gripper_force);
-  const Vector2<double> frequencies(omega, 0.0);
-  const Vector2<double> phases(0.0, M_PI_2);
+  const Vector2d amplitudes(f0, grip_actuation_force);
+  const Vector2d frequencies(omega, 0.0);
+  const Vector2d phases(0.0, M_PI_2);
   const auto& harmonic_force =
       *builder.AddSystem<Sine>(amplitudes, frequencies, phases);
 
@@ -282,12 +336,10 @@ int do_main() {
   systems::Context<double>& plant_context =
       diagram->GetMutableSubsystemContext(plant, diagram_context.get());
 
-  // Get joints so that we can set initial conditions.
-  const PrismaticJoint<double>& finger_slider =
-      plant.GetJointByName<PrismaticJoint>("finger_sliding_joint");
-
-  // Set initial position of the left finger.
-  finger_slider.set_translation(&plant_context, -FLAGS_grip_width);
+  // Set initial position of the fingers.
+  const double finger_offset = FLAGS_grip_width / 2.0;
+  left_slider.set_translation(&plant_context, -finger_offset);
+  right_slider.set_translation(&plant_context, finger_offset);
 
   // Initialize the mug pose to be right in the middle between the fingers.
   const Vector3d& p_WBr =
@@ -309,6 +361,14 @@ int do_main() {
   translate_joint.set_translation(&plant_context, 0.0);
   translate_joint.set_translation_rate(&plant_context, v0);
 
+  if (FLAGS_discrete_solver == "tamsi") {
+    drake::log()->warn(
+        "discrete_solver = 'tamsi'. Since TAMSI does not support coupler "
+        "constraints to model the coupling of the fingers, this simple example "
+        "locks the right finger and only the left finger is allowed to move.");
+    right_slider.Lock(&plant_context);
+  }
+
   // Set up simulator.
   auto simulator =
       MakeSimulatorFromGflags(*diagram, std::move(diagram_context));

examples/simple_gripper/simple_gripper.sdf

@@ -3,15 +3,22 @@
   <!-- Note: This is the accompaning SDF file for the example demo in
        simple_gripper.cc and therefore these two files must be kept in sync.
 
-         This file defines the model for a simple gripper having two fingers.
+       This file defines the model for a simple gripper having two fingers.
        The right finger is fixed to the body of the gripper. The left finger
        is attached to the main body by a prismatic joint that allows it to
        perform a grip.
-         The frame of the gripper, G, has its x-axis pointing to the right
+
+       The frame of the gripper, G, has its x-axis pointing to the right
        of the gripper, its y-axis pointing "forward" (towards the fingers
        side) and, the z-axis pointing upwards. This file only defines visual
        geometry but not contact geometry, allowing the demo in
        simple_gripper.cc to add contact geometry programmatically.
+
+      simple_gripper.cc adds pads to the model programatically. Including the
+      thickness of these pads, 6 mm, and the offset from the center of the
+      corresponding finger, 4.6 mm, each finger is positioned along the x-axis
+      such that at q=0 the pads from each finger barely touch. This is 6 mm +
+      4.6 mm = 10.5 mm.
   -->
   <model name="simple_gripper">
     <!-- Pose X_WG of the gripper model frame G in the world frame W. -->
@@ -67,7 +74,10 @@
       </visual>
     </link>
     <link name="left_finger">
-      <pose>0.040 0.029 0 0 0 0</pose>
+      <!-- Each finger is positioned along the x-axis such that at q=0 the pads
+      of each finger barely touch each other. See notes at the top of this
+      file. -->
+      <pose>-0.0105 0.029 0 0 0 0</pose>
       <inertial>
         <mass>0.05</mass>
         <inertia>
@@ -88,7 +98,10 @@
       </visual>
     </link>
     <link name="right_finger">
-      <pose>0.047 0.029 0 0 0 0</pose>
+      <!-- Each finger is positioned along the x-axis such that at q=0 the pads
+      of each finger barely touch each other. See notes at the top of this
+      file. -->
+      <pose>0.0105 0.029 0 0 0 0</pose>
       <inertial>
         <mass>0.05</mass>
         <inertia>
@@ -108,12 +121,8 @@
         </geometry>
       </visual>
     </link>
-    <joint name="weld_right_finger" type="fixed">
+    <joint name="left_slider" type="prismatic">
       <parent>body</parent>
-      <child>right_finger</child>
-    </joint>
-    <joint name="finger_sliding_joint" type="prismatic">
-      <parent>right_finger</parent>
       <child>left_finger</child>
       <axis>
         <xyz>1 0 0</xyz>
@@ -124,5 +133,17 @@
         </limit>
       </axis>
     </joint>
+    <joint name="right_slider" type="prismatic">
+      <parent>body</parent>
+      <child>right_finger</child>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <!-- Drake attaches an actuator to all joints with a non-zero effort
+             limit. We do NOT want an actuator for this joint. -->
+        <limit>
+          <effort>0</effort>
+        </limit>
+      </axis>
+    </joint>
   </model>
 </sdf>

multibody/plant/BUILD.bazel

@@ -18,6 +18,7 @@ drake_cc_package_library(
     visibility = ["//visibility:public"],
     deps = [
         ":calc_distance_and_time_derivative",
+        ":constraint_specs",
         ":contact_jacobians",
         ":contact_results",
         ":coulomb_friction",
@@ -38,6 +39,20 @@ drake_cc_package_library(
     ],
 )
 
+drake_cc_library(
+    name = "constraint_specs",
+    srcs = [
+        "constraint_specs.cc",
+    ],
+    hdrs = [
+        "constraint_specs.h",
+    ],
+    deps = [
+        "//common:default_scalars",
+        "//multibody/tree:multibody_tree_indexes",
+    ],
+)
+
 drake_cc_library(
     name = "deformable_model",
     srcs = [
@@ -112,6 +127,7 @@ drake_cc_library(
     ],
     visibility = ["//visibility:private"],
     deps = [
+        ":constraint_specs",
         ":contact_jacobians",
         ":contact_results",
         ":coulomb_friction",

multibody/plant/compliant_contact_manager.cc

@@ -16,6 +16,7 @@
 #include "drake/multibody/contact_solvers/contact_solver_utils.h"
 #include "drake/multibody/contact_solvers/sap/sap_contact_problem.h"
 #include "drake/multibody/contact_solvers/sap/sap_friction_cone_constraint.h"
+#include "drake/multibody/contact_solvers/sap/sap_holonomic_constraint.h"
 #include "drake/multibody/contact_solvers/sap/sap_limit_constraint.h"
 #include "drake/multibody/contact_solvers/sap/sap_solver.h"
 #include "drake/multibody/contact_solvers/sap/sap_solver_results.h"
@@ -32,6 +33,7 @@ using drake::multibody::contact_solvers::internal::ExtractTangent;
 using drake::multibody::contact_solvers::internal::SapConstraint;
 using drake::multibody::contact_solvers::internal::SapContactProblem;
 using drake::multibody::contact_solvers::internal::SapFrictionConeConstraint;
+using drake::multibody::contact_solvers::internal::SapHolonomicConstraint;
 using drake::multibody::contact_solvers::internal::SapLimitConstraint;
 using drake::multibody::contact_solvers::internal::SapSolver;
 using drake::multibody::contact_solvers::internal::SapSolverResults;
@@ -876,6 +878,73 @@ void CompliantContactManager<T>::AddLimitConstraints(
   }
 }
 
+template <typename T>
+void CompliantContactManager<T>::AddCouplerConstraints(
+    const systems::Context<T>& context, SapContactProblem<T>* problem) const {
+  DRAKE_DEMAND(problem != nullptr);
+
+  // Previous time step positions.
+  const VectorX<T> q0 = plant().GetPositions(context);
+
+  // Couplers do not have impulse limits, they are bi-lateral constraints. Each
+  // coupler constraint introduces a single constraint equation.
+  constexpr double kInfinity = std::numeric_limits<double>::infinity();
+  const Vector1<T> gamma_lower(-kInfinity);
+  const Vector1<T> gamma_upper(kInfinity);
+
+  // Stiffness and dissipation are set so that the constraint is in the
+  // "near-rigid" regime, [Castro et al., 2022].
+  const Vector1<T> stiffness(kInfinity);
+  const Vector1<T> relaxation_time(plant().time_step());
+
+  for (const CouplerConstraintSpecs<T>& info :
+       this->coupler_constraints_specs()) {
+    const Joint<T>& joint0 = plant().get_joint(info.joint0_index);
+    const Joint<T>& joint1 = plant().get_joint(info.joint1_index);
+    const int dof0 = joint0.velocity_start();
+    const int dof1 = joint1.velocity_start();
+    const TreeIndex tree0 = tree_topology().velocity_to_tree_index(dof0);
+    const TreeIndex tree1 = tree_topology().velocity_to_tree_index(dof1);
+
+    // Sanity check.
+    DRAKE_DEMAND(tree0.is_valid() && tree1.is_valid());
+
+    // DOFs local to their tree.
+    const int tree_dof0 = dof0 - tree_topology().tree_velocities_start(tree0);
+    const int tree_dof1 = dof1 - tree_topology().tree_velocities_start(tree1);
+
+    // Constraint function defined as g = q₀ - ρ⋅q₁ - Δq, with ρ the gear ratio
+    // and Δq a fixed position offset.
+    const Vector1<T> g0(q0[dof0] - info.gear_ratio * q0[dof1] - info.offset);
+
+    // TODO(amcastro-tri): consider exposing this parameter.
+    const double beta = 0.1;
+
+    const typename SapHolonomicConstraint<T>::Parameters parameters{
+        gamma_lower, gamma_upper, stiffness, relaxation_time, beta};
+
+    if (tree0 == tree1) {
+      const int nv = tree_topology().num_tree_velocities(tree0);
+      MatrixX<T> J = MatrixX<T>::Zero(1, nv);
+      // J = dg/dv
+      J(0, tree_dof0) = 1.0;
+      J(0, tree_dof1) = -info.gear_ratio;
+
+      problem->AddConstraint(std::make_unique<SapHolonomicConstraint<T>>(
+          tree0, g0, J, parameters));
+    } else {
+      const int nv0 = tree_topology().num_tree_velocities(tree0);
+      const int nv1 = tree_topology().num_tree_velocities(tree1);
+      MatrixX<T> J0 = MatrixX<T>::Zero(1, nv0);
+      MatrixX<T> J1 = MatrixX<T>::Zero(1, nv1);
+      J0(0, tree_dof0) = 1.0;
+      J1(0, tree_dof1) = -info.gear_ratio;
+      problem->AddConstraint(std::make_unique<SapHolonomicConstraint<T>>(
+          tree0, tree1, g0, J0, J1, parameters));
+    }
+  }
+}
+
 template <typename T>
 void CompliantContactManager<T>::CalcContactProblemCache(
     const systems::Context<T>& context, ContactProblemCache<T>* cache) const {
@@ -891,6 +960,7 @@ void CompliantContactManager<T>::CalcContactProblemCache(
   // Do not change this order here!
   cache->R_WC = AddContactConstraints(context, &problem);
   AddLimitConstraints(context, problem.v_star(), &problem);
+  AddCouplerConstraints(context, &problem);
 }
 
 template <typename T>