From b3fd84eb2c139ac4cc649154a706add9e00104c6 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Tue, 28 Jul 2026 08:24:07 -0700 Subject: [PATCH] [multibody] Apply surface velocity in discrete contact - DiscreteUpdateManager gets the core bookkeeping that affects all discrete solers. - We then explicitly implement in SAP and deformables. --- .../contact_solvers/contact_configuration.h | 16 +- .../contact_solvers/sap/sap_constraint.h | 14 ++ .../sap/sap_contact_problem.cc | 7 +- .../sap/sap_friction_cone_constraint.cc | 5 +- .../sap/sap_friction_cone_constraint.h | 2 + .../sap/sap_hunt_crossley_constraint.cc | 12 +- .../sap/sap_hunt_crossley_constraint.h | 2 + multibody/contact_solvers/sap/sap_solver.cc | 9 + .../contact_solvers/sap/sap_solver_results.h | 5 +- .../sap/test/sap_contact_problem_test.cc | 56 +++++- .../test/sap_friction_cone_constraint_test.cc | 40 +++- .../test/sap_hunt_crossley_constraint_test.cc | 21 ++ multibody/plant/deformable_driver.cc | 16 ++ multibody/plant/discrete_contact_pair.h | 4 + multibody/plant/discrete_update_manager.cc | 64 ++++-- multibody/plant/discrete_update_manager.h | 5 + ...y_plant_discrete_update_manager_attorney.h | 10 + multibody/plant/test/surface_velocity_test.cc | 187 +++++++++++++----- 18 files changed, 385 insertions(+), 90 deletions(-) diff --git a/multibody/contact_solvers/contact_configuration.h b/multibody/contact_solvers/contact_configuration.h index 7cc63223e6dd..2a5e81677102 100644 --- a/multibody/contact_solvers/contact_configuration.h +++ b/multibody/contact_solvers/contact_configuration.h @@ -30,7 +30,8 @@ struct ContactConfiguration { .vn = ExtractDoubleOrThrow(vn), .fe = ExtractDoubleOrThrow(fe), .R_WC = - math::RotationMatrix(math::DiscardGradient(R_WC.matrix()))}; + math::RotationMatrix(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 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 v_b{0, 0, 0}; }; // Extracts a ContactConfiguration from the given DiscreteContactPair. @@ -90,7 +101,8 @@ ContactConfiguration 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 diff --git a/multibody/contact_solvers/sap/sap_constraint.h b/multibody/contact_solvers/sap/sap_constraint.h index 84b4e23f531b..36e5bd436eef 100644 --- a/multibody/contact_solvers/sap/sap_constraint.h +++ b/multibody/contact_solvers/sap/sap_constraint.h @@ -291,6 +291,14 @@ class SapConstraint { const math::internal::PartialPermutation& clique_permutation, const std::vector>& 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 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*) const = 0; virtual std::unique_ptr> DoClone() const = 0; virtual std::unique_ptr> 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 DoCalcBiasVelocity() const { + return VectorX::Zero(num_constraint_equations()); + } // @} private: diff --git a/multibody/contact_solvers/sap/sap_contact_problem.cc b/multibody/contact_solvers/sap/sap_contact_problem.cc index ef52b1d0be63..44be7200526c 100644 --- a/multibody/contact_solvers/sap/sap_contact_problem.cc +++ b/multibody/contact_solvers/sap/sap_contact_problem.cc @@ -181,7 +181,9 @@ void SapContactProblem::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& c = get_constraint(i); @@ -199,6 +201,7 @@ void SapContactProblem::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::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); } diff --git a/multibody/contact_solvers/sap/sap_friction_cone_constraint.cc b/multibody/contact_solvers/sap/sap_friction_cone_constraint.cc index ce7a7315ff5f..bc91158fc7ec 100644 --- a/multibody/contact_solvers/sap/sap_friction_cone_constraint.cc +++ b/multibody/contact_solvers/sap/sap_friction_cone_constraint.cc @@ -98,8 +98,9 @@ void SapFrictionConeConstraint::DoCalcData( auto& data = abstract_data->get_mutable_value>(); - 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); diff --git a/multibody/contact_solvers/sap/sap_friction_cone_constraint.h b/multibody/contact_solvers/sap/sap_friction_cone_constraint.h index d86ee8ba4d93..fd180708f9de 100644 --- a/multibody/contact_solvers/sap/sap_friction_cone_constraint.h +++ b/multibody/contact_solvers/sap/sap_friction_cone_constraint.h @@ -279,6 +279,8 @@ class SapFrictionConeConstraint final : public SapConstraint { void ProjectImpulse(const SapFrictionConeConstraintData& data, Vector3* gamma) const; + VectorX DoCalcBiasVelocity() const final { return configuration_.v_b; } + Parameters parameters_; ContactConfiguration configuration_; }; diff --git a/multibody/contact_solvers/sap/sap_hunt_crossley_constraint.cc b/multibody/contact_solvers/sap/sap_hunt_crossley_constraint.cc index 6a7901c00eb9..200b2b5e9cf1 100644 --- a/multibody/contact_solvers/sap/sap_hunt_crossley_constraint.cc +++ b/multibody/contact_solvers/sap/sap_hunt_crossley_constraint.cc @@ -59,7 +59,11 @@ std::unique_ptr SapHuntCrossleyConstraint::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::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) { diff --git a/multibody/contact_solvers/sap/sap_hunt_crossley_constraint.h b/multibody/contact_solvers/sap/sap_hunt_crossley_constraint.h index e3d4c09e28e6..716006823e91 100644 --- a/multibody/contact_solvers/sap/sap_hunt_crossley_constraint.h +++ b/multibody/contact_solvers/sap/sap_hunt_crossley_constraint.h @@ -229,6 +229,8 @@ class SapHuntCrossleyConstraint final : public SapConstraint { // @param vn Normal component of the contact velocity. T CalcDiscreteHuntCrossleyImpulseGradient(const T& dt, const T& vn) const; + VectorX DoCalcBiasVelocity() const final { return configuration_.v_b; } + Parameters parameters_; ContactConfiguration configuration_; }; diff --git a/multibody/contact_solvers/sap/sap_solver.cc b/multibody/contact_solvers/sap/sap_solver.cc index 2e236fe9a1b0..b8ebc988fd70 100644 --- a/multibody/contact_solvers/sap/sap_solver.cc +++ b/multibody/contact_solvers/sap/sap_solver.cc @@ -65,6 +65,15 @@ void SapSolver::PackSapSolverResults(const SapModel& model, // original order described by the model right after. const VectorX& 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& problem = model.problem(); + for (int i = 0; i < problem.num_constraints(); ++i) { + const SapConstraint& 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& gamma_clustered = model.EvalImpulses(context); model.impulses_permutation().ApplyInverse(gamma_clustered, &results->gamma); diff --git a/multibody/contact_solvers/sap/sap_solver_results.h b/multibody/contact_solvers/sap/sap_solver_results.h index 7166ef50b92d..bb7accbdaf37 100644 --- a/multibody/contact_solvers/sap/sap_solver_results.h +++ b/multibody/contact_solvers/sap/sap_solver_results.h @@ -27,8 +27,9 @@ struct SapSolverResults { // Constraints' impulses, of size `num_constraint_equations`. VectorX 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 vc; // Vector of generalized impulses j = Jᵀ⋅γ due to constraints, where J is the diff --git a/multibody/contact_solvers/sap/test/sap_contact_problem_test.cc b/multibody/contact_solvers/sap/test/sap_contact_problem_test.cc index 13af51fd947c..89490672a59e 100644 --- a/multibody/contact_solvers/sap/test/sap_contact_problem_test.cc +++ b/multibody/contact_solvers/sap/test/sap_contact_problem_test.cc @@ -46,21 +46,35 @@ class TestConstraint final : public SapConstraint { 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 bias = {}) : SapConstraint( {clique, MatrixX::Ones(num_constraint_equations, clique_nv)}, - {}) {} + {}), + bias_(std::move(bias)) { + if (bias_.size() == 0) { + bias_ = VectorX::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 bias = {}) : SapConstraint( {first_clique, MatrixX::Ones(num_constraint_equations, first_clique_nv), second_clique, MatrixX::Ones(num_constraint_equations, second_clique_nv)}, - {}) {} + {}), + bias_(std::move(bias)) { + if (bias_.size() == 0) { + bias_ = VectorX::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 { this->num_velocities(0), this->second_clique(), this->num_velocities(1)); } + + VectorX DoCalcBiasVelocity() const final { return bias_; } + + VectorX 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 A{S22}; + const VectorXd v_star = (VectorXd(2) << 1.0, 2.0).finished(); + SapContactProblem problem(time_step, std::move(A), std::move(v_star)); + + const VectorX v_bias = + (VectorX(3) << 0.25, -0.5, 0.75).finished(); + problem.AddConstraint( + std::make_unique>(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> reduced_problem = + problem.MakeReduced({0, 1}, {{0, 1}}, &mapping); + ASSERT_EQ(reduced_problem->num_constraint_equations(), 0); + + SapSolverResults reduced_results; + reduced_results.Resize(0, 0); + + SapSolverResults results; + problem.ExpandContactSolverResults(mapping, reduced_results, &results); + + const VectorX vc_expected = + VectorX::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 A{S22, S33, S44, S22}; diff --git a/multibody/contact_solvers/sap/test/sap_friction_cone_constraint_test.cc b/multibody/contact_solvers/sap/test/sap_friction_cone_constraint_test.cc index a6c5d59baf4f..4b813266cf59 100644 --- a/multibody/contact_solvers/sap/test/sap_friction_cone_constraint_test.cc +++ b/multibody/contact_solvers/sap/test/sap_friction_cone_constraint_test.cc @@ -61,7 +61,11 @@ ContactConfiguration MakeArbitraryConfiguration() { .phi = -2.5e-3, .vn = kInf, .fe = kInf, - .R_WC = RotationMatrix::Identity()}; + .R_WC = RotationMatrix::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(0.0, 0.0, 0.0)}; } template @@ -206,6 +210,27 @@ GTEST_TEST(SapFrictionConeConstraint, CalcRegularization) { MatrixCompareType::relative)); } +GTEST_TEST(SapFrictionConeConstraint, BiasVelocity) { + const int clique = 12; + SapConstraintJacobian J(clique, J32); + ContactConfiguration configuration = MakeArbitraryConfiguration(); + configuration.v_b = Vector3d(0.25, -0.5, 0.75); + const SapFrictionConeConstraint::Parameters parameters = + MakeArbitraryParameters(); + SapFrictionConeConstraint c(configuration, std::move(J), parameters); + + ASSERT_TRUE(CompareMatrices(c.bias_velocity(), configuration.v_b)); + + std::unique_ptr 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>(); + 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 J(clique, J32); - const ContactConfiguration configuration = - MakeArbitraryConfiguration(); + ContactConfiguration 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::Parameters parameters = MakeArbitraryParameters(); SapFrictionConeConstraint c(configuration, std::move(J), parameters); @@ -409,9 +437,11 @@ GTEST_TEST(SapFrictionConeConstraint, SingleCliqueConstraintClone) { ExpectEqual(c, *clone); // Test ToDouble. + auto configuration_ad = MakeArbitraryConfiguration(); + // The matching bias values to confirm inclusion in cloning. + configuration_ad.v_b = velocity_bias.cast(); SapFrictionConeConstraint c_ad( - MakeArbitraryConfiguration(), - SapConstraintJacobian(clique, J32), + configuration_ad, SapConstraintJacobian(clique, J32), MakeArbitraryParameters()); auto clone_from_ad = dynamic_pointer_cast>(c_ad.ToDouble()); diff --git a/multibody/contact_solvers/sap/test/sap_hunt_crossley_constraint_test.cc b/multibody/contact_solvers/sap/test/sap_hunt_crossley_constraint_test.cc index b1c96e4052d0..a80656bb64ea 100644 --- a/multibody/contact_solvers/sap/test/sap_hunt_crossley_constraint_test.cc +++ b/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 configuration = MakeArbitraryConfiguration(); + configuration.v_b = Vector3d(0.25, -0.5, 0.75); + SapConstraintJacobian J(clique, J32); + const SapHuntCrossleyConstraint::Parameters parameters = + MakeArbitraryParameters(); + SapHuntCrossleyConstraint c(configuration, std::move(J), parameters); + + ASSERT_TRUE(CompareMatrices(c.bias_velocity(), configuration.v_b)); + + std::unique_ptr 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>(); + 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) { diff --git a/multibody/plant/deformable_driver.cc b/multibody/plant/deformable_driver.cc index 702ded8a7b67..c75864b851a7 100644 --- a/multibody/plant/deformable_driver.cc +++ b/multibody/plant/deformable_driver.cc @@ -571,6 +571,21 @@ void DeformableDriver::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 v_b = Vector3::Zero(); + if (!is_deformable_vs_deformable) { + const BodyIndex rigid_body_B_index(body_index_B); + Vector3 v_b_W = Vector3::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 contact_pair{ .jacobian = std::move(jacobian_blocks), .id_A = id_A, @@ -584,6 +599,7 @@ void DeformableDriver::AppendDiscreteContactPairs( .nhat_BA_W = nhat_BA_W, .phi0 = phi0, .vn0 = v_AcBc_Cz, + .v_b = v_b, .fn0 = fn0, .stiffness = k, .damping = d, diff --git a/multibody/plant/discrete_contact_pair.h b/multibody/plant/discrete_contact_pair.h index a525a5569f18..25b0e3c7bc26 100644 --- a/multibody/plant/discrete_contact_pair.h +++ b/multibody/plant/discrete_contact_pair.h @@ -92,6 +92,10 @@ struct DiscreteContactPair { /* Normal velocity, defined as the rate of change of phi. Therefore vn0 > 0 implies bodies are moving away from each other. */ T vn0{0.0}; + /* Velocity bias term arising from (pseudo)motion unrelated to body + configuration (e.g., modeling conveyor belt using "surface velocity"). + Expressed in the contact frame. */ + Vector3 v_b; /* The (undamped) normal contact force at the current configuration before a discrete update is made. With "undamped" we mean this force only contains the compliant component of the model, without the Hunt & Crossley term. */ diff --git a/multibody/plant/discrete_update_manager.cc b/multibody/plant/discrete_update_manager.cc index 47077249a52d..95d9d016c114 100644 --- a/multibody/plant/discrete_update_manager.cc +++ b/multibody/plant/discrete_update_manager.cc @@ -57,6 +57,8 @@ template void DiscreteUpdateManager::DeclareCacheEntries() { const auto& query_object_input_ticket = plant().get_geometry_query_input_port().ticket(); + const auto& surface_speeds_input_ticket = + plant().get_surface_speeds_input_port().ticket(); const auto& contact_solver_results_cache_entry = DeclareCacheEntry( "Contact solver results", systems::ValueProducer( @@ -109,7 +111,8 @@ void DiscreteUpdateManager::DeclareCacheEntries() { systems::ValueProducer( this, &DiscreteUpdateManager::CalcDiscreteContactPairs), {systems::System::xd_ticket(), - systems::System::all_parameters_ticket(), query_object_input_ticket}); + systems::System::all_parameters_ticket(), query_object_input_ticket, + surface_speeds_input_ticket}); cache_indexes_.discrete_contact_pairs = discrete_contact_pairs_cache_entry.cache_index(); @@ -130,6 +133,15 @@ const GeometryContactData& DiscreteUpdateManager::EvalGeometryContactData( T>::EvalGeometryContactData(plant(), context); } +template +void DiscreteUpdateManager::AddSurfaceVelocityBias( + const systems::Context& context, BodyIndex bodyA_index, + BodyIndex bodyB_index, const Vector3& nhat_BA_W, + Vector3* v_AcBc_W) const { + MultibodyPlantDiscreteUpdateManagerAttorney::AddSurfaceVelocityBias( + plant(), context, bodyA_index, bodyB_index, nhat_BA_W, v_AcBc_W); +} + template systems::CacheEntry& DiscreteUpdateManager::DeclareCacheEntry( std::string description, systems::ValueProducer value_producer, @@ -698,6 +710,17 @@ void DiscreteUpdateManager::AppendDiscreteContactPairsForPointContact( math::RotationMatrix R_WC = math::RotationMatrix::MakeFromOneVector(nhat_AB_W, 2); + const RigidTransform& X_WA = + plant().EvalBodyPoseInWorld(context, body_A); + const RigidTransform& X_WB = + plant().EvalBodyPoseInWorld(context, body_B); + + // Relative separation velocity due to surface velocity in contact frame C. + Vector3 v_AcBc_W_ss = Vector3::Zero(); + AddSurfaceVelocityBias(context, body_A.index(), body_B.index(), + pair.nhat_BA_W, &v_AcBc_W_ss); + const Vector3 v_AcBc_C_ss = R_WC.transpose() * v_AcBc_W_ss; + // Contact velocity stored in the current context (previous time step). const Vector3 v_AcBc_W = Jv_AcBc_W * v; const Vector3 v_AcBc_C = R_WC.transpose() * v_AcBc_W; @@ -748,12 +771,8 @@ void DiscreteUpdateManager::AppendDiscreteContactPairsForPointContact( const T fn0 = k * pair.depth; // Contact point position relative to each body. - const RigidTransform& X_WA = - plant().EvalBodyPoseInWorld(context, body_A); const Vector3& p_WA = X_WA.translation(); const Vector3 p_AC_W = p_WC - p_WA; - const RigidTransform& X_WB = - plant().EvalBodyPoseInWorld(context, body_B); const Vector3& p_WB = X_WB.translation(); const Vector3 p_BC_W = p_WC - p_WB; @@ -769,6 +788,7 @@ void DiscreteUpdateManager::AppendDiscreteContactPairsForPointContact( .nhat_BA_W = pair.nhat_BA_W, .phi0 = phi0, .vn0 = vn0, + .v_b = v_AcBc_C_ss, .fn0 = fn0, .stiffness = k, .damping = d, @@ -826,9 +846,11 @@ void DiscreteUpdateManager::AppendDiscreteContactPairsForHydroelasticContact( // We always call the body associated with geometry M, A, and the body // associated with geometry N, B. - const BodyIndex body_A_index = FindBodyByGeometryId(s.id_M()); + const GeometryId id_M = s.id_M(); + const GeometryId id_N = s.id_N(); + const BodyIndex body_A_index = FindBodyByGeometryId(id_M); const RigidBody& body_A = plant().get_body(body_A_index); - const BodyIndex body_B_index = FindBodyByGeometryId(s.id_N()); + const BodyIndex body_B_index = FindBodyByGeometryId(id_N); const RigidBody& body_B = plant().get_body(body_B_index); const TreeIndex& tree_A_index = forest.link_to_tree_index(body_A_index); @@ -844,15 +866,15 @@ void DiscreteUpdateManager::AppendDiscreteContactPairsForHydroelasticContact( // TODO(amcastro-tri): Consider making the modulus required, instead of // a default infinite value. const T hydro_modulus_M = GetHydroelasticModulus( - s.id_M(), std::numeric_limits::infinity(), inspector); + id_M, std::numeric_limits::infinity(), inspector); const T hydro_modulus_N = GetHydroelasticModulus( - s.id_N(), std::numeric_limits::infinity(), inspector); + id_N, std::numeric_limits::infinity(), inspector); // Hunt & Crossley dissipation. Used by the Tamsi, Lagged, and Similar // contact models. Ignored by Sap. See // multibody::DiscreteContactApproximation for details about these contact // models. const T d = GetCombinedHuntCrossleyDissipation( - s.id_M(), s.id_N(), hydro_modulus_M, hydro_modulus_N, + id_M, id_N, hydro_modulus_M, hydro_modulus_N, default_contact_dissipation(), inspector); // Dissipation time scale. Used by Sap contact model. Ignored by Tamsi, // Lagged, and Similar contact model. See @@ -860,11 +882,15 @@ void DiscreteUpdateManager::AppendDiscreteContactPairsForHydroelasticContact( // models. const double default_dissipation_time_constant = 0.1; const T tau = GetCombinedDissipationTimeConstant( - s.id_M(), s.id_N(), default_dissipation_time_constant, body_A.name(), + id_M, id_N, default_dissipation_time_constant, body_A.name(), body_B.name(), inspector); // Combine friction coefficients. - const T mu = - GetCombinedDynamicCoulombFriction(s.id_M(), s.id_N(), inspector); + const T mu = GetCombinedDynamicCoulombFriction(id_M, id_N, inspector); + + const RigidTransform& X_WA = + plant().EvalBodyPoseInWorld(context, body_A); + const RigidTransform& X_WB = + plant().EvalBodyPoseInWorld(context, body_B); for (int face = 0; face < s.num_faces(); ++face) { const T& Ae = s.area(face); // Face element area. @@ -937,6 +963,13 @@ void DiscreteUpdateManager::AppendDiscreteContactPairsForHydroelasticContact( math::RotationMatrix R_WC = math::RotationMatrix::MakeFromOneVector(nhat_AB_W, 2); + // Relative separation velocity due to surface velocity in contact frame + // C. + Vector3 v_AcBc_W_ss = Vector3::Zero(); + AddSurfaceVelocityBias(context, body_A_index, body_B_index, nhat_BA_W, + &v_AcBc_W_ss); + const Vector3 v_AcBc_C_ss = R_WC.transpose() * v_AcBc_W_ss; + // Contact velocity stored in the current context (previous time // step). const Vector3 v_AcBc_W = Jv_AcBc_W * v; @@ -995,12 +1028,8 @@ void DiscreteUpdateManager::AppendDiscreteContactPairsForHydroelasticContact( const T phi0 = -p0 / g; // Contact point position relative to each body. - const RigidTransform& X_WA = - plant().EvalBodyPoseInWorld(context, body_A); const Vector3& p_WA = X_WA.translation(); const Vector3 p_AC_W = p_WC - p_WA; - const RigidTransform& X_WB = - plant().EvalBodyPoseInWorld(context, body_B); const Vector3& p_WB = X_WB.translation(); const Vector3 p_BC_W = p_WC - p_WB; @@ -1017,6 +1046,7 @@ void DiscreteUpdateManager::AppendDiscreteContactPairsForHydroelasticContact( .nhat_BA_W = nhat_BA_W, .phi0 = phi0, .vn0 = vn0, + .v_b = v_AcBc_C_ss, .fn0 = fn0, .stiffness = k, .damping = d, diff --git a/multibody/plant/discrete_update_manager.h b/multibody/plant/discrete_update_manager.h index afb40eee1702..0dda7f2f86eb 100644 --- a/multibody/plant/discrete_update_manager.h +++ b/multibody/plant/discrete_update_manager.h @@ -237,6 +237,11 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent { const GeometryContactData& EvalGeometryContactData( const systems::Context& context) const; + void AddSurfaceVelocityBias(const systems::Context& context, + BodyIndex bodyA_index, BodyIndex bodyB_index, + const Vector3& nhat_BA_W, + Vector3* v_AcBc_W) const; + double default_contact_stiffness() const; double default_contact_dissipation() const; diff --git a/multibody/plant/multibody_plant_discrete_update_manager_attorney.h b/multibody/plant/multibody_plant_discrete_update_manager_attorney.h index 29783c35a24d..aa548f08586a 100644 --- a/multibody/plant/multibody_plant_discrete_update_manager_attorney.h +++ b/multibody/plant/multibody_plant_discrete_update_manager_attorney.h @@ -48,6 +48,16 @@ class MultibodyPlantDiscreteUpdateManagerAttorney { return plant.EvalGeometryContactData(context); } + static void AddSurfaceVelocityBias(const MultibodyPlant& plant, + const systems::Context& context, + BodyIndex bodyA_index, + BodyIndex bodyB_index, + const Vector3& nhat_BA_W, + Vector3* v_AcBc_W) { + plant.AddSurfaceVelocityBias(context, bodyA_index, bodyB_index, nhat_BA_W, + v_AcBc_W); + } + static void AddJointLimitsPenaltyForces(const MultibodyPlant& plant, const systems::Context& context, MultibodyForces* forces) { diff --git a/multibody/plant/test/surface_velocity_test.cc b/multibody/plant/test/surface_velocity_test.cc index 659d2099421d..98ede47af1d6 100644 --- a/multibody/plant/test/surface_velocity_test.cc +++ b/multibody/plant/test/surface_velocity_test.cc @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -90,7 +91,9 @@ using geometry::AddContactMaterial; using geometry::AddRigidHydroelasticProperties; using geometry::Box; using geometry::GeometryId; +using geometry::GeometryInstance; using geometry::ProximityProperties; +using geometry::Sphere; using math::RigidTransformd; using math::RollPitchYawd; @@ -586,10 +589,14 @@ TEST_F(SurfaceDisplacementTest, ContinuousIntegration) { // // We'll test for a force in that direction and verify that a force applied // in that direction leads to motion in the same direction. +// +// The suite verifies these invariants across a number of solver variants +// including continuous, various discrete solvers, and deformable. struct OrthogonalContactTestConfig { std::string description; MultibodyPlantConfig plant_config; + bool use_deformable{false}; }; // Formatter for OrthogonalContactTestConfig so that if a test fails, we get @@ -599,7 +606,8 @@ void PrintTo(const OrthogonalContactTestConfig& config, std::ostream* os) { *os << "{ time_step: " << plant_config.time_step << ", contact_model: " << plant_config.contact_model << ", discrete_contact_approximation: " - << plant_config.discrete_contact_approximation << " }"; + << plant_config.discrete_contact_approximation + << ", use_deformable: " << config.use_deformable << " }"; } class OrthogonalSurfaceVelocityTest @@ -643,17 +651,30 @@ class OrthogonalSurfaceVelocityTest Box(10.0, 10.0, 1.0), "ground", rigid); plant_->SetSurfaceVelocityAxis(*ground_, Vector3d(1, 0, 0)); - // Box: free floating, 0.2 m cube. - // axis_ss_B = (0,-1,0) → world surface velocity = kBoxSpeed * Wy. - // (Box is body B; contact pairs pass −Ẑ to B, so the cross product - // flips sign.) - box_ = &plant_->AddRigidBody( - "box", SpatialInertia::SolidBoxWithMass( - 1.0, 2 * kHalfSize, 2 * kHalfSize, 2 * kHalfSize)); - plant_->RegisterCollisionGeometry( - *box_, RigidTransformd::Identity(), - Box(2 * kHalfSize, 2 * kHalfSize, 2 * kHalfSize), "box", compliant); - plant_->SetSurfaceVelocityAxis(*box_, Vector3d(0, -1, 0)); + if (!GetParam().use_deformable) { + // Box: free floating, 0.2 m cube. + // axis_ss_B = (0,-1,0) → world surface velocity = kBoxSpeed * (+Y). + // (Box is body B; contact pairs pass −Ẑ to B, so the cross product + // flips.) + box_ = &plant_->AddRigidBody( + "box", SpatialInertia::SolidBoxWithMass( + 1.0, 2 * kHalfSize, 2 * kHalfSize, 2 * kHalfSize)); + plant_->RegisterCollisionGeometry( + *box_, RigidTransformd::Identity(), + Box(2 * kHalfSize, 2 * kHalfSize, 2 * kHalfSize), "box", compliant); + plant_->SetSurfaceVelocityAxis(*box_, Vector3d(0, -1, 0)); + } else { + // Deformable sphere: no surface velocity; contacts the ground belt only. + auto sphere_instance = std::make_unique( + RigidTransformd(Vector3d(0, 0, kHalfSize - kPenetration)), + std::make_unique(kHalfSize), "deformable_sphere"); + ProximityProperties deformable_props(material); + sphere_instance->set_proximity_properties(std::move(deformable_props)); + fem::DeformableBodyConfig body_config; + deformable_id_ = + plant_->mutable_deformable_model().RegisterDeformableBody( + std::move(sphere_instance), body_config, kHalfSize); + } plant_->Finalize(); auto diagram = builder.Build(); @@ -662,18 +683,23 @@ class OrthogonalSurfaceVelocityTest plant_->GetMyMutableContextFromRoot(&sim_->get_mutable_context()); // Place box rotated kTheta around Z with bottom face at z = -kPenetration. - plant_->SetFloatingBaseBodyPoseInWorldFrame( - &plant_context, *box_, - RigidTransformd(Rz_90, Vector3d(0, 0, kHalfSize - kPenetration))); + if (!GetParam().use_deformable) { + plant_->SetFloatingBaseBodyPoseInWorldFrame( + &plant_context, *box_, + RigidTransformd(Rz_90, Vector3d(0, 0, kHalfSize - kPenetration))); + } systems::BusValue bus; bus.Set(ground_->scoped_name().to_string(), Value(kGroundSpeed)); - bus.Set(box_->scoped_name().to_string(), Value(kBoxSpeed)); + if (!GetParam().use_deformable) { + bus.Set(box_->scoped_name().to_string(), Value(kBoxSpeed)); + } plant_->get_surface_speeds_input_port().FixValue(&plant_context, bus); sim_->Initialize(); } - // Returns the total contact force on the contacting body from ContactResults. + // Returns the total contact force on the contacting body (rigid box or + // deformable sphere) from ContactResults. Vector3d ContactForceOnContactingBody( const systems::Context& plant_context) const { const auto& results = @@ -690,33 +716,41 @@ class OrthogonalSurfaceVelocityTest int active_forces = 0; Vector3d f_Box = Vector3d::Zero(); - // Point contacts: contact_force() is the force on body B. - for (int i = 0; i < results.num_point_pair_contacts(); ++i) { - const auto& info = results.point_pair_contact_info(i); - if (info.bodyB_index() == box_->index()) { - f_Box += info.contact_force(); - ++active_forces; - } else if (info.bodyA_index() == box_->index()) { - f_Box -= info.contact_force(); - ++active_forces; + if (box_ != nullptr) { + // Point contacts: contact_force() is the force on body B. + for (int i = 0; i < results.num_point_pair_contacts(); ++i) { + const auto& info = results.point_pair_contact_info(i); + if (info.bodyB_index() == box_->index()) { + f_Box += info.contact_force(); + ++active_forces; + } else if (info.bodyA_index() == box_->index()) { + f_Box -= info.contact_force(); + ++active_forces; + } } - } - // Hydroelastic contacts: F_Ac_W() is the force on body A (id_M geometry). - // If the box is body A in the contact results, we apply the force, - // otherwise, we reverse the force. - const auto& box_geom_ids = plant_->GetCollisionGeometriesForBody(*box_); - for (int i = 0; i < results.num_hydroelastic_contacts(); ++i) { - const auto& hydro_info = results.hydroelastic_contact_info(i); - const GeometryId id_M = hydro_info.contact_surface().id_M(); - const bool box_is_body_A = - std::find(box_geom_ids.begin(), box_geom_ids.end(), id_M) != - box_geom_ids.end(); - if (box_is_body_A) { - f_Box += hydro_info.F_Ac_W().translational(); - ++active_forces; - } else { - f_Box -= hydro_info.F_Ac_W().translational(); + // Hydroelastic contacts: F_Ac_W() is the force on body A (id_M geometry). + // If the box is body A in the contact results, we apply the force, + // otherwise, we reverse the force. + const auto& box_geom_ids = plant_->GetCollisionGeometriesForBody(*box_); + for (int i = 0; i < results.num_hydroelastic_contacts(); ++i) { + const auto& hydro_info = results.hydroelastic_contact_info(i); + const GeometryId id_m = hydro_info.contact_surface().id_M(); + const bool box_is_body_m = + std::find(box_geom_ids.begin(), box_geom_ids.end(), id_m) != + box_geom_ids.end(); + if (box_is_body_m) { + f_Box += hydro_info.F_Ac_W().translational(); + ++active_forces; + } else { + f_Box -= hydro_info.F_Ac_W().translational(); + ++active_forces; + } + } + } else { + // Deformable contacts: F_Ac_W() is the force on the deformable body A. + for (int i = 0; i < results.num_deformable_contacts(); ++i) { + f_Box += results.deformable_contact_info(i).F_Ac_W().translational(); ++active_forces; } } @@ -726,15 +760,22 @@ class OrthogonalSurfaceVelocityTest return f_Box; } - // Returns the position of the box. - Vector3d BoxPositionInWorld( + // Returns the position of the "free" body -- the box or deformable sphere. + Vector3d FreeBodyPositionInWorld( const systems::Context& plant_context) const { - return plant_->EvalBodyPoseInWorld(plant_context, *box_).translation(); + if (box_ != nullptr) { + return plant_->EvalBodyPoseInWorld(plant_context, *box_).translation(); + } + return plant_->deformable_model() + .GetPositions(plant_context, deformable_id_.value()) + .rowwise() + .mean(); } MultibodyPlant* plant_{nullptr}; const RigidBody* ground_{nullptr}; const RigidBody* box_{nullptr}; + std::optional deformable_id_; std::unique_ptr> sim_; }; @@ -744,14 +785,22 @@ class OrthogonalSurfaceVelocityTest // surface. With kGroundSpeed == kBoxSpeed the two components are equal in // magnitude. TEST_P(OrthogonalSurfaceVelocityTest, ContactForceTangentialDirection) { + const double time_step = GetParam().plant_config.time_step; + // Discrete: advance one step to populate DiscreteStepMemory. + // Continuous: contact results are available on demand at t = 0. + if (time_step > 0.0) sim_->AdvanceTo(time_step); const Vector3d f = ContactForceOnContactingBody( plant_->GetMyContextFromRoot(sim_->get_context())); EXPECT_GT(f.z(), 0.0); // Normal force pushes in +Wz. - EXPECT_GT(f.x(), 0.0); // Conveyor friction pushes in +Wx. - EXPECT_LT(f.y(), 0.0); // Conveyor friction pushes in -Wy. - // Equal speeds → equal-magnitude tangential components, within 10%. - EXPECT_NEAR(f.x(), -f.y(), 0.1 * f.x()); + EXPECT_GT(f.x(), 0.0); // Friction from conveyor velocity pushes in +Wx. + if (!GetParam().use_deformable) { + // Note: the deformable sphere has no surface velocity. So, we skip the + // tests that depend on the free body having surface velocity. + EXPECT_LT(f.y(), 0.0); // Friction from box velocity pushes in -Wy. + // Equal speeds → equal-magnitude tangential components, within 10%. + EXPECT_NEAR(f.x(), -f.y(), 0.1 * f.x()); + } } // Confirms the effect of the surface velocity on continuous dynamics. @@ -762,13 +811,15 @@ TEST_P(OrthogonalSurfaceVelocityTest, BoxDisplacementDirection) { // Continuous simulation is *very* slow; don't advance too far. sim_->AdvanceTo(0.1); const auto& final_context = plant_->GetMyContextFromRoot(sim_->get_context()); - const Vector3d p_WBody = BoxPositionInWorld(final_context); + const Vector3d p_WBody = FreeBodyPositionInWorld(final_context); constexpr double kMinDisplacement = 0.01; // 1 cm EXPECT_GT(p_WBody.x(), kMinDisplacement) - << "body should have moved in +Wx due to ground belt"; - EXPECT_LT(p_WBody.y(), -kMinDisplacement) - << "box should have moved in -Wy due to box surface velocity"; + << "body should have moved in +Wx due to ground surface velocity"; + if (!GetParam().use_deformable) { + EXPECT_LT(p_WBody.y(), -kMinDisplacement) + << "box should have moved in -Wy due to box surface velocity"; + } } INSTANTIATE_TEST_SUITE_P( @@ -781,6 +832,30 @@ INSTANTIATE_TEST_SUITE_P( continuous_hydro.time_step = 0.0; continuous_hydro.contact_model = "hydroelastic_with_fallback"; + // Note: we don't bother with "similar" or "lagged" because the SAP + // constraints are shared among them. + MultibodyPlantConfig discrete_point_sap; + discrete_point_sap.time_step = 1e-3; + discrete_point_sap.contact_model = "point"; + discrete_point_sap.discrete_contact_approximation = "sap"; + + MultibodyPlantConfig discrete_hydro_sap; + discrete_hydro_sap.time_step = 1e-3; + discrete_hydro_sap.contact_model = "hydroelastic_with_fallback"; + discrete_hydro_sap.discrete_contact_approximation = "sap"; + + // Note: we don't bother with "similar" because "lagged" provides the + // coverage on Sap's Hunt-Crossley constraint. + MultibodyPlantConfig discrete_point_lagged; + discrete_point_lagged.time_step = 1e-3; + discrete_point_lagged.contact_model = "point"; + discrete_point_lagged.discrete_contact_approximation = "lagged"; + + MultibodyPlantConfig discrete_hydro_lagged; + discrete_hydro_lagged.time_step = 1e-3; + discrete_hydro_lagged.contact_model = "hydroelastic_with_fallback"; + discrete_hydro_lagged.discrete_contact_approximation = "lagged"; + return std::vector{ // Keep continuous_point and continuous_hydro next to each other in // this list so that the sharding will make sure they end up in @@ -788,6 +863,12 @@ INSTANTIATE_TEST_SUITE_P( // terrifyingly slow. {"continuous_point", continuous_point}, {"continuous_hydro", continuous_hydro}, + {"discrete_point_sap", discrete_point_sap}, + {"discrete_hydro_sap", discrete_hydro_sap}, + {"discrete_deformable_sap", discrete_hydro_sap, + /* use_deformable = */ true}, + {"discrete_point_lagged", discrete_point_lagged}, + {"discrete_hydro_lagged", discrete_hydro_lagged}, }; }()), [](const testing::TestParamInfo& param_info) {