[multibody] Apply surface velocity in discrete contact

multibody/contact_solvers/contact_configuration.h

@@ -30,7 +30,8 @@ struct ContactConfiguration {
         .vn = ExtractDoubleOrThrow(vn),
         .fe = ExtractDoubleOrThrow(fe),
         .R_WC =
-            math::RotationMatrix<double>(math::DiscardGradient(R_WC.matrix()))};
+            math::RotationMatrix<double>(math::DiscardGradient(R_WC.matrix())),
+        .v_b = math::DiscardGradient(v_b)};
   }
 
   bool operator==(const ContactConfiguration& other) const {
@@ -42,6 +43,7 @@ struct ContactConfiguration {
     if (vn != other.vn) return false;
     if (fe != other.fe) return false;
     if (!R_WC.IsExactlyEqualTo(other.R_WC)) return false;
+    if (v_b != other.v_b) return false;
     return true;
   }
 
@@ -77,6 +79,15 @@ struct ContactConfiguration {
   // Orientation of contact frame C in the world frame W.
   // Rz_WC = R_WC.col(2) corresponds to the normal from object A into object B.
   math::RotationMatrix<T> R_WC;
+
+  // Mathematically, this is a bias term to the contact velocity: vc = Jv + v_b,
+  // where vc is the relative velocity of the two bodies at the contact point,
+  // J the contact jacobian and v the vector of generalized velocities. In
+  // practice, it is used to model additional velocity at the contact point,
+  // such as when an imaginary conveyor belt is wrapped around one or both of
+  // the objects in contact.
+  // The velocity bias is expressed in the contact frame C.
+  Vector3<T> v_b{0, 0, 0};
 };
 
 // Extracts a ContactConfiguration from the given DiscreteContactPair.
@@ -90,7 +101,8 @@ ContactConfiguration<T> MakeContactConfiguration(
                                  .phi = input.phi0,
                                  .vn = input.vn0,
                                  .fe = input.fn0,
-                                 .R_WC = input.R_WC};
+                                 .R_WC = input.R_WC,
+                                 .v_b = input.v_b};
 }
 
 }  // namespace internal

multibody/contact_solvers/sap/sap_constraint.h

@@ -291,6 +291,14 @@ class SapConstraint {
       const math::internal::PartialPermutation& clique_permutation,
       const std::vector<std::vector<int>>& per_clique_known_dofs) const;
 
+  /* Returns the constant velocity bias v_b such that the physical constraint
+   velocity is vc_phys = J⋅v + v_b.  Most constraints have zero bias and do
+   not override DoCalcBiasVelocity(). Constraints that model contact with a
+   moving surface (e.g. a conveyor belt) can return a non-zero value.
+   The bias is expressed in the constraint frame C.
+   @post The returned vector has size num_constraint_equations(). */
+  VectorX<T> bias_velocity() const { return DoCalcBiasVelocity(); }
+
  protected:
   /* Protected copy construction is enabled for sub-classes to use in their
    implementation of DoClone(). */
@@ -326,6 +334,12 @@ class SapConstraint {
                                            SpatialForce<T>*) const = 0;
   virtual std::unique_ptr<SapConstraint<T>> DoClone() const = 0;
   virtual std::unique_ptr<SapConstraint<double>> DoToDouble() const = 0;
+  /* Default implementation returns zero bias. Constraints that include a bias
+   (e.g., friction constraint on a moving surface, such as a conveyor belt)
+   should override this. */
+  virtual VectorX<T> DoCalcBiasVelocity() const {
+    return VectorX<T>::Zero(num_constraint_equations());
+  }
   // @}
 
  private:

multibody/contact_solvers/sap/sap_contact_problem.cc

@@ -181,7 +181,9 @@ void SapContactProblem<T>::ExpandContactSolverResults(
   results->vc.setZero();
   results->j.setZero();
 
-  // Set vc to vc* for known DoFs. Unknown DoFs will be overwritten below.
+  // Initialize constraint velocities using the free-motion generalized
+  // velocities, vc = J * v_star + v_b. Values for participating constraints
+  // will be overwritten from the reduced_results below.
   for (int i = 0; i < num_constraints(); ++i) {
     const SapConstraint<T>& c = get_constraint(i);
 
@@ -199,6 +201,7 @@ void SapContactProblem<T>::ExpandContactSolverResults(
                              num_velocities(c.second_clique())),
           &vc_segment);
     }
+    vc_segment += c.bias_velocity();
   }
 
   // Copy v and j for participating velocities.
@@ -210,6 +213,8 @@ void SapContactProblem<T>::ExpandContactSolverResults(
   // Copy gamma and vc for participating constraints.
   reduced_mapping.constraint_equation_permutation.ApplyInverse(
       reduced_results.gamma, &results->gamma);
+  // Note: for participating constraints, the velocity bias has already been
+  // added in SapSolver::PackSapSolverResults().
   reduced_mapping.constraint_equation_permutation.ApplyInverse(
       reduced_results.vc, &results->vc);
 }

multibody/contact_solvers/sap/sap_friction_cone_constraint.cc

@@ -98,8 +98,9 @@ void SapFrictionConeConstraint<T>::DoCalcData(
   auto& data =
       abstract_data->get_mutable_value<SapFrictionConeConstraintData<T>>();
 
-  data.mutable_vc() = vc;
-  data.mutable_y() = data.R_inv().asDiagonal() * (data.v_hat() - vc);
+  // Both vc and v_b are expressed in the contact frame C.
+  data.mutable_vc() = vc + this->bias_velocity();
+  data.mutable_y() = data.R_inv().asDiagonal() * (data.v_hat() - data.vc());
   const auto yt = data.y().template head<2>();
   data.mutable_yr() = SoftNorm(yt);
   data.mutable_yn() = data.y()(2);

multibody/contact_solvers/sap/sap_friction_cone_constraint.h

@@ -279,6 +279,8 @@ class SapFrictionConeConstraint final : public SapConstraint<T> {
   void ProjectImpulse(const SapFrictionConeConstraintData<T>& data,
                       Vector3<T>* gamma) const;
 
+  VectorX<T> DoCalcBiasVelocity() const final { return configuration_.v_b; }
+
   Parameters parameters_;
   ContactConfiguration<T> configuration_;
 };

multibody/contact_solvers/sap/sap_hunt_crossley_constraint.cc

@@ -59,7 +59,11 @@ std::unique_ptr<AbstractValue> SapHuntCrossleyConstraint<T>::DoMakeData(
       data.invariant_data;
   p.dt = time_step;
   const T& fe0 = configuration_.fe;
-  const T& vn0 = configuration_.vn;
+  // Generally, we expect the velocity bias to be zero in the contact normal
+  // direction. However, we conservatively add the z-component into the normal
+  // velocity measure to future-proof the code against any change that
+  // were to introduce non-tangential bias.
+  const T& vn0 = configuration_.vn + this->bias_velocity().z();
   const T damping = max(0.0, 1.0 - d * vn0);
   const T ne0 = max(0.0, time_step * fe0);
   p.n0 = ne0 * damping;
@@ -171,9 +175,9 @@ void SapHuntCrossleyConstraint<T>::DoCalcData(
   const T& epsilon_soft = data.invariant_data.epsilon_soft;
 
   // Computations dependent on vc.
-  data.vc = vc;
-  data.vn = vc[2];
-  data.vt = vc.template head<2>();
+  data.vc = vc + this->bias_velocity();
+  data.vn = data.vc.z();
+  data.vt = data.vc.template head<2>();
   data.vt_soft = SoftNorm(data.vt, epsilon_soft);
   data.t_soft = data.vt / (data.vt_soft + epsilon_soft);
   switch (parameters_.model) {

multibody/contact_solvers/sap/sap_hunt_crossley_constraint.h

@@ -229,6 +229,8 @@ class SapHuntCrossleyConstraint final : public SapConstraint<T> {
   // @param vn Normal component of the contact velocity.
   T CalcDiscreteHuntCrossleyImpulseGradient(const T& dt, const T& vn) const;
 
+  VectorX<T> DoCalcBiasVelocity() const final { return configuration_.v_b; }
+
   Parameters parameters_;
   ContactConfiguration<T> configuration_;
 };

multibody/contact_solvers/sap/sap_solver.cc

@@ -65,6 +65,15 @@ void SapSolver<T>::PackSapSolverResults(const SapModel<T>& model,
   // original order described by the model right after.
   const VectorX<T>& vc_clustered = model.EvalConstraintVelocities(context);
   model.impulses_permutation().ApplyInverse(vc_clustered, &results->vc);
+  // Add per-constraint kinematic velocity bias v_b so that vc = J⋅v + v_b,
+  // matching the physical contact velocity seen by each constraint.
+  const SapContactProblem<T>& problem = model.problem();
+  for (int i = 0; i < problem.num_constraints(); ++i) {
+    const SapConstraint<T>& c = problem.get_constraint(i);
+    const int start = problem.constraint_equations_start(i);
+    results->vc.segment(start, c.num_constraint_equations()) +=
+        c.bias_velocity();
+  }
   const VectorX<T>& gamma_clustered = model.EvalImpulses(context);
   model.impulses_permutation().ApplyInverse(gamma_clustered, &results->gamma);
 

multibody/contact_solvers/sap/sap_solver_results.h

@@ -27,8 +27,9 @@ struct SapSolverResults {
   // Constraints' impulses, of size `num_constraint_equations`.
   VectorX<T> gamma;
 
-  // Constraints' velocities vc = J⋅v, where J is the contact Jacobian. Of size
-  // `num_constraint_equations`.
+  // Constraints' velocities vc = J⋅v + v_b, where J is the contact Jacobian
+  // and v_b is the per-constraint kinematic velocity bias (e.g. a conveyor
+  // belt surface velocity). Of size `num_constraint_equations`.
   VectorX<T> vc;
 
   // Vector of generalized impulses j = Jᵀ⋅γ due to constraints, where J is the

multibody/contact_solvers/sap/test/sap_contact_problem_test.cc

@@ -46,21 +46,35 @@ class TestConstraint final : public SapConstraint<T> {
  public:
   // Constructor for a constraint on a single clique.
   // No objects are registered.
-  TestConstraint(int num_constraint_equations, int clique, int clique_nv)
+  TestConstraint(int num_constraint_equations, int clique, int clique_nv,
+                 VectorX<T> bias = {})
       : SapConstraint<T>(
             {clique, MatrixX<T>::Ones(num_constraint_equations, clique_nv)},
-            {}) {}
+            {}),
+        bias_(std::move(bias)) {
+    if (bias_.size() == 0) {
+      bias_ = VectorX<T>::Zero(num_constraint_equations);
+    }
+    DRAKE_DEMAND(bias_.size() == num_constraint_equations);
+  }
 
   // Constructor for a constraint between two cliques.
   // Registers objects with index first_clique and second_clique, for testing.
   TestConstraint(int num_constraint_equations, int first_clique,
-                 int first_clique_nv, int second_clique, int second_clique_nv)
+                 int first_clique_nv, int second_clique, int second_clique_nv,
+                 VectorX<T> bias = {})
       : SapConstraint<T>(
             {first_clique,
              MatrixX<T>::Ones(num_constraint_equations, first_clique_nv),
              second_clique,
              MatrixX<T>::Ones(num_constraint_equations, second_clique_nv)},
-            {}) {}
+            {}),
+        bias_(std::move(bias)) {
+    if (bias_.size() == 0) {
+      bias_ = VectorX<T>::Zero(num_constraint_equations);
+    }
+    DRAKE_DEMAND(bias_.size() == num_constraint_equations);
+  }
 
   // N.B no-op overloads to allow us compile this testing constraint. These
   // methods are only tested for specific derived classes, not in this file.
@@ -109,6 +123,10 @@ class TestConstraint final : public SapConstraint<T> {
         this->num_velocities(0), this->second_clique(),
         this->num_velocities(1));
   }
+
+  VectorX<T> DoCalcBiasVelocity() const final { return bias_; }
+
+  VectorX<T> bias_;
 };
 
 // Test construction of an empty problem.
@@ -594,6 +612,36 @@ GTEST_TEST(ContactProblem, ExpandContactSolverResults) {
   EXPECT_TRUE(CompareMatrices(results.vc, vc_expected));
 }
 
+GTEST_TEST(ContactProblem, ExpandContactSolverResultsWithBiasVelocity) {
+  const double time_step = 0.01;
+  const std::vector<MatrixXd> A{S22};
+  const VectorXd v_star = (VectorXd(2) << 1.0, 2.0).finished();
+  SapContactProblem<double> problem(time_step, std::move(A), std::move(v_star));
+
+  const VectorX<double> v_bias =
+      (VectorX<double>(3) << 0.25, -0.5, 0.75).finished();
+  problem.AddConstraint(
+      std::make_unique<TestConstraint<double>>(3, 0, 2, v_bias));
+
+  // Lock all dofs, so the reduced problem has no participating constraint
+  // equations. Expanding the empty result still reports vc = J⋅v* + v_b for
+  // the original problem.
+  ReducedMapping mapping;
+  std::unique_ptr<SapContactProblem<double>> reduced_problem =
+      problem.MakeReduced({0, 1}, {{0, 1}}, &mapping);
+  ASSERT_EQ(reduced_problem->num_constraint_equations(), 0);
+
+  SapSolverResults<double> reduced_results;
+  reduced_results.Resize(0, 0);
+
+  SapSolverResults<double> results;
+  problem.ExpandContactSolverResults(mapping, reduced_results, &results);
+
+  const VectorX<double> vc_expected =
+      VectorX<double>::Constant(3, 3.0) + v_bias;
+  EXPECT_TRUE(CompareMatrices(results.vc, vc_expected));
+}
+
 GTEST_TEST(ContactProblem, CalcConstraintMultibodyForces) {
   const double time_step = 0.01;
   const std::vector<MatrixXd> A{S22, S33, S44, S22};

multibody/contact_solvers/sap/test/sap_friction_cone_constraint_test.cc

@@ -61,7 +61,11 @@ ContactConfiguration<T> MakeArbitraryConfiguration() {
                                  .phi = -2.5e-3,
                                  .vn = kInf,
                                  .fe = kInf,
-                                 .R_WC = RotationMatrix<T>::Identity()};
+                                 .R_WC = RotationMatrix<T>::Identity(),
+                                 // We're leaving this as zero here to preserve
+                                 // the majority of tests. We'll set it
+                                 // explicitly when we need to test cloning.
+                                 .v_b = Vector3<T>(0.0, 0.0, 0.0)};
 }
 
 template <typename T = double>
@@ -206,6 +210,27 @@ GTEST_TEST(SapFrictionConeConstraint, CalcRegularization) {
                               MatrixCompareType::relative));
 }
 
+GTEST_TEST(SapFrictionConeConstraint, BiasVelocity) {
+  const int clique = 12;
+  SapConstraintJacobian<double> J(clique, J32);
+  ContactConfiguration<double> configuration = MakeArbitraryConfiguration();
+  configuration.v_b = Vector3d(0.25, -0.5, 0.75);
+  const SapFrictionConeConstraint<double>::Parameters parameters =
+      MakeArbitraryParameters();
+  SapFrictionConeConstraint<double> c(configuration, std::move(J), parameters);
+
+  ASSERT_TRUE(CompareMatrices(c.bias_velocity(), configuration.v_b));
+
+  std::unique_ptr<AbstractValue> abstract_data =
+      c.MakeData(0.01, Vector3d::Constant(3.0));
+  const Vector3d vc(-0.4, 0.5, -0.6);
+  c.CalcData(vc, abstract_data.get());
+
+  const auto& data =
+      abstract_data->get_value<SapFrictionConeConstraintData<double>>();
+  EXPECT_TRUE(CompareMatrices(data.vc(), vc + configuration.v_b));
+}
+
 constexpr double kTolerance = 1.0e-8;
 
 // This method solves the projection in the norm defined by R:
@@ -395,8 +420,11 @@ GTEST_TEST(SapFrictionConeConstraint, RegionIII) {
 GTEST_TEST(SapFrictionConeConstraint, SingleCliqueConstraintClone) {
   const int clique = 12;
   SapConstraintJacobian<double> J(clique, J32);
-  const ContactConfiguration<double> configuration =
-      MakeArbitraryConfiguration();
+  ContactConfiguration<double> configuration = MakeArbitraryConfiguration();
+  // Note: we're using *this* test to make sure the bias velocity survives
+  // cloning. This is omitted from the other cloning tests.
+  const Vector3d velocity_bias(0.25, -0.5, 0.75);
+  configuration.v_b = velocity_bias;
   const SapFrictionConeConstraint<double>::Parameters parameters =
       MakeArbitraryParameters();
   SapFrictionConeConstraint<double> c(configuration, std::move(J), parameters);
@@ -409,9 +437,11 @@ GTEST_TEST(SapFrictionConeConstraint, SingleCliqueConstraintClone) {
   ExpectEqual(c, *clone);
 
   // Test ToDouble.
+  auto configuration_ad = MakeArbitraryConfiguration<AutoDiffXd>();
+  // The matching bias values to confirm inclusion in cloning.
+  configuration_ad.v_b = velocity_bias.cast<AutoDiffXd>();
   SapFrictionConeConstraint<AutoDiffXd> c_ad(
-      MakeArbitraryConfiguration<AutoDiffXd>(),
-      SapConstraintJacobian<AutoDiffXd>(clique, J32),
+      configuration_ad, SapConstraintJacobian<AutoDiffXd>(clique, J32),
       MakeArbitraryParameters<AutoDiffXd>());
   auto clone_from_ad =
       dynamic_pointer_cast<SapFrictionConeConstraint<double>>(c_ad.ToDouble());

multibody/contact_solvers/sap/test/sap_hunt_crossley_constraint_test.cc

@@ -129,6 +129,27 @@ GTEST_TEST(SapHuntCrossleyConstraint, TwoCliquesConstraint) {
   EXPECT_EQ(c.configuration(), configuration);
 }
 
+GTEST_TEST(SapHuntCrossleyConstraint, BiasVelocity) {
+  const int clique = 12;
+  ContactConfiguration<double> configuration = MakeArbitraryConfiguration();
+  configuration.v_b = Vector3d(0.25, -0.5, 0.75);
+  SapConstraintJacobian<double> J(clique, J32);
+  const SapHuntCrossleyConstraint<double>::Parameters parameters =
+      MakeArbitraryParameters();
+  SapHuntCrossleyConstraint<double> c(configuration, std::move(J), parameters);
+
+  ASSERT_TRUE(CompareMatrices(c.bias_velocity(), configuration.v_b));
+
+  std::unique_ptr<AbstractValue> abstract_data =
+      c.MakeData(0.01, Vector3d::Constant(3.0));
+  const Vector3d vc(-0.4, 0.5, -0.6);
+  c.CalcData(vc, abstract_data.get());
+
+  const auto& data =
+      abstract_data->get_value<SapHuntCrossleyConstraintData<double>>();
+  EXPECT_TRUE(CompareMatrices(data.vc, vc + configuration.v_b));
+}
+
 // Unit test the addition of SAP's regularization, controlled by parameter
 // SapHuntCrossleyConstraint::Parameters::sigma.
 GTEST_TEST(SapHuntCrossleyConstraint, SapRegularization) {

multibody/plant/deformable_driver.cc

@@ -571,6 +571,21 @@ void DeformableDriver<T>::AppendDiscreteContactPairs(
       /* The normal (scalar) component of the contact velocity in the contact
        frame. */
       const T v_AcBc_Cz = nhat_AB_W.dot(v_WBc - v_WAc);
+      /* Surface velocity bias in the contact frame. Body A is always
+       deformable, so it carries no surface velocity. For deformable-vs-rigid
+       contact, use the world body as a non-contributing surrogate for
+       deformable A and add rigid body B's contribution. surface.nhats_W()[i]
+       points out of B (from B toward A), matching AddSurfaceVelocityBias()'s
+       normal convention. */
+      Vector3<T> v_b = Vector3<T>::Zero();
+      if (!is_deformable_vs_deformable) {
+        const BodyIndex rigid_body_B_index(body_index_B);
+        Vector3<T> v_b_W = Vector3<T>::Zero();
+        manager_->AddSurfaceVelocityBias(
+            context, manager_->plant().world_body().index(), rigid_body_B_index,
+            surface.nhats_W()[i], &v_b_W);
+        v_b = R_WC.transpose() * v_b_W;
+      }
       DiscreteContactPair<T> contact_pair{
           .jacobian = std::move(jacobian_blocks),
           .id_A = id_A,
@@ -584,6 +599,7 @@ void DeformableDriver<T>::AppendDiscreteContactPairs(
           .nhat_BA_W = nhat_BA_W,
           .phi0 = phi0,
           .vn0 = v_AcBc_Cz,
+          .v_b = v_b,
           .fn0 = fn0,
           .stiffness = k,
           .damping = d,