diff --git a/multibody/plant/compliant_contact_manager.cc b/multibody/plant/compliant_contact_manager.cc index 9bd2259c1d72..edd9e2696942 100644 --- a/multibody/plant/compliant_contact_manager.cc +++ b/multibody/plant/compliant_contact_manager.cc @@ -13,7 +13,6 @@ #include "drake/geometry/geometry_ids.h" #include "drake/geometry/proximity_properties.h" #include "drake/geometry/query_results/penetration_as_point_pair.h" -#include "drake/multibody/plant/contact_properties.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/plant/sap_driver.h" #include "drake/multibody/plant/tamsi_driver.h" @@ -88,16 +87,6 @@ void CompliantContactManager::DoDeclareCacheEntries() { // Therefore if we make it dependent on q_ticket() the Jacobian only // gets evaluated once at the start of the simulation. - // Cache discrete contact pairs. - const auto& discrete_contact_pairs_cache_entry = this->DeclareCacheEntry( - "Discrete contact pairs.", - systems::ValueProducer( - this, &CompliantContactManager::CalcDiscreteContactPairs), - {systems::System::xd_ticket(), - systems::System::all_parameters_ticket()}); - cache_indexes_.discrete_contact_pairs = - discrete_contact_pairs_cache_entry.cache_index(); - // Cache hydroelastic contact info. const auto& hydroelastic_contact_info_cache_entry = this->DeclareCacheEntry( "Hydroelastic contact info.", @@ -110,16 +99,6 @@ void CompliantContactManager::DoDeclareCacheEntries() { cache_indexes_.hydroelastic_contact_info = hydroelastic_contact_info_cache_entry.cache_index(); - // Cache contact kinematics. - const auto& contact_kinematics_cache_entry = this->DeclareCacheEntry( - "Contact kinematics.", - systems::ValueProducer(this, - &CompliantContactManager::CalcContactKinematics), - {systems::System::xd_ticket(), - systems::System::all_parameters_ticket()}); - cache_indexes_.contact_kinematics = - contact_kinematics_cache_entry.cache_index(); - // Accelerations due to non-constraint forces. // We cache non-contact forces, ABA forces and accelerations into an // AccelerationsDueNonConstraintForcesCache. @@ -141,490 +120,12 @@ void CompliantContactManager::DoDeclareCacheEntries() { cache_indexes_.non_constraint_forces_accelerations = non_constraint_forces_accelerations_cache_entry.cache_index(); - if constexpr (std::is_same_v) { - if (deformable_driver_ != nullptr) { - deformable_driver_->DeclareCacheEntries(this); - } - } - // Discrete updates with SAP are not supported when T = symbolic::Expression. if constexpr (!std::is_same_v) { if (sap_driver_ != nullptr) sap_driver_->DeclareCacheEntries(this); } } -template -DiscreteContactData> -CompliantContactManager::CalcContactKinematics( - const systems::Context& context) const { - const DiscreteContactData>& contact_pairs = - this->EvalDiscreteContactPairs(context); - DiscreteContactData> contact_kinematics; - const int num_contacts = contact_pairs.size(); - - // Quick no-op exit. - if (num_contacts == 0) return contact_kinematics; - - contact_kinematics.Reserve(contact_pairs.num_point_contacts(), - contact_pairs.num_hydro_contacts(), - contact_pairs.num_deformable_contacts()); - AppendContactKinematics(context, contact_pairs.point_contact_data(), - DiscreteContactType::kPoint, &contact_kinematics); - AppendContactKinematics(context, contact_pairs.hydro_contact_data(), - DiscreteContactType::kHydroelastic, - &contact_kinematics); - if constexpr (std::is_same_v) { - if (deformable_driver_ != nullptr) { - deformable_driver_->AppendContactKinematics(context, &contact_kinematics); - } - } - return contact_kinematics; -} - -template -void CompliantContactManager::AppendContactKinematics( - const systems::Context& context, - const std::vector>& contact_pairs, - DiscreteContactType type, - DiscreteContactData>* contact_kinematics) const { - // Scratch workspace variables. - const int nv = plant().num_velocities(); - Matrix3X Jv_WAc_W(3, nv); - Matrix3X Jv_WBc_W(3, nv); - Matrix3X Jv_AcBc_W(3, nv); - - const Frame& frame_W = plant().world_frame(); - for (int icontact = 0; icontact < ssize(contact_pairs); ++icontact) { - const auto& point_pair = contact_pairs[icontact]; - - const GeometryId geometryA_id = point_pair.id_A; - const GeometryId geometryB_id = point_pair.id_B; - - BodyIndex bodyA_index = this->geometry_id_to_body_index().at(geometryA_id); - const Body& bodyA = plant().get_body(bodyA_index); - BodyIndex bodyB_index = this->geometry_id_to_body_index().at(geometryB_id); - const Body& bodyB = plant().get_body(bodyB_index); - - // Contact normal from point A into B. - const Vector3& nhat_W = -point_pair.nhat_BA_W; - const Vector3& p_WC = point_pair.p_WC; - - // Contact point position relative to each body. - const RigidTransform& X_WA = plant().EvalBodyPoseInWorld(context, bodyA); - const Vector3& p_WA = X_WA.translation(); - const Vector3 p_AC_W = p_WC - p_WA; - const RigidTransform& X_WB = plant().EvalBodyPoseInWorld(context, bodyB); - const Vector3& p_WB = X_WB.translation(); - const Vector3 p_BC_W = p_WC - p_WB; - - // Since v_AcBc_W = v_WBc - v_WAc the relative velocity Jacobian will be: - // J_AcBc_W = Jv_WBc_W - Jv_WAc_W. - // That is the relative velocity at C is v_AcBc_W = J_AcBc_W * v. - this->internal_tree().CalcJacobianTranslationalVelocity( - context, JacobianWrtVariable::kV, bodyA.body_frame(), frame_W, p_WC, - frame_W, frame_W, &Jv_WAc_W); - this->internal_tree().CalcJacobianTranslationalVelocity( - context, JacobianWrtVariable::kV, bodyB.body_frame(), frame_W, p_WC, - frame_W, frame_W, &Jv_WBc_W); - Jv_AcBc_W = Jv_WBc_W - Jv_WAc_W; - - // Define a contact frame C at the contact point such that the z-axis Cz - // equals nhat_W. The tangent vectors are arbitrary, with the only - // requirement being that they form a valid right handed basis with nhat_W. - math::RotationMatrix R_WC = - math::RotationMatrix::MakeFromOneVector(nhat_W, 2); - - const TreeIndex& treeA_index = - tree_topology().body_to_tree_index(bodyA_index); - const TreeIndex& treeB_index = - tree_topology().body_to_tree_index(bodyB_index); - // Sanity check, at least one must be valid. - DRAKE_DEMAND(treeA_index.is_valid() || treeB_index.is_valid()); - - // We have at most two blocks per contact. - std::vector::JacobianTreeBlock> - jacobian_blocks; - jacobian_blocks.reserve(2); - - // Tree A contribution to contact Jacobian Jv_W_AcBc_C. - if (treeA_index.is_valid()) { - Matrix3X J = R_WC.matrix().transpose() * - Jv_AcBc_W.middleCols( - tree_topology().tree_velocities_start(treeA_index), - tree_topology().num_tree_velocities(treeA_index)); - jacobian_blocks.emplace_back(treeA_index, MatrixBlock(std::move(J))); - } - - // Tree B contribution to contact Jacobian Jv_W_AcBc_C. - // This contribution must be added only if B is different from A. - if ((treeB_index.is_valid() && !treeA_index.is_valid()) || - (treeB_index.is_valid() && treeB_index != treeA_index)) { - Matrix3X J = R_WC.matrix().transpose() * - Jv_AcBc_W.middleCols( - tree_topology().tree_velocities_start(treeB_index), - tree_topology().num_tree_velocities(treeB_index)); - jacobian_blocks.emplace_back(treeB_index, MatrixBlock(std::move(J))); - } - - ContactConfiguration configuration{.objectA = bodyA_index, - .p_ApC_W = p_AC_W, - .objectB = bodyB_index, - .p_BqC_W = p_BC_W, - .phi = point_pair.phi0, - .R_WC = R_WC}; - switch (type) { - case DiscreteContactType::kPoint: { - contact_kinematics->AppendPointData(ContactPairKinematics{ - std::move(jacobian_blocks), std::move(configuration)}); - break; - } - case DiscreteContactType::kHydroelastic: { - contact_kinematics->AppendHydroData(ContactPairKinematics{ - std::move(jacobian_blocks), std::move(configuration)}); - break; - } - case DiscreteContactType::kDeformable: { - throw std::logic_error( - "Call DeformableDriver::AppendContactKinematics() to compute " - "contact kinematics for deformable contact instead."); - } - } - } -} - -template -const DiscreteContactData>& -CompliantContactManager::EvalContactKinematics( - const systems::Context& context) const { - return plant() - .get_cache_entry(cache_indexes_.contact_kinematics) - .template Eval>>(context); -} - -template <> -void CompliantContactManager::CalcDiscreteContactPairs( - const drake::systems::Context&, - DiscreteContactData>*) const { - // Currently, the computation of contact pairs is not supported when T = - // symbolic::Expression. - throw std::domain_error( - fmt::format("This method doesn't support T = {}.", - NiceTypeName::Get())); -} - -template -void CompliantContactManager::CalcDiscreteContactPairs( - const systems::Context& context, - DiscreteContactData>* contact_pairs) const { - plant().ValidateContext(context); - DRAKE_DEMAND(contact_pairs != nullptr); - - contact_pairs->Clear(); - if (plant().num_collision_geometries() == 0) return; - - const auto contact_model = plant().get_contact_model(); - - // We first compute the number of contact pairs so that we can allocate all - // memory at once. - // N.B. num_point_pairs = 0 when: - // 1. There are legitimately no point pairs or, - // 2. the point pair model is not even in use. - // We guard for case (2) since EvalPointPairPenetrations() cannot be called - // when point contact is not used and would otherwise throw an exception. - int num_point_pairs = 0; // The number of point contact pairs. - if (contact_model == ContactModel::kPoint || - contact_model == ContactModel::kHydroelasticWithFallback) { - num_point_pairs = plant().EvalPointPairPenetrations(context).size(); - } - - int num_quadrature_pairs = 0; - // N.B. For discrete hydro we use a first order quadrature rule. As such, - // the per-face quadrature point is the face's centroid and the weight is 1. - // This is compatible with a mesh that is triangle or polygon. If we attempted - // higher order quadrature, polygons would have to be decomposed into smaller - // n-gons which can receive an appropriate set of quadrature points. - if (contact_model == ContactModel::kHydroelastic || - contact_model == ContactModel::kHydroelasticWithFallback) { - const std::vector>& surfaces = - this->EvalContactSurfaces(context); - for (const auto& s : surfaces) { - // One quadrature point per face. - num_quadrature_pairs += s.num_faces(); - } - } - contact_pairs->Reserve(num_point_pairs, num_quadrature_pairs, 0); - - if (contact_model == ContactModel::kPoint || - contact_model == ContactModel::kHydroelasticWithFallback) { - AppendDiscreteContactPairsForPointContact(context, contact_pairs); - } - if (contact_model == ContactModel::kHydroelastic || - contact_model == ContactModel::kHydroelasticWithFallback) { - AppendDiscreteContactPairsForHydroelasticContact(context, contact_pairs); - } - if constexpr (std::is_same_v) { - if (deformable_driver_ != nullptr) { - deformable_driver_->AppendDiscreteContactPairs(context, contact_pairs); - } - } -} - -template -void CompliantContactManager::AppendDiscreteContactPairsForPointContact( - const systems::Context& context, - DiscreteContactData>* result) const { - DiscreteContactData>& contact_pairs = *result; - - const geometry::QueryObject& query_object = - this->plant() - .get_geometry_query_input_port() - .template Eval>(context); - const geometry::SceneGraphInspector& inspector = query_object.inspector(); - - const std::vector>& per_tree_unlocked_indices = - this->EvalJointLockingCache(context).unlocked_velocity_indices_per_tree; - const MultibodyTreeTopology& topology = this->internal_tree().get_topology(); - - // Fill in the point contact pairs. - const std::vector>& point_pairs = - plant().EvalPointPairPenetrations(context); - for (const PenetrationAsPointPair& pair : point_pairs) { - const BodyIndex body_A_index = - this->geometry_id_to_body_index().at(pair.id_A); - const Body& body_A = plant().get_body(body_A_index); - const BodyIndex body_B_index = - this->geometry_id_to_body_index().at(pair.id_B); - const Body& body_B = plant().get_body(body_B_index); - - const TreeIndex& treeA_index = topology.body_to_tree_index(body_A_index); - const TreeIndex& treeB_index = topology.body_to_tree_index(body_B_index); - - // For joint locking, filter out contacts between bodies who belong to - // trees with 0 degrees of freedom. For a contact to remain in - // consideration, at least one of the trees involved has to be valid and - // have a non-zero number of DOFs. - if ((treeA_index.is_valid() && - per_tree_unlocked_indices[treeA_index].size() != 0) || - (treeB_index.is_valid() && - per_tree_unlocked_indices[treeB_index].size() != 0)) { - const T kA = GetPointContactStiffness( - pair.id_A, this->default_contact_stiffness(), inspector); - const T kB = GetPointContactStiffness( - pair.id_B, this->default_contact_stiffness(), inspector); - const T k = GetCombinedPointContactStiffness( - pair.id_A, pair.id_B, this->default_contact_stiffness(), inspector); - - // Hunt & Crossley dissipation. Used by TAMSI, ignored by SAP. - const T d = GetCombinedHuntCrossleyDissipation( - pair.id_A, pair.id_B, kA, kB, this->default_contact_dissipation(), - inspector); - - // Dissipation time scale. Used by SAP, ignored by TAMSI. - const double default_dissipation_time_constant = 0.1; - const T tau = GetCombinedDissipationTimeConstant( - pair.id_A, pair.id_B, default_dissipation_time_constant, - body_A.name(), body_B.name(), inspector); - const T mu = - GetCombinedDynamicCoulombFriction(pair.id_A, pair.id_B, inspector); - - // We compute the position of the point contact based on Hertz's theory - // for contact between two elastic bodies. - const T denom = kA + kB; - const T wA = (denom == 0 ? 0.5 : kA / denom); - const T wB = (denom == 0 ? 0.5 : kB / denom); - const Vector3 p_WC = wA * pair.p_WCa + wB * pair.p_WCb; - - const T phi0 = -pair.depth; - const T fn0 = k * pair.depth; // Used by TAMSI, ignored by SAP. - - contact_pairs.AppendPointData( - DiscreteContactPair{pair.id_A, - pair.id_B, - p_WC, - pair.nhat_BA_W, - phi0, - fn0, - k, - d, - tau, - mu, - {} /* no surface index */, - {} /* no face index */}); - } - } -} - -template <> -void CompliantContactManager:: - AppendDiscreteContactPairsForHydroelasticContact( - const drake::systems::Context&, - DiscreteContactData>*) const { - throw std::domain_error( - fmt::format("This method doesn't support T = {}.", - NiceTypeName::Get())); -} - -template -void CompliantContactManager:: - AppendDiscreteContactPairsForHydroelasticContact( - const systems::Context& context, - DiscreteContactData>* result) const { - DiscreteContactData>& contact_pairs = *result; - - // N.B. For discrete hydro we use a first order quadrature rule. As such, - // the per-face quadrature point is the face's centroid and the weight is 1. - // This is compatible with a mesh that is triangle or polygon. If we attempted - // higher order quadrature, polygons would have to be decomposed into smaller - // n-gons which can receive an appropriate set of quadrature points. - - const geometry::QueryObject& query_object = - this->plant() - .get_geometry_query_input_port() - .template Eval>(context); - const geometry::SceneGraphInspector& inspector = query_object.inspector(); - const std::vector>& surfaces = - this->EvalContactSurfaces(context); - - const std::vector>& per_tree_unlocked_indices = - this->EvalJointLockingCache(context).unlocked_velocity_indices_per_tree; - const MultibodyTreeTopology& topology = this->internal_tree().get_topology(); - - const int num_surfaces = surfaces.size(); - for (int surface_index = 0; surface_index < num_surfaces; ++surface_index) { - const auto& s = surfaces[surface_index]; - - const bool M_is_compliant = s.HasGradE_M(); - const bool N_is_compliant = s.HasGradE_N(); - DRAKE_DEMAND(M_is_compliant || N_is_compliant); - - // Combine dissipation. - const BodyIndex body_M_index = - this->geometry_id_to_body_index().at(s.id_M()); - const Body& body_M = plant().get_body(body_M_index); - const BodyIndex body_N_index = - this->geometry_id_to_body_index().at(s.id_N()); - const Body& body_N = plant().get_body(body_N_index); - - const TreeIndex& treeM_index = topology.body_to_tree_index(body_M_index); - const TreeIndex& treeN_index = topology.body_to_tree_index(body_N_index); - - // For joint locking, filter out contacts between bodies who belong to - // trees with 0 degrees of freedom. For a contact to remain in - // consideration, at least one of the trees involved has to be valid and - // have a non-zero number of DOFs. - if ((treeM_index.is_valid() && - per_tree_unlocked_indices[treeM_index].size() != 0) || - (treeN_index.is_valid() && - per_tree_unlocked_indices[treeN_index].size() != 0)) { - // 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); - const T hydro_modulus_N = GetHydroelasticModulus( - s.id_N(), std::numeric_limits::infinity(), inspector); - - // Hunt & Crossley dissipation. Used by TAMSI, ignored by SAP. - const T d = GetCombinedHuntCrossleyDissipation( - s.id_M(), s.id_N(), hydro_modulus_M, hydro_modulus_N, - 0.0 /* Default value */, inspector); - - // Dissipation time scale. Used by SAP, ignored by TAMSI. - const double default_dissipation_time_constant = 0.1; - const T tau = GetCombinedDissipationTimeConstant( - s.id_M(), s.id_N(), default_dissipation_time_constant, body_M.name(), - body_N.name(), inspector); - - // Combine friction coefficients. - const T mu = - GetCombinedDynamicCoulombFriction(s.id_M(), s.id_N(), inspector); - - for (int face = 0; face < s.num_faces(); ++face) { - const T& Ae = s.area(face); // Face element area. - - // We found out that the hydroelastic query might report - // infinitesimally small triangles (consider for instance an initial - // condition that perfectly places an object at zero distance from the - // ground.) While the area of zero sized triangles is not a problem by - // itself, the badly computed normal on these triangles leads to - // problems when computing the contact Jacobians (since we need to - // obtain an orthonormal basis based on that normal.) - // We therefore ignore infinitesimally small triangles. The tolerance - // below is somehow arbitrary and could possibly be tightened. - if (Ae > 1.0e-14) { - // From ContactSurface's documentation: The normal of each face is - // guaranteed to point "out of" N and "into" M. - const Vector3& nhat_W = s.face_normal(face); - - // One dimensional pressure gradient (in Pa/m). Unlike [Masterjohn - // 2022], for convenience we define both pressure gradients - // to be positive in the direction "into" the bodies. Therefore, - // we use the minus sign for gN. - // [Masterjohn 2022] Velocity Level Approximation of Pressure - // Field Contact Patches. - const T gM = M_is_compliant - ? s.EvaluateGradE_M_W(face).dot(nhat_W) - : T(std::numeric_limits::infinity()); - const T gN = N_is_compliant - ? -s.EvaluateGradE_N_W(face).dot(nhat_W) - : T(std::numeric_limits::infinity()); - - constexpr double kGradientEpsilon = 1.0e-14; - if (gM < kGradientEpsilon || gN < kGradientEpsilon) { - // Mathematically g = gN*gM/(gN+gM) and therefore g = 0 when - // either gradient on one of the bodies is zero. A zero gradient - // means there is no contact constraint, and therefore we - // ignore it to avoid numerical problems in the discrete solver. - continue; - } - - // Effective hydroelastic pressure gradient g result of - // compliant-compliant interaction, see [Masterjohn 2022]. - // The expression below is mathematically equivalent to g = - // gN*gM/(gN+gM) but it has the advantage of also being valid if - // one of the gradients is infinity. - const T g = 1.0 / (1.0 / gM + 1.0 / gN); - - // Position of quadrature point Q in the world frame (since mesh_W - // is measured and expressed in W). - const Vector3& p_WQ = s.centroid(face); - // For a triangle, its centroid has the fixed barycentric - // coordinates independent of the shape of the triangle. Using - // barycentric coordinates to evaluate field value could be - // faster than using Cartesian coordinates, especially if the - // TriangleSurfaceMeshFieldLinear<> does not store gradients and - // has to solve linear equations to convert Cartesian to - // barycentric coordinates. - const Vector3 tri_centroid_barycentric(1 / 3., 1 / 3., 1 / 3.); - // Pressure at the quadrature point. - const T p0 = - s.is_triangle() - ? s.tri_e_MN().Evaluate(face, tri_centroid_barycentric) - : s.poly_e_MN().EvaluateCartesian(face, p_WQ); - - // Force contribution by this quadrature point. - const T fn0 = Ae * p0; - - // Effective compliance in the normal direction for the given - // discrete patch, refer to [Masterjohn 2022] for details. - // [Masterjohn 2022] Masterjohn J., Guoy D., Shepherd J. and - // Castro A., 2022. Velocity Level Approximation of Pressure Field - // Contact Patches. Available at https://arxiv.org/abs/2110.04157. - const T k = Ae * g; - - // phi < 0 when in penetration. - const T phi0 = -p0 / g; - - if (k > 0) { - contact_pairs.AppendHydroData(DiscreteContactPair{ - s.id_M(), s.id_N(), p_WQ, nhat_W, phi0, fn0, k, d, tau, mu, - surface_index, face}); - } - } - } - } - } -} - template VectorX CompliantContactManager::CalcEffectiveDamping( const systems::Context& context) const { @@ -691,15 +192,6 @@ void CompliantContactManager::CalcAccelerationsDueToNonConstraintForcesCache( &forward_dynamics_cache->ac); } -template -const DiscreteContactData>& -CompliantContactManager::EvalDiscreteContactPairs( - const systems::Context& context) const { - return plant() - .get_cache_entry(cache_indexes_.discrete_contact_pairs) - .template Eval>>(context); -} - template const multibody::internal::AccelerationKinematicsCache& CompliantContactManager::EvalAccelerationsDueToNonConstraintForcesCache( @@ -731,38 +223,6 @@ void CompliantContactManager::DoCalcContactSolverResults( } } -template -void CompliantContactManager::DoCalcDiscreteValues( - const drake::systems::Context& context, - drake::systems::DiscreteValues* updates) const { - const ContactSolverResults& results = - this->EvalContactSolverResults(context); - - // Previous time step positions. - const int nq = plant().num_positions(); - const VectorX& x0 = - context.get_discrete_state(this->multibody_state_index()).value(); - const auto q0 = x0.topRows(nq); - - // Retrieve the rigid velocity for the next time step. - const VectorX& v_next = results.v_next.head(plant().num_velocities()); - - // Update generalized positions. - VectorX qdot_next(plant().num_positions()); - plant().MapVelocityToQDot(context, v_next, &qdot_next); - const VectorX q_next = q0 + plant().time_step() * qdot_next; - - VectorX x_next(plant().num_multibody_states()); - x_next << q_next, v_next; - updates->set_value(this->multibody_state_index(), x_next); - - if constexpr (std::is_same_v) { - if (deformable_driver_ != nullptr) { - deformable_driver_->CalcDiscreteStates(context, updates); - } - } -} - template void CompliantContactManager::AppendContactResultsForPointContact( const drake::systems::Context& context, @@ -917,7 +377,7 @@ void CompliantContactManager::CalcHydroelasticContactInfo( F_Ao_W_per_surface[surface_index] += Fq_Ao_W; // Velocity of Aq relative to Bq in the tangent direction. - // N.B. CompliantContactManager::CalcContactKinematics() uses the + // N.B. DiscreteUpdateManager::CalcContactKinematics() uses the // convention of computing J_AcBc_C and thus J_AcBc_C * v = v_AcBc_W (i.e. // relative velocity of Bc with respect to Ac). Thus we flip the sign here // for the convention used by HydroelasticQuadratureData. @@ -1032,7 +492,7 @@ CompliantContactManager::CloneToSymbolic() const { } template -void CompliantContactManager::ExtractModelInfo() { +void CompliantContactManager::DoExtractModelInfo() { // Collect joint damping coefficients into a vector. joint_damping_ = VectorX::Zero(plant().num_velocities()); for (JointIndex j(0); j < plant().num_joints(); ++j) { @@ -1068,40 +528,6 @@ void CompliantContactManager::ExtractModelInfo() { tamsi_driver_ = std::make_unique>(this); break; } - - // Collect information from each PhysicalModel owned by the plant. - const std::vector*> physical_models = - this->plant().physical_models(); - for (const auto* model : physical_models) { - std::visit( - [this](auto&& concrete_model) { - this->ExtractConcreteModel(concrete_model); - }, - model->ToPhysicalModelPointerVariant()); - } -} - -template -void CompliantContactManager::ExtractConcreteModel( - const DeformableModel* model) { - if constexpr (std::is_same_v) { - DRAKE_DEMAND(model != nullptr); - // TODO(xuchenhan-tri): Demote this to a DRAKE_DEMAND when we check for - // duplicated model with MbP::AddPhysicalModel. - if (deformable_driver_ != nullptr) { - throw std::logic_error( - fmt::format("{}: A deformable model has already been registered. " - "Repeated registration is not allowed.", - __func__)); - } - deformable_driver_ = - std::make_unique>(model, this); - } else { - unused(model); - throw std::logic_error( - "Only T = double is supported for the simulation of deformable " - "bodies."); - } } template diff --git a/multibody/plant/compliant_contact_manager.h b/multibody/plant/compliant_contact_manager.h index af12a0fbd6c4..fd320af96201 100644 --- a/multibody/plant/compliant_contact_manager.h +++ b/multibody/plant/compliant_contact_manager.h @@ -8,10 +8,8 @@ #include "drake/common/drake_copyable.h" #include "drake/common/eigen_types.h" #include "drake/geometry/scene_graph_inspector.h" -#include "drake/multibody/plant/contact_pair_kinematics.h" #include "drake/multibody/plant/contact_results.h" #include "drake/multibody/plant/deformable_driver.h" -#include "drake/multibody/plant/discrete_contact_data.h" #include "drake/multibody/plant/discrete_update_manager.h" #include "drake/systems/framework/context.h" @@ -70,9 +68,6 @@ struct AccelerationsDueNonConstraintForcesCache { // where p₀ is the object-centric virtual pressure field introduced by the // hydroelastic model. // -// TODO(amcastro-tri): Retire code from MultibodyPlant as this contact manager -// replaces all the contact related capabilities, per #16106. -// // @warning Scalar support on T = symbolic::Expression is only limited, // conditional to the solver in use: // - For TAMSI. Discrete updates are only supported when there is no contact @@ -121,8 +116,6 @@ class CompliantContactManager final : public DiscreteUpdateManager { // Struct used to conglomerate the indexes of cache entries declared by the // manager. struct CacheIndexes { - systems::CacheIndex contact_kinematics; - systems::CacheIndex discrete_contact_pairs; systems::CacheIndex hydroelastic_contact_info; systems::CacheIndex non_constraint_forces_accelerations; }; @@ -135,33 +128,13 @@ class CompliantContactManager final : public DiscreteUpdateManager { // Provide private access for unit testing only. friend class CompliantContactManagerTester; - const MultibodyTreeTopology& tree_topology() const { - return internal::GetInternalTree(this->plant()).get_topology(); - } - std::unique_ptr> CloneToDouble() const final; std::unique_ptr> CloneToAutoDiffXd() const final; std::unique_ptr> CloneToSymbolic() const final; - // Extracts non state dependent model information from MultibodyPlant. See - // DiscreteUpdateManager for details. - void ExtractModelInfo() final; - - // Associates the given `DeformableModel` with `this` manager. The discrete - // states of the deformable bodies registered in the given `model` will be - // advanced by this manager. This manager holds onto the given pointer and - // therefore the model must outlive the manager. - // @throws std::exception if a deformable model has already been registered. - // @pre model != nullptr. - void ExtractConcreteModel(const DeformableModel* model); - - // For testing purposes only, we provide a default no-op implementation on - // arbitrary models of unknown concrete model type. Otherwise, for the closed - // list of models forward declared in physical_model.h, we must provide a - // function that extracts the particular variant of the physical model. - void ExtractConcreteModel(std::monostate) {} + void DoExtractModelInfo() final; void DoDeclareCacheEntries() final; @@ -181,8 +154,6 @@ class CompliantContactManager final : public DiscreteUpdateManager { void DoCalcContactSolverResults( const systems::Context&, contact_solvers::internal::ContactSolverResults*) const final; - void DoCalcDiscreteValues(const systems::Context&, - systems::DiscreteValues*) const final; void DoCalcAccelerationKinematicsCache( const systems::Context&, multibody::internal::AccelerationKinematicsCache*) const final; @@ -192,51 +163,6 @@ class CompliantContactManager final : public DiscreteUpdateManager { const systems::Context& context, MultibodyForces* forces) const final; - // This method computes sparse kinematics information for each contact pair at - // the given configuration stored in `context`. - DiscreteContactData> CalcContactKinematics( - const systems::Context& context) const; - - // Eval version of CalcContactKinematics(). - const DiscreteContactData>& EvalContactKinematics( - const systems::Context& context) const; - - // Helper function for CalcContactKinematics() that computes the contact pair - // kinematics for point contact and hydroelastic contact respectively, - // depending on the value of `type`. - void AppendContactKinematics( - const systems::Context& context, - const std::vector>& contact_pairs, - DiscreteContactType type, - DiscreteContactData>* contact_kinematics) const; - - // Given the configuration stored in `context`, this method appends discrete - // pairs corresponding to point contact into `pairs`. - // @pre pairs != nullptr. - void AppendDiscreteContactPairsForPointContact( - const systems::Context& context, - DiscreteContactData>* pairs) const; - - // Given the configuration stored in `context`, this method appends discrete - // pairs corresponding to hydroelastic contact into `pairs`. - // @pre pairs != nullptr. - void AppendDiscreteContactPairsForHydroelasticContact( - const systems::Context& context, - DiscreteContactData>* pairs) const; - - // Given the configuration stored in `context`, this method computes all - // discrete contact pairs, including point, hydroelastic, and deformable - // contact, into `pairs`. Contact pairs including deformable bodies are - // guaranteed to come after point and hydroelastic contact pairs. Throws an - // exception if `pairs` is nullptr. - void CalcDiscreteContactPairs( - const systems::Context& context, - DiscreteContactData>* pairs) const; - - // Eval version of CalcDiscreteContactPairs(). - const DiscreteContactData>& EvalDiscreteContactPairs( - const systems::Context& context) const; - // Computes per-face contact information for the hydroelastic model (slip // velocity, traction, etc). On return contact_info->size() will equal the // number of faces discretizing the contact surface. @@ -278,10 +204,6 @@ class CompliantContactManager final : public DiscreteUpdateManager { // This information is extracted during the call to ExtractModelInfo(). VectorX joint_damping_; - // deformable_driver_ computes the information on all deformable bodies needed - // to advance the discrete states. - std::unique_ptr> deformable_driver_; - // Specific contact solver drivers are created at ExtractModelInfo() time, // when the manager retrieves modeling information from MultibodyPlant. // Only one of these drivers will be non-nullptr. @@ -289,18 +211,6 @@ class CompliantContactManager final : public DiscreteUpdateManager { std::unique_ptr> tamsi_driver_; }; -// N.B. These geometry queries are not supported when T = symbolic::Expression -// and therefore their implementation throws. -template <> -void CompliantContactManager::CalcDiscreteContactPairs( - const drake::systems::Context&, - DiscreteContactData>*) const; -template <> -void CompliantContactManager:: - AppendDiscreteContactPairsForHydroelasticContact( - const drake::systems::Context&, - DiscreteContactData>*) const; - } // namespace internal } // namespace multibody } // namespace drake diff --git a/multibody/plant/deformable_driver.cc b/multibody/plant/deformable_driver.cc index 008b7caf10d7..a51ed9cca683 100644 --- a/multibody/plant/deformable_driver.cc +++ b/multibody/plant/deformable_driver.cc @@ -16,6 +16,7 @@ #include "drake/multibody/fem/fem_model.h" #include "drake/multibody/fem/velocity_newmark_scheme.h" #include "drake/multibody/plant/contact_properties.h" +#include "drake/multibody/plant/multibody_plant.h" #include "drake/systems/framework/context.h" using drake::geometry::GeometryId; diff --git a/multibody/plant/deformable_driver.h b/multibody/plant/deformable_driver.h index 9963848ac54a..e5254bd921f8 100644 --- a/multibody/plant/deformable_driver.h +++ b/multibody/plant/deformable_driver.h @@ -15,7 +15,7 @@ #include "drake/multibody/plant/contact_pair_kinematics.h" #include "drake/multibody/plant/deformable_model.h" #include "drake/multibody/plant/discrete_contact_data.h" -#include "drake/multibody/plant/discrete_update_manager.h" +#include "drake/multibody/plant/discrete_contact_pair.h" #include "drake/systems/framework/context.h" namespace drake { @@ -69,6 +69,9 @@ class Multiplexer { int num_entries_{0}; }; +template +class DiscreteUpdateManager; + /* DeformableDriver is responsible for computing dynamics information about all deformable bodies. It works in tandem with a DeformableModel and a DiscreteUpdateManager that are provided at construction time. The deformable diff --git a/multibody/plant/deformable_model.cc b/multibody/plant/deformable_model.cc index 097e905186b9..db56d29e9103 100644 --- a/multibody/plant/deformable_model.cc +++ b/multibody/plant/deformable_model.cc @@ -11,6 +11,7 @@ #include "drake/multibody/fem/linear_simplex_element.h" #include "drake/multibody/fem/simplex_gaussian_quadrature.h" #include "drake/multibody/fem/volumetric_model.h" +#include "drake/multibody/plant/multibody_plant.h" namespace drake { namespace multibody { @@ -25,6 +26,12 @@ using geometry::SourceId; using fem::DeformableBodyConfig; using fem::MaterialModel; +template +DeformableModel::DeformableModel(MultibodyPlant* plant) : plant_(plant) { + DRAKE_DEMAND(plant_ != nullptr); + DRAKE_DEMAND(!plant_->is_finalized()); +} + template DeformableBodyId DeformableModel::RegisterDeformableBody( std::unique_ptr geometry_instance, diff --git a/multibody/plant/deformable_model.h b/multibody/plant/deformable_model.h index 0af68a9b17a2..6fdad2a6b0e0 100644 --- a/multibody/plant/deformable_model.h +++ b/multibody/plant/deformable_model.h @@ -12,12 +12,15 @@ #include "drake/multibody/fem/fem_model.h" #include "drake/multibody/plant/constraint_specs.h" #include "drake/multibody/plant/deformable_ids.h" -#include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/plant/physical_model.h" +#include "drake/multibody/tree/body.h" namespace drake { namespace multibody { +template +class MultibodyPlant; + /** DeformableModel implements the interface in PhysicalModel and provides the functionalities to specify deformable bodies. Unlike rigid bodies, the shape of deformable bodies can change in a simulation. Each deformable body is modeled @@ -34,10 +37,7 @@ class DeformableModel final : public multibody::PhysicalModel { /** Constructs a DeformableModel to be owned by the given MultibodyPlant. @pre plant != nullptr. @pre Finalize() has not been called on `plant`. */ - explicit DeformableModel(MultibodyPlant* plant) : plant_(plant) { - DRAKE_DEMAND(plant_ != nullptr); - DRAKE_DEMAND(!plant_->is_finalized()); - } + explicit DeformableModel(MultibodyPlant* plant); /** Returns the number of deformable bodies registered with this DeformableModel. */ diff --git a/multibody/plant/discrete_update_manager.cc b/multibody/plant/discrete_update_manager.cc index f64db87e8094..446b710e6ed7 100644 --- a/multibody/plant/discrete_update_manager.cc +++ b/multibody/plant/discrete_update_manager.cc @@ -1,13 +1,48 @@ #include "drake/multibody/plant/discrete_update_manager.h" +#include #include +#include "drake/multibody/plant/contact_properties.h" +#include "drake/multibody/plant/deformable_driver.h" +#include "drake/multibody/plant/deformable_model.h" +#include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/plant/multibody_plant_discrete_update_manager_attorney.h" namespace drake { namespace multibody { namespace internal { +using drake::geometry::ContactSurface; +using drake::geometry::GeometryId; +using drake::geometry::PenetrationAsPointPair; +using drake::math::RigidTransform; +using drake::math::RotationMatrix; +using drake::multibody::contact_solvers::internal::ContactConfiguration; +using drake::multibody::contact_solvers::internal::ContactSolverResults; +using drake::multibody::contact_solvers::internal::MatrixBlock; +using drake::multibody::internal::DiscreteContactPair; +using drake::multibody::internal::MultibodyTreeTopology; +using drake::systems::Context; + +template +void DiscreteUpdateManager::CalcDiscreteValues( + const systems::Context& context, + systems::DiscreteValues* updates) const { + // The discrete sampling of input ports needs to be the first step of a + // discrete update. + SampleDiscreteInputPortForces(context); + DRAKE_DEMAND(updates != nullptr); + // Perform discrete updates for deformable bodies if they exist. + if constexpr (std::is_same_v) { + if (deformable_driver_ != nullptr) { + deformable_driver_->CalcDiscreteStates(context, updates); + } + } + // Perform discrete updates for rigid bodies. + DoCalcDiscreteValues(context, updates); +} + template void DiscreteUpdateManager::DeclareCacheEntries() { // The Correct Solution: @@ -82,6 +117,32 @@ void DiscreteUpdateManager::DeclareCacheEntries() { systems::System::all_parameters_ticket()}); cache_indexes_.contact_results = contact_results_cache_entry.cache_index(); + // Cache discrete contact pairs. + const auto& discrete_contact_pairs_cache_entry = this->DeclareCacheEntry( + "Discrete contact pairs.", + systems::ValueProducer( + this, &DiscreteUpdateManager::CalcDiscreteContactPairs), + {systems::System::xd_ticket(), + systems::System::all_parameters_ticket()}); + cache_indexes_.discrete_contact_pairs = + discrete_contact_pairs_cache_entry.cache_index(); + + // Cache contact kinematics. + const auto& contact_kinematics_cache_entry = this->DeclareCacheEntry( + "Contact kinematics.", + systems::ValueProducer(this, + &DiscreteUpdateManager::CalcContactKinematics), + {systems::System::xd_ticket(), + systems::System::all_parameters_ticket()}); + cache_indexes_.contact_kinematics = + contact_kinematics_cache_entry.cache_index(); + + if constexpr (std::is_same_v) { + if (deformable_driver_ != nullptr) { + deformable_driver_->DeclareCacheEntries(this); + } + } + // Allow derived classes to declare their own cache entries. DoDeclareCacheEntries(); } @@ -155,6 +216,12 @@ bool DiscreteUpdateManager::is_cloneable_to_symbolic() const { return false; } +template +const MultibodyPlant& DiscreteUpdateManager::plant() const { + DRAKE_DEMAND(plant_ != nullptr); + return *plant_; +} + template const MultibodyTree& DiscreteUpdateManager::internal_tree() const { return MultibodyPlantDiscreteUpdateManagerAttorney::internal_tree(plant()); @@ -412,6 +479,467 @@ void DiscreteUpdateManager::CalcDiscreteUpdateMultibodyForces( DoCalcDiscreteUpdateMultibodyForces(context, forces); } +template +void DiscreteUpdateManager::CalcContactKinematics( + const systems::Context& context, + DiscreteContactData>* result) const { + plant().ValidateContext(context); + DRAKE_DEMAND(result != nullptr); + result->Clear(); + const DiscreteContactData>& contact_pairs = + EvalDiscreteContactPairs(context); + const int num_contacts = contact_pairs.size(); + + // Quick no-op exit. + if (num_contacts == 0) return; + + result->Reserve(contact_pairs.num_point_contacts(), + contact_pairs.num_hydro_contacts(), + contact_pairs.num_deformable_contacts()); + AppendContactKinematics(context, contact_pairs.point_contact_data(), + DiscreteContactType::kPoint, result); + AppendContactKinematics(context, contact_pairs.hydro_contact_data(), + DiscreteContactType::kHydroelastic, result); + if constexpr (std::is_same_v) { + if (deformable_driver_ != nullptr) { + deformable_driver_->AppendContactKinematics(context, result); + } + } +} + +template +void DiscreteUpdateManager::AppendContactKinematics( + const systems::Context& context, + const std::vector>& contact_pairs, + DiscreteContactType type, + DiscreteContactData>* contact_kinematics) const { + // Scratch workspace variables. + const int nv = plant().num_velocities(); + Matrix3X Jv_WAc_W(3, nv); + Matrix3X Jv_WBc_W(3, nv); + Matrix3X Jv_AcBc_W(3, nv); + + const Frame& frame_W = plant().world_frame(); + for (int icontact = 0; icontact < ssize(contact_pairs); ++icontact) { + const auto& point_pair = contact_pairs[icontact]; + + const GeometryId geometryA_id = point_pair.id_A; + const GeometryId geometryB_id = point_pair.id_B; + + BodyIndex bodyA_index = this->geometry_id_to_body_index().at(geometryA_id); + const Body& bodyA = plant().get_body(bodyA_index); + BodyIndex bodyB_index = this->geometry_id_to_body_index().at(geometryB_id); + const Body& bodyB = plant().get_body(bodyB_index); + + // Contact normal from point A into B. + const Vector3& nhat_W = -point_pair.nhat_BA_W; + const Vector3& p_WC = point_pair.p_WC; + + // Contact point position relative to each body. + const RigidTransform& X_WA = plant().EvalBodyPoseInWorld(context, bodyA); + const Vector3& p_WA = X_WA.translation(); + const Vector3 p_AC_W = p_WC - p_WA; + const RigidTransform& X_WB = plant().EvalBodyPoseInWorld(context, bodyB); + const Vector3& p_WB = X_WB.translation(); + const Vector3 p_BC_W = p_WC - p_WB; + + // Since v_AcBc_W = v_WBc - v_WAc the relative velocity Jacobian will be: + // J_AcBc_W = Jv_WBc_W - Jv_WAc_W. + // That is the relative velocity at C is v_AcBc_W = J_AcBc_W * v. + this->internal_tree().CalcJacobianTranslationalVelocity( + context, JacobianWrtVariable::kV, bodyA.body_frame(), frame_W, p_WC, + frame_W, frame_W, &Jv_WAc_W); + this->internal_tree().CalcJacobianTranslationalVelocity( + context, JacobianWrtVariable::kV, bodyB.body_frame(), frame_W, p_WC, + frame_W, frame_W, &Jv_WBc_W); + Jv_AcBc_W = Jv_WBc_W - Jv_WAc_W; + + // Define a contact frame C at the contact point such that the z-axis Cz + // equals nhat_W. The tangent vectors are arbitrary, with the only + // requirement being that they form a valid right handed basis with nhat_W. + math::RotationMatrix R_WC = + math::RotationMatrix::MakeFromOneVector(nhat_W, 2); + + const TreeIndex& treeA_index = + tree_topology().body_to_tree_index(bodyA_index); + const TreeIndex& treeB_index = + tree_topology().body_to_tree_index(bodyB_index); + // Sanity check, at least one must be valid. + DRAKE_DEMAND(treeA_index.is_valid() || treeB_index.is_valid()); + + // We have at most two blocks per contact. + std::vector::JacobianTreeBlock> + jacobian_blocks; + jacobian_blocks.reserve(2); + + // Tree A contribution to contact Jacobian Jv_W_AcBc_C. + if (treeA_index.is_valid()) { + Matrix3X J = R_WC.matrix().transpose() * + Jv_AcBc_W.middleCols( + tree_topology().tree_velocities_start(treeA_index), + tree_topology().num_tree_velocities(treeA_index)); + jacobian_blocks.emplace_back(treeA_index, MatrixBlock(std::move(J))); + } + + // Tree B contribution to contact Jacobian Jv_W_AcBc_C. + // This contribution must be added only if B is different from A. + if ((treeB_index.is_valid() && !treeA_index.is_valid()) || + (treeB_index.is_valid() && treeB_index != treeA_index)) { + Matrix3X J = R_WC.matrix().transpose() * + Jv_AcBc_W.middleCols( + tree_topology().tree_velocities_start(treeB_index), + tree_topology().num_tree_velocities(treeB_index)); + jacobian_blocks.emplace_back(treeB_index, MatrixBlock(std::move(J))); + } + + ContactConfiguration configuration{.objectA = bodyA_index, + .p_ApC_W = p_AC_W, + .objectB = bodyB_index, + .p_BqC_W = p_BC_W, + .phi = point_pair.phi0, + .R_WC = R_WC}; + switch (type) { + case DiscreteContactType::kPoint: { + contact_kinematics->AppendPointData(ContactPairKinematics{ + std::move(jacobian_blocks), std::move(configuration)}); + break; + } + case DiscreteContactType::kHydroelastic: { + contact_kinematics->AppendHydroData(ContactPairKinematics{ + std::move(jacobian_blocks), std::move(configuration)}); + break; + } + case DiscreteContactType::kDeformable: { + throw std::logic_error( + "Call DeformableDriver::AppendContactKinematics() to compute " + "contact kinematics for deformable contact instead."); + } + } + } +} + +template +void DiscreteUpdateManager::CalcDiscreteContactPairs( + const systems::Context& context, + DiscreteContactData>* result) const { + plant().ValidateContext(context); + DRAKE_DEMAND(result != nullptr); + result->Clear(); + if (plant().num_collision_geometries() == 0) return; + + const auto contact_model = plant().get_contact_model(); + + // We first compute the number of contact pairs so that we can allocate all + // memory at once. + // N.B. num_point_pairs = 0 when: + // 1. There are legitimately no point pairs or, + // 2. the point pair model is not even in use. + // We guard for case (2) since EvalPointPairPenetrations() cannot be called + // when point contact is not used and would otherwise throw an exception. + int num_point_pairs = 0; // The number of point contact pairs. + if (contact_model == ContactModel::kPoint || + contact_model == ContactModel::kHydroelasticWithFallback) { + num_point_pairs = plant().EvalPointPairPenetrations(context).size(); + } + + int num_quadrature_pairs = 0; + // N.B. For discrete hydro we use a first order quadrature rule. As such, + // the per-face quadrature point is the face's centroid and the weight is 1. + // This is compatible with a mesh that is triangle or polygon. If we attempted + // higher order quadrature, polygons would have to be decomposed into smaller + // n-gons which can receive an appropriate set of quadrature points. + if (contact_model == ContactModel::kHydroelastic || + contact_model == ContactModel::kHydroelasticWithFallback) { + const std::vector>& surfaces = + this->EvalContactSurfaces(context); + for (const auto& s : surfaces) { + // One quadrature point per face. + num_quadrature_pairs += s.num_faces(); + } + } + result->Reserve(num_point_pairs, num_quadrature_pairs, 0); + + if (contact_model == ContactModel::kPoint || + contact_model == ContactModel::kHydroelasticWithFallback) { + AppendDiscreteContactPairsForPointContact(context, result); + } + if (contact_model == ContactModel::kHydroelastic || + contact_model == ContactModel::kHydroelasticWithFallback) { + AppendDiscreteContactPairsForHydroelasticContact(context, result); + } + if constexpr (std::is_same_v) { + if (deformable_driver_ != nullptr) { + deformable_driver_->AppendDiscreteContactPairs(context, result); + } + } +} + +template <> +void DiscreteUpdateManager::CalcDiscreteContactPairs( + const drake::systems::Context&, + DiscreteContactData>*) const { + // Currently, the computation of contact pairs is not supported when T = + // symbolic::Expression. + throw std::domain_error( + fmt::format("This method doesn't support T = {}.", + NiceTypeName::Get())); +} + +template +void DiscreteUpdateManager::AppendDiscreteContactPairsForPointContact( + const systems::Context& context, + DiscreteContactData>* result) const { + DiscreteContactData>& contact_pairs = *result; + + const geometry::QueryObject& query_object = + this->plant() + .get_geometry_query_input_port() + .template Eval>(context); + const geometry::SceneGraphInspector& inspector = query_object.inspector(); + + const std::vector>& per_tree_unlocked_indices = + this->EvalJointLockingCache(context).unlocked_velocity_indices_per_tree; + const MultibodyTreeTopology& topology = this->internal_tree().get_topology(); + + // Fill in the point contact pairs. + const std::vector>& point_pairs = + plant().EvalPointPairPenetrations(context); + for (const PenetrationAsPointPair& pair : point_pairs) { + const BodyIndex body_A_index = + this->geometry_id_to_body_index().at(pair.id_A); + const Body& body_A = plant().get_body(body_A_index); + const BodyIndex body_B_index = + this->geometry_id_to_body_index().at(pair.id_B); + const Body& body_B = plant().get_body(body_B_index); + + const TreeIndex& treeA_index = topology.body_to_tree_index(body_A_index); + const TreeIndex& treeB_index = topology.body_to_tree_index(body_B_index); + + // For joint locking, filter out contacts between bodies who belong to + // trees with 0 degrees of freedom. For a contact to remain in + // consideration, at least one of the trees involved has to be valid and + // have a non-zero number of DOFs. + if ((treeA_index.is_valid() && + per_tree_unlocked_indices[treeA_index].size() != 0) || + (treeB_index.is_valid() && + per_tree_unlocked_indices[treeB_index].size() != 0)) { + const T kA = GetPointContactStiffness( + pair.id_A, this->default_contact_stiffness(), inspector); + const T kB = GetPointContactStiffness( + pair.id_B, this->default_contact_stiffness(), inspector); + const T k = GetCombinedPointContactStiffness( + pair.id_A, pair.id_B, this->default_contact_stiffness(), inspector); + + // Hunt & Crossley dissipation. Used by TAMSI, ignored by SAP. + const T d = GetCombinedHuntCrossleyDissipation( + pair.id_A, pair.id_B, kA, kB, this->default_contact_dissipation(), + inspector); + + // Dissipation time scale. Used by SAP, ignored by TAMSI. + const double default_dissipation_time_constant = 0.1; + const T tau = GetCombinedDissipationTimeConstant( + pair.id_A, pair.id_B, default_dissipation_time_constant, + body_A.name(), body_B.name(), inspector); + const T mu = + GetCombinedDynamicCoulombFriction(pair.id_A, pair.id_B, inspector); + + // We compute the position of the point contact based on Hertz's theory + // for contact between two elastic bodies. + const T denom = kA + kB; + const T wA = (denom == 0 ? 0.5 : kA / denom); + const T wB = (denom == 0 ? 0.5 : kB / denom); + const Vector3 p_WC = wA * pair.p_WCa + wB * pair.p_WCb; + + const T phi0 = -pair.depth; + const T fn0 = k * pair.depth; // Used by TAMSI, ignored by SAP. + + contact_pairs.AppendPointData( + DiscreteContactPair{pair.id_A, + pair.id_B, + p_WC, + pair.nhat_BA_W, + phi0, + fn0, + k, + d, + tau, + mu, + {} /* no surface index */, + {} /* no face index */}); + } + } +} + +template <> +void DiscreteUpdateManager:: + AppendDiscreteContactPairsForHydroelasticContact( + const drake::systems::Context&, + DiscreteContactData>*) const { + throw std::domain_error( + fmt::format("This method doesn't support T = {}.", + NiceTypeName::Get())); +} + +template +void DiscreteUpdateManager::AppendDiscreteContactPairsForHydroelasticContact( + const systems::Context& context, + DiscreteContactData>* result) const { + DiscreteContactData>& contact_pairs = *result; + + // N.B. For discrete hydro we use a first order quadrature rule. As such, + // the per-face quadrature point is the face's centroid and the weight is 1. + // This is compatible with a mesh that is triangle or polygon. If we attempted + // higher order quadrature, polygons would have to be decomposed into smaller + // n-gons which can receive an appropriate set of quadrature points. + + const geometry::QueryObject& query_object = + this->plant() + .get_geometry_query_input_port() + .template Eval>(context); + const geometry::SceneGraphInspector& inspector = query_object.inspector(); + const std::vector>& surfaces = + this->EvalContactSurfaces(context); + + const std::vector>& per_tree_unlocked_indices = + this->EvalJointLockingCache(context).unlocked_velocity_indices_per_tree; + const MultibodyTreeTopology& topology = this->internal_tree().get_topology(); + + const int num_surfaces = surfaces.size(); + for (int surface_index = 0; surface_index < num_surfaces; ++surface_index) { + const auto& s = surfaces[surface_index]; + + const bool M_is_compliant = s.HasGradE_M(); + const bool N_is_compliant = s.HasGradE_N(); + DRAKE_DEMAND(M_is_compliant || N_is_compliant); + + // Combine dissipation. + const BodyIndex body_M_index = + this->geometry_id_to_body_index().at(s.id_M()); + const Body& body_M = plant().get_body(body_M_index); + const BodyIndex body_N_index = + this->geometry_id_to_body_index().at(s.id_N()); + const Body& body_N = plant().get_body(body_N_index); + + const TreeIndex& treeM_index = topology.body_to_tree_index(body_M_index); + const TreeIndex& treeN_index = topology.body_to_tree_index(body_N_index); + + // For joint locking, filter out contacts between bodies who belong to + // trees with 0 degrees of freedom. For a contact to remain in + // consideration, at least one of the trees involved has to be valid and + // have a non-zero number of DOFs. + if ((treeM_index.is_valid() && + per_tree_unlocked_indices[treeM_index].size() != 0) || + (treeN_index.is_valid() && + per_tree_unlocked_indices[treeN_index].size() != 0)) { + // 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); + const T hydro_modulus_N = GetHydroelasticModulus( + s.id_N(), std::numeric_limits::infinity(), inspector); + + // Hunt & Crossley dissipation. Used by TAMSI, ignored by SAP. + const T d = GetCombinedHuntCrossleyDissipation( + s.id_M(), s.id_N(), hydro_modulus_M, hydro_modulus_N, + 0.0 /* Default value */, inspector); + + // Dissipation time scale. Used by SAP, ignored by TAMSI. + const double default_dissipation_time_constant = 0.1; + const T tau = GetCombinedDissipationTimeConstant( + s.id_M(), s.id_N(), default_dissipation_time_constant, body_M.name(), + body_N.name(), inspector); + + // Combine friction coefficients. + const T mu = + GetCombinedDynamicCoulombFriction(s.id_M(), s.id_N(), inspector); + + for (int face = 0; face < s.num_faces(); ++face) { + const T& Ae = s.area(face); // Face element area. + + // We found out that the hydroelastic query might report + // infinitesimally small triangles (consider for instance an initial + // condition that perfectly places an object at zero distance from the + // ground.) While the area of zero sized triangles is not a problem by + // itself, the badly computed normal on these triangles leads to + // problems when computing the contact Jacobians (since we need to + // obtain an orthonormal basis based on that normal.) + // We therefore ignore infinitesimally small triangles. The tolerance + // below is somehow arbitrary and could possibly be tightened. + if (Ae > 1.0e-14) { + // From ContactSurface's documentation: The normal of each face is + // guaranteed to point "out of" N and "into" M. + const Vector3& nhat_W = s.face_normal(face); + + // One dimensional pressure gradient (in Pa/m). Unlike [Masterjohn + // 2022], for convenience we define both pressure gradients + // to be positive in the direction "into" the bodies. Therefore, + // we use the minus sign for gN. + // [Masterjohn 2022] Velocity Level Approximation of Pressure + // Field Contact Patches. + const T gM = M_is_compliant + ? s.EvaluateGradE_M_W(face).dot(nhat_W) + : T(std::numeric_limits::infinity()); + const T gN = N_is_compliant + ? -s.EvaluateGradE_N_W(face).dot(nhat_W) + : T(std::numeric_limits::infinity()); + + constexpr double kGradientEpsilon = 1.0e-14; + if (gM < kGradientEpsilon || gN < kGradientEpsilon) { + // Mathematically g = gN*gM/(gN+gM) and therefore g = 0 when + // either gradient on one of the bodies is zero. A zero gradient + // means there is no contact constraint, and therefore we + // ignore it to avoid numerical problems in the discrete solver. + continue; + } + + // Effective hydroelastic pressure gradient g result of + // compliant-compliant interaction, see [Masterjohn 2022]. + // The expression below is mathematically equivalent to g = + // gN*gM/(gN+gM) but it has the advantage of also being valid if + // one of the gradients is infinity. + const T g = 1.0 / (1.0 / gM + 1.0 / gN); + + // Position of quadrature point Q in the world frame (since mesh_W + // is measured and expressed in W). + const Vector3& p_WQ = s.centroid(face); + // For a triangle, its centroid has the fixed barycentric + // coordinates independent of the shape of the triangle. Using + // barycentric coordinates to evaluate field value could be + // faster than using Cartesian coordinates, especially if the + // TriangleSurfaceMeshFieldLinear<> does not store gradients and + // has to solve linear equations to convert Cartesian to + // barycentric coordinates. + const Vector3 tri_centroid_barycentric(1 / 3., 1 / 3., 1 / 3.); + // Pressure at the quadrature point. + const T p0 = + s.is_triangle() + ? s.tri_e_MN().Evaluate(face, tri_centroid_barycentric) + : s.poly_e_MN().EvaluateCartesian(face, p_WQ); + + // Force contribution by this quadrature point. + const T fn0 = Ae * p0; + + // Effective compliance in the normal direction for the given + // discrete patch, refer to [Masterjohn 2022] for details. + // [Masterjohn 2022] Masterjohn J., Guoy D., Shepherd J. and + // Castro A., 2022. Velocity Level Approximation of Pressure Field + // Contact Patches. Available at https://arxiv.org/abs/2110.04157. + const T k = Ae * g; + + // phi < 0 when in penetration. + const T phi0 = -p0 / g; + + if (k > 0) { + contact_pairs.AppendHydroData(DiscreteContactPair{ + s.id_M(), s.id_N(), p_WQ, nhat_W, phi0, fn0, k, d, tau, mu, + surface_index, face}); + } + } + } + } + } +} + template const MultibodyForces& DiscreteUpdateManager::EvalDiscreteUpdateMultibodyForces( @@ -429,6 +957,89 @@ const ContactResults& DiscreteUpdateManager::EvalContactResults( .template Eval>(context); } +template +const DiscreteContactData>& +DiscreteUpdateManager::EvalContactKinematics( + const systems::Context& context) const { + return plant() + .get_cache_entry(cache_indexes_.contact_kinematics) + .template Eval>>(context); +} + +template +const DiscreteContactData>& +DiscreteUpdateManager::EvalDiscreteContactPairs( + const systems::Context& context) const { + return plant() + .get_cache_entry(cache_indexes_.discrete_contact_pairs) + .template Eval>>(context); +} + +template +void DiscreteUpdateManager::DoCalcDiscreteValues( + const systems::Context& context, + systems::DiscreteValues* updates) const { + const ContactSolverResults& results = EvalContactSolverResults(context); + + // Previous time step positions. + const int nq = plant().num_positions(); + const VectorX& x0 = + context.get_discrete_state(this->multibody_state_index()).value(); + const auto q0 = x0.topRows(nq); + + // Retrieve the rigid velocity for the next time step. + const VectorX& v_next = results.v_next.head(plant().num_velocities()); + + // Update generalized positions. + VectorX qdot_next(plant().num_positions()); + plant().MapVelocityToQDot(context, v_next, &qdot_next); + const VectorX q_next = q0 + plant().time_step() * qdot_next; + + VectorX x_next(plant().num_multibody_states()); + x_next << q_next, v_next; + updates->set_value(this->multibody_state_index(), x_next); +} + +template +void DiscreteUpdateManager::ExtractModelInfo() { + // Collect information from each PhysicalModel owned by the plant. + const std::vector*> physical_models = + this->plant().physical_models(); + for (const auto* model : physical_models) { + std::visit( + [this](auto&& concrete_model) { + this->ExtractConcreteModel(concrete_model); + }, + model->ToPhysicalModelPointerVariant()); + } + + // Allow derived class to extract relevant model information. + DoExtractModelInfo(); +} + +template +void DiscreteUpdateManager::ExtractConcreteModel( + const DeformableModel* model) { + if constexpr (std::is_same_v) { + DRAKE_DEMAND(model != nullptr); + // TODO(xuchenhan-tri): Demote this to a DRAKE_DEMAND when we check for + // duplicated model with MbP::AddPhysicalModel. + if (deformable_driver_ != nullptr) { + throw std::logic_error( + fmt::format("{}: A deformable model has already been registered. " + "Repeated registration is not allowed.", + __func__)); + } + deformable_driver_ = + std::make_unique>(model, this); + } else { + unused(model); + throw std::logic_error( + "Only T = double is supported for the simulation of deformable " + "bodies."); + } +} + } // namespace internal } // namespace multibody } // namespace drake diff --git a/multibody/plant/discrete_update_manager.h b/multibody/plant/discrete_update_manager.h index 4cb4132272f5..339d9bcc1c4c 100644 --- a/multibody/plant/discrete_update_manager.h +++ b/multibody/plant/discrete_update_manager.h @@ -14,8 +14,12 @@ #include "drake/multibody/contact_solvers/contact_solver_results.h" #include "drake/multibody/plant/constraint_specs.h" #include "drake/multibody/plant/contact_jacobians.h" +#include "drake/multibody/plant/contact_pair_kinematics.h" #include "drake/multibody/plant/contact_results.h" #include "drake/multibody/plant/coulomb_friction.h" +#include "drake/multibody/plant/deformable_driver.h" +#include "drake/multibody/plant/deformable_model.h" +#include "drake/multibody/plant/discrete_contact_data.h" #include "drake/multibody/plant/discrete_contact_pair.h" #include "drake/multibody/plant/scalar_convertible_component.h" #include "drake/multibody/tree/multibody_tree.h" @@ -35,12 +39,12 @@ class AccelerationKinematicsCache; template struct JointLockingCacheData; -// Struct to store MultibodyPlant input forces. +/* Struct to store MultibodyPlant input forces. */ template struct InputPortForces { DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(InputPortForces); - // Constructs an `InputPortForces` to store input port values for the given - // `plant`. Values are initialized to zero at construction. + /* Constructs an `InputPortForces` to store input port values for the given + `plant`. Values are initialized to zero at construction. */ explicit InputPortForces(const MultibodyPlant& plant) : externally_applied_forces(plant), actuation_w_pd(plant.num_velocities()), @@ -52,12 +56,12 @@ struct InputPortForces { actuation_w_pd.setZero(); actuation_wo_pd.setZero(); } - // Externally applied generalized and body spatial forces. + /* Externally applied generalized and body spatial forces. */ MultibodyForces externally_applied_forces; - // Joint actuation, indexed by DOF. We split them into actuators with and - // without PD control. Both have size equal to the number of generalized - // velocities. Entries with no contribution are zero. In other words, the - // total actuation equals actuation_w_pd + actuation_wo_pd. + /* Joint actuation, indexed by DOF. We split them into actuators with and + without PD control. Both have size equal to the number of generalized + velocities. Entries with no contribution are zero. In other words, the + total actuation equals actuation_w_pd + actuation_wo_pd. */ VectorX actuation_w_pd; // For actuated joints with PD control. VectorX actuation_wo_pd; // For actuated joints without PD control. }; @@ -114,10 +118,7 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent { /* Returns the MultibodyPlant that owns this DiscreteUpdateManager. @pre SetOwningMultibodyPlant() has been successfully invoked. */ - const MultibodyPlant& plant() const { - DRAKE_DEMAND(plant_ != nullptr); - return *plant_; - } + const MultibodyPlant& plant() const; /* (Internal) Sets the given `plant` as the MultibodyPlant owning this DiscreteUpdateManager. This method is meant to be called by @@ -162,24 +163,18 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent { /* MultibodyPlant invokes this method to perform the discrete variables update. */ void CalcDiscreteValues(const systems::Context& context, - systems::DiscreteValues* updates) const { - // The discrete sampling of input ports needs to be the first step of a - // discrete update. - SampleDiscreteInputPortForces(context); - DRAKE_DEMAND(updates != nullptr); - DoCalcDiscreteValues(context, updates); - } + systems::DiscreteValues* updates) const; /* Evaluates the contact results used in CalcDiscreteValues() to advance the discrete update from the state stored in `context`. */ const ContactResults& EvalContactResults( const systems::Context& context) const; - // Computes all non-contact applied forces including: - // - Force elements. - // - Discretely sampled joint actuation. - // - Discretely sampled externally applied spatial forces. - // - (possibly) Joint limits. + /* Computes all non-contact applied forces including: + - Force elements. + - Discretely sampled joint actuation. + - Discretely sampled externally applied spatial forces. + - (possibly) Joint limits. */ void CalcNonContactForces(const drake::systems::Context& context, bool include_joint_limit_penalty_forces, bool include_pd_controlled_input, @@ -198,11 +193,21 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent { const MultibodyForces& EvalDiscreteUpdateMultibodyForces( const systems::Context& context) const; + /* Evaluates sparse kinematics information for each contact pair at + the given configuration stored in `context`. */ + const DiscreteContactData>& EvalContactKinematics( + const systems::Context& context) const; + + /* Given the configuration stored in `context`, evalutates all discrete + contact pairs, including point, hydroelastic, and deformable contact. */ + const DiscreteContactData>& EvalDiscreteContactPairs( + const systems::Context& context) const; + /* Publicly exposed MultibodyPlant private/protected methods. @{ */ - // N.B. Keep the spelling and order of declarations here identical to the - // MultibodyPlantDiscreteUpdateManagerAttorney spelling and order of same. + /* N.B. Keep the spelling and order of declarations here identical to the + MultibodyPlantDiscreteUpdateManagerAttorney spelling and order of same. */ const MultibodyTree& internal_tree() const; @@ -218,6 +223,16 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent { /* @} */ + const MultibodyTreeTopology& tree_topology() const { + return internal::GetInternalTree(this->plant()).get_topology(); + } + + /* Returns the pointer to the DeformableDriver owned by `this` manager if one + exists. Otherwise, returns nullptr. */ + const DeformableDriver* deformable_driver() const { + return deformable_driver_.get(); + } + protected: /* Derived classes that support making a clone that uses double as a scalar type must implement this so that it creates a copy of the object with double @@ -239,9 +254,9 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent { virtual std::unique_ptr> CloneToSymbolic() const; - /* Derived DiscreteUpdateManager should override this method to extract + /* Derived DiscreteUpdateManager can override this method to extract information from the owning MultibodyPlant. */ - virtual void ExtractModelInfo() {} + virtual void DoExtractModelInfo() {} /* Derived classes can implement this method if they wish to declare their own cache entries. It defaults to a no-op. */ @@ -280,8 +295,8 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent { /* Exposed MultibodyPlant private/protected methods. @{ */ - // N.B. Keep the spelling and order of declarations here identical to the - // MultibodyPlantDiscreteUpdateManagerAttorney spelling and order of same. + /* N.B. Keep the spelling and order of declarations here identical to the + MultibodyPlantDiscreteUpdateManagerAttorney spelling and order of same. */ const std::vector>& EvalContactSurfaces( const systems::Context& context) const; @@ -329,12 +344,6 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent { const systems::Context& context, internal::AccelerationKinematicsCache* ac) const = 0; - virtual void DoCalcDiscreteValues( - const systems::Context& context, - systems::DiscreteValues* updates) const = 0; - - /* Concrete managers must implement this method to compute contact results - according to the underlying formulation of contact. */ virtual void DoCalcContactResults( const systems::Context& context, ContactResults* contact_results) const = 0; @@ -348,73 +357,147 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent { virtual void DoCalcDiscreteUpdateMultibodyForces( const systems::Context& context, MultibodyForces* forces) const = 0; - // Struct used to conglomerate the indexes of cache entries declared by the - // manager. + /* Performs discrete updates for rigid DoFs in the system. Defaults to + symplectic Euler updates. Derived classes may choose to override the + implemention to provide a different update scheme. */ + virtual void DoCalcDiscreteValues(const systems::Context& context, + systems::DiscreteValues* updates) const; + + /* Extracts information from all PhysicalModels that are added to the + MultibodyPlant associated with this discrete update manager. */ + void ExtractModelInfo(); + + /* Associates the given `DeformableModel` with `this` manager. The discrete + states of the deformable bodies registered in the given `model` will be + advanced by this manager. This manager holds onto the given pointer and + therefore the model must outlive the manager. + @throws std::exception if a deformable model has already been registered. + @pre model != nullptr. */ + void ExtractConcreteModel(const DeformableModel* model); + + /* For testing purposes only, we provide a default no-op implementation on + arbitrary models of unknown concrete model type. Otherwise, for the closed + list of models forward declared in physical_model.h, we must provide a + function that extracts the particular variant of the physical model. */ + void ExtractConcreteModel(std::monostate) {} + + /* Struct used to conglomerate the indexes of cache entries declared by the + manager. */ struct CacheIndexes { - // Manually managed cache entry that mimics a discrete sampling of forces - // added to the owning MbP via input ports (see issue #12786). This cache - // entry is manually marked out-of-date, updated, and immediately marked - // up-to-date at the beginning of each discrete update. So when caching is - // enabled, it is always up-to-date as long as any discrete update has - // happened. The only time this cache entry may be updated automatically via - // the caching mechanism is when a downstream cache entry that depends on - // this cache entry requests its value before any discrete update has - // happened. Evaluating this cache entry when caching is disabled throws an - // exception. + /* Manually managed cache entry that mimics a discrete sampling of forces + added to the owning MbP via input ports (see issue #12786). This cache + entry is manually marked out-of-date, updated, and immediately marked + up-to-date at the beginning of each discrete update. So when caching is + enabled, it is always up-to-date as long as any discrete update has + happened. The only time this cache entry may be updated automatically via + the caching mechanism is when a downstream cache entry that depends on + this cache entry requests its value before any discrete update has + happened. Evaluating this cache entry when caching is disabled throws an + exception. */ systems::CacheIndex discrete_input_port_forces; systems::CacheIndex contact_solver_results; systems::CacheIndex non_contact_forces_evaluation_in_progress; systems::CacheIndex contact_results; systems::CacheIndex discrete_update_multibody_forces; + systems::CacheIndex contact_kinematics; + systems::CacheIndex discrete_contact_pairs; }; - // Exposes indices for the cache entries declared by this class for derived - // classes to depend on. + /* Exposes indices for the cache entries declared by this class for derived + classes to depend on. */ CacheIndexes cache_indexes() const { return cache_indexes_; } private: - // Due to issue #12786, we cannot mark the calculation of non-contact forces - // (and the acceleration it induces) dependent on the discrete - // MultibodyPlant's inputs, as it should. However, by removing this - // dependency, we run the risk of an undetected algebraic loop. We use this - // function to guard against such algebraic loop. In particular, calling this - // function immediately upon entering the calculation of non-contact forces - // sets a flag indicating the calculation of non-contact forces is in - // progress. Then, this function returns a ScopeExit which turns off the flag - // when going out of scope at the end of the non-contact forces calculation. - // If this function is called again while the flag is on, it means that an - // algebraic loop exists and an exception is thrown. + /* Due to issue #12786, we cannot mark the calculation of non-contact forces + (and the acceleration it induces) dependent on the discrete + MultibodyPlant's inputs, as it should. However, by removing this + dependency, we run the risk of an undetected algebraic loop. We use this + function to guard against such algebraic loop. In particular, calling this + function immediately upon entering the calculation of non-contact forces + sets a flag indicating the calculation of non-contact forces is in + progress. Then, this function returns a ScopeExit which turns off the flag + when going out of scope at the end of the non-contact forces calculation. + If this function is called again while the flag is on, it means that an + algebraic loop exists and an exception is thrown. */ [[nodiscard]] ScopeExit ThrowIfNonContactForceInProgress( const systems::Context& context) const; - // Updates the discrete_input_forces cache entry. This should only be called - // at the beginning of each discrete update. - // @throws std::exception if caching is disabled for the given `context`. + /* Updates the discrete_input_forces cache entry. This should only be called + at the beginning of each discrete update. + @throws std::exception if caching is disabled for the given `context`. */ void SampleDiscreteInputPortForces(const systems::Context& context) const; - // Collects the sum of all forces added to the owning MultibodyPlant and store - // them in given `forces`. The existing values in `forces` is cleared. + /* Collects the sum of all forces added to the owning MultibodyPlant and store + them in given `forces`. The existing values in `forces` is cleared. */ void CalcInputPortForces(const systems::Context& context, InputPortForces* forces) const; - // NVI to DoDeclareCacheEntries(). + /* NVI to DoDeclareCacheEntries(). */ void DeclareCacheEntries(); /* Calc version of EvalContactResults(), NVI to DoCalcContactResults(). */ void CalcContactResults(const systems::Context& context, ContactResults* contact_results) const; - // Calc version of EvalDiscreteUpdateMultibodyForces, NVI to - // DoCalcDiscreteUpdateMultibodyForces. + /* Calc version of EvalDiscreteUpdateMultibodyForces, NVI to + DoCalcDiscreteUpdateMultibodyForces. */ void CalcDiscreteUpdateMultibodyForces(const systems::Context& context, MultibodyForces* forces) const; + /* Calc version of EvalContactKinematics(). */ + void CalcContactKinematics( + const systems::Context& context, + DiscreteContactData>* result) const; + + /* Helper function for CalcContactKinematics() that computes the contact pair + kinematics for point contact and hydroelastic contact respectively, + depending on the value of `type`. */ + void AppendContactKinematics( + const systems::Context& context, + const std::vector>& contact_pairs, + DiscreteContactType type, + DiscreteContactData>* contact_kinematics) const; + + /* Calc version of EvalDiscreteContactPairs(). */ + void CalcDiscreteContactPairs( + const systems::Context& context, + DiscreteContactData>* result) const; + + /* Given the configuration stored in `context`, this method appends discrete + pairs corresponding to point contact into `pairs`. + @pre pairs != nullptr. */ + void AppendDiscreteContactPairsForPointContact( + const systems::Context& context, + DiscreteContactData>* pairs) const; + + /* Given the configuration stored in `context`, this method appends discrete + pairs corresponding to hydroelastic contact into `pairs`. + @pre pairs != nullptr. */ + void AppendDiscreteContactPairsForHydroelasticContact( + const systems::Context& context, + DiscreteContactData>* pairs) const; + const MultibodyPlant* plant_{nullptr}; MultibodyPlant* mutable_plant_{nullptr}; systems::DiscreteStateIndex multibody_state_index_; CacheIndexes cache_indexes_; + /* deformable_driver_ computes the information on all deformable bodies needed + to advance the discrete states. */ + std::unique_ptr> deformable_driver_; }; +/* N.B. These geometry queries are not supported when T = symbolic::Expression + and therefore their implementation throws. */ +template <> +void DiscreteUpdateManager::CalcDiscreteContactPairs( + const drake::systems::Context&, + DiscreteContactData>*) const; +template <> +void DiscreteUpdateManager:: + AppendDiscreteContactPairsForHydroelasticContact( + const drake::systems::Context&, + DiscreteContactData>*) const; + } // namespace internal } // namespace multibody } // namespace drake diff --git a/multibody/plant/sap_driver.cc b/multibody/plant/sap_driver.cc index 999b98519ce8..f9ab1d6f0604 100644 --- a/multibody/plant/sap_driver.cc +++ b/multibody/plant/sap_driver.cc @@ -151,8 +151,8 @@ void SapDriver::CalcLinearDynamicsMatrix(const systems::Context& context, } if constexpr (std::is_same_v) { - if (manager().deformable_driver_ != nullptr) { - manager().deformable_driver_->AppendLinearDynamicsMatrix(context, A); + if (manager().deformable_driver() != nullptr) { + manager().deformable_driver()->AppendLinearDynamicsMatrix(context, A); } } } @@ -174,9 +174,9 @@ void SapDriver::CalcFreeMotionVelocities(const systems::Context& context, context.get_discrete_state(manager().multibody_state_index()).value(); const auto v0 = x0.bottomRows(this->plant().num_velocities()); if constexpr (std::is_same_v) { - if (manager().deformable_driver_ != nullptr) { + if (manager().deformable_driver() != nullptr) { const VectorX& deformable_v_star = - manager().deformable_driver_->EvalParticipatingFreeMotionVelocities( + manager().deformable_driver()->EvalParticipatingFreeMotionVelocities( context); const int rigid_dofs = v0.size(); const int deformable_dofs = deformable_v_star.size(); @@ -726,11 +726,12 @@ void SapDriver::AddFixedConstraints( contact_solvers::internal::SapContactProblem* problem) const { DRAKE_DEMAND(problem != nullptr); if constexpr (std::is_same_v) { - if (manager().deformable_driver_ != nullptr) { + if (manager().deformable_driver() != nullptr) { std::vector> kinematics; manager() - .deformable_driver_->AppendDeformableRigidFixedConstraintKinematics( - context, &kinematics); + .deformable_driver() + ->AppendDeformableRigidFixedConstraintKinematics(context, + &kinematics); for (FixedConstraintKinematics& k : kinematics) { problem->AddConstraint( std::make_unique>(std::move(k))); @@ -750,9 +751,9 @@ void SapDriver::CalcContactProblemCache( CalcFreeMotionVelocities(context, &v_star); const int num_rigid_bodies = plant().num_bodies(); const int num_deformable_bodies = - (manager().deformable_driver_ == nullptr) + (manager().deformable_driver() == nullptr) ? 0 - : manager().deformable_driver_->num_deformable_bodies(); + : manager().deformable_driver()->num_deformable_bodies(); const int num_objects = num_rigid_bodies + num_deformable_bodies; cache->sap_problem = std::make_unique>( plant().time_step(), std::move(A), std::move(v_star)); @@ -851,7 +852,7 @@ void SapDriver::AddCliqueContribution( EigenPtr> values) const { if (clique >= tree_topology().num_trees()) { const DeformableDriver* deformable_driver = - manager().deformable_driver_.get(); + manager().deformable_driver(); DRAKE_THROW_UNLESS(deformable_driver != nullptr); if constexpr (std::is_same_v) { const int num_deformable_dofs = values->size() - plant().num_velocities(); @@ -896,9 +897,9 @@ void SapDriver::CalcSapSolverResults( } if constexpr (std::is_same_v) { - if (manager().deformable_driver_ != nullptr) { + if (manager().deformable_driver() != nullptr) { const VectorX deformable_v0 = - manager().deformable_driver_->EvalParticipatingVelocities(context); + manager().deformable_driver()->EvalParticipatingVelocities(context); const int rigid_dofs = v0.size(); const int deformable_dofs = deformable_v0.size(); v0.conservativeResize(rigid_dofs + deformable_dofs); @@ -994,7 +995,7 @@ void SapDriver::CalcDiscreteUpdateMultibodyForces( // Include the contribution from constraints. // TODO(amcastro-tri): Consider deformables. - if (manager().deformable_driver_ != nullptr) { + if (manager().deformable_driver() != nullptr) { throw std::logic_error( "The computation of MultibodyForces must be updated to include " "deformable objects."); diff --git a/multibody/plant/test/compliant_contact_manager_test.cc b/multibody/plant/test/compliant_contact_manager_test.cc index 1bd05a55e9fd..f5f6d748ae77 100644 --- a/multibody/plant/test/compliant_contact_manager_test.cc +++ b/multibody/plant/test/compliant_contact_manager_test.cc @@ -87,7 +87,7 @@ class SapDriverTest { // cause a DeformableDriver to be instantiated in the manager. GTEST_TEST(CompliantContactManagerTest, ExtractModelInfo) { CompliantContactManager manager; - EXPECT_EQ(CompliantContactManagerTester::deformable_driver(manager), nullptr); + EXPECT_EQ(manager.deformable_driver(), nullptr); MultibodyPlant plant(0.01); auto deformable_model = std::make_unique>(&plant); plant.AddPhysicalModel(std::move(deformable_model)); @@ -98,9 +98,7 @@ GTEST_TEST(CompliantContactManagerTest, ExtractModelInfo) { const CompliantContactManager* contact_manager_ptr = contact_manager.get(); plant.SetDiscreteUpdateManager(std::move(contact_manager)); - EXPECT_NE( - CompliantContactManagerTester::deformable_driver(*contact_manager_ptr), - nullptr); + EXPECT_NE(contact_manager_ptr->deformable_driver(), nullptr); } // TODO(DamrongGuoy): Simplify the test fixture somehow (need discussion diff --git a/multibody/plant/test/compliant_contact_manager_tester.h b/multibody/plant/test/compliant_contact_manager_tester.h index b5e601513581..bc2654eca90b 100644 --- a/multibody/plant/test/compliant_contact_manager_tester.h +++ b/multibody/plant/test/compliant_contact_manager_tester.h @@ -51,7 +51,7 @@ class CompliantContactManagerTester { static DiscreteContactData> CalcContactKinematics(const CompliantContactManager& manager, const drake::systems::Context& context) { - return manager.CalcContactKinematics(context); + return manager.EvalContactKinematics(context); } static void DoCalcContactResults( @@ -61,11 +61,6 @@ class CompliantContactManagerTester { return manager.DoCalcContactResults(context, contact_results); } - static const DeformableDriver* deformable_driver( - const CompliantContactManager& manager) { - return manager.deformable_driver_.get(); - } - static const SapDriver& sap_driver( const CompliantContactManager& manager) { DRAKE_DEMAND(manager.sap_driver_ != nullptr); diff --git a/multibody/plant/test/deformable_boundary_condition_test.cc b/multibody/plant/test/deformable_boundary_condition_test.cc index b374fd5367c3..b66d8dc555fd 100644 --- a/multibody/plant/test/deformable_boundary_condition_test.cc +++ b/multibody/plant/test/deformable_boundary_condition_test.cc @@ -29,16 +29,6 @@ namespace drake { namespace multibody { namespace internal { -/* Provides access to a selection of private functions in - CompliantContactManager for testing purposes. */ -class CompliantContactManagerTester { - public: - static const DeformableDriver* deformable_driver( - const CompliantContactManager& manager) { - return manager.deformable_driver_.get(); - } -}; - /* Deformable body parameters. */ constexpr double kRadius = 0.1; // unit: m constexpr double kYoungsModulus = 2e3; // unit: N/m² @@ -85,7 +75,8 @@ class DeformableIntegrationTest : public ::testing::Test { auto contact_manager = make_unique>(); manager_ = contact_manager.get(); plant_->SetDiscreteUpdateManager(std::move(contact_manager)); - driver_ = CompliantContactManagerTester::deformable_driver(*manager_); + driver_ = manager_->deformable_driver(); + DRAKE_DEMAND(driver_ != nullptr); /* Connect visualizer. Useful for when this test is used for debugging. */ geometry::DrakeVisualizerd::AddToBuilder(&builder, *scene_graph_); diff --git a/multibody/plant/test/deformable_driver_contact_kinematics_test.cc b/multibody/plant/test/deformable_driver_contact_kinematics_test.cc index e7869195e3f8..c2ac851985f2 100644 --- a/multibody/plant/test/deformable_driver_contact_kinematics_test.cc +++ b/multibody/plant/test/deformable_driver_contact_kinematics_test.cc @@ -5,6 +5,7 @@ #include "drake/math/roll_pitch_yaw.h" #include "drake/multibody/plant/compliant_contact_manager.h" #include "drake/multibody/plant/deformable_driver.h" +#include "drake/multibody/plant/multibody_plant.h" #include "drake/systems/framework/diagram_builder.h" using drake::geometry::GeometryId; @@ -30,16 +31,6 @@ namespace drake { namespace multibody { namespace internal { -/* Friend class used to provide access to a selection of private functions in - CompliantContactManager for testing purposes. */ -class CompliantContactManagerTester { - public: - static const DeformableDriver* deformable_driver( - const CompliantContactManager& manager) { - return manager.deformable_driver_.get(); - } -}; - /* Registers a deformable octrahedron with the given `name` and pose in the world to the given `model`. The octahedron looks like this in its geometry frame, F. @@ -156,7 +147,8 @@ class DeformableDriverContactKinematicsTest auto contact_manager = make_unique>(); manager_ = contact_manager.get(); plant_->SetDiscreteUpdateManager(std::move(contact_manager)); - driver_ = CompliantContactManagerTester::deformable_driver(*manager_); + driver_ = manager_->deformable_driver(); + DRAKE_DEMAND(driver_ != nullptr); builder.Connect(model_->vertex_positions_port(), scene_graph_->get_source_configuration_port( @@ -443,8 +435,8 @@ GTEST_TEST(DeformableDriverContactKinematicsWithBcTest, auto contact_manager = make_unique>(); const CompliantContactManager* manager = contact_manager.get(); plant.SetDiscreteUpdateManager(std::move(contact_manager)); - const DeformableDriver* driver = - CompliantContactManagerTester::deformable_driver(*manager); + const DeformableDriver* driver = manager->deformable_driver(); + ASSERT_NE(driver, nullptr); builder.Connect( model->vertex_positions_port(), @@ -499,8 +491,8 @@ GTEST_TEST(DeformableDriverConstraintParticipation, ConstraintWithoutContact) { auto contact_manager = make_unique>(); const CompliantContactManager* manager = contact_manager.get(); plant.SetDiscreteUpdateManager(std::move(contact_manager)); - const DeformableDriver* driver = - CompliantContactManagerTester::deformable_driver(*manager); + const DeformableDriver* driver = manager->deformable_driver(); + ASSERT_NE(driver, nullptr); // TODO(xuchenhan-tri): AddMultibodyPlant and AddMultibodyPlantSceneGraph // should connect this port automatically. builder.Connect( diff --git a/multibody/plant/test/deformable_driver_contact_test.cc b/multibody/plant/test/deformable_driver_contact_test.cc index 825f990f1722..d21d5f8e690b 100644 --- a/multibody/plant/test/deformable_driver_contact_test.cc +++ b/multibody/plant/test/deformable_driver_contact_test.cc @@ -5,6 +5,7 @@ #include "drake/multibody/fem/matrix_utilities.h" #include "drake/multibody/plant/compliant_contact_manager.h" #include "drake/multibody/plant/deformable_driver.h" +#include "drake/multibody/plant/multibody_plant.h" #include "drake/systems/framework/diagram_builder.h" using drake::geometry::GeometryId; @@ -30,16 +31,6 @@ namespace drake { namespace multibody { namespace internal { -// Friend class used to provide access to a selection of private functions in -// CompliantContactManager for testing purposes. -class CompliantContactManagerTester { - public: - static const DeformableDriver* deformable_driver( - const CompliantContactManager& manager) { - return manager.deformable_driver_.get(); - } -}; - /* This fixture tests DeformableDriver member functions associated with the concept of contact. In particular, it sets up two identical and overlapping deformable octahedron bodies centered at world origin, each with 8 elements, @@ -81,7 +72,8 @@ class DeformableDriverContactTest : public ::testing::Test { auto contact_manager = make_unique>(); manager_ = contact_manager.get(); plant_->SetDiscreteUpdateManager(std::move(contact_manager)); - driver_ = CompliantContactManagerTester::deformable_driver(*manager_); + driver_ = manager_->deformable_driver(); + DRAKE_DEMAND(driver_ != nullptr); builder.Connect(model_->vertex_positions_port(), scene_graph_->get_source_configuration_port( diff --git a/multibody/plant/test/deformable_driver_test.cc b/multibody/plant/test/deformable_driver_test.cc index 840e3f9a455d..31f762e804d1 100644 --- a/multibody/plant/test/deformable_driver_test.cc +++ b/multibody/plant/test/deformable_driver_test.cc @@ -6,7 +6,7 @@ #include "drake/common/test_utilities/expect_throws_message.h" #include "drake/geometry/proximity_properties.h" #include "drake/multibody/plant/compliant_contact_manager.h" -#include "drake/multibody/plant/test/compliant_contact_manager_tester.h" +#include "drake/multibody/plant/multibody_plant.h" #include "drake/systems/analysis/simulator.h" #include "drake/systems/framework/diagram_builder.h" @@ -44,7 +44,8 @@ class DeformableDriverTest : public ::testing::Test { auto contact_manager = make_unique>(); manager_ = contact_manager.get(); plant_->SetDiscreteUpdateManager(std::move(contact_manager)); - driver_ = CompliantContactManagerTester::deformable_driver(*manager_); + driver_ = manager_->deformable_driver(); + DRAKE_DEMAND(driver_ != nullptr); builder.Connect(model_->vertex_positions_port(), scene_graph_->get_source_configuration_port( diff --git a/multibody/plant/test/deformable_integration_test.cc b/multibody/plant/test/deformable_integration_test.cc index 354ad5f3020d..86c9878d4ffa 100644 --- a/multibody/plant/test/deformable_integration_test.cc +++ b/multibody/plant/test/deformable_integration_test.cc @@ -34,16 +34,6 @@ namespace drake { namespace multibody { namespace internal { -/* Provides access to a selection of private functions in - CompliantContactManager for testing purposes. */ -class CompliantContactManagerTester { - public: - static const DeformableDriver* deformable_driver( - const CompliantContactManager& manager) { - return manager.deformable_driver_.get(); - } -}; - /* Deformable body parameters. */ constexpr double kYoungsModulus = 1e5; // unit: N/m² constexpr double kPoissonsRatio = 0.4; // unitless. @@ -101,7 +91,8 @@ class DeformableIntegrationTest : public ::testing::Test { auto contact_manager = make_unique>(); manager_ = contact_manager.get(); plant_->SetDiscreteUpdateManager(std::move(contact_manager)); - driver_ = CompliantContactManagerTester::deformable_driver(*manager_); + driver_ = manager_->deformable_driver(); + DRAKE_DEMAND(driver_ != nullptr); /* Connect visualizer. Useful for when this test is used for debugging. */ geometry::DrakeVisualizerd::AddToBuilder(&builder, *scene_graph_); diff --git a/multibody/plant/test/deformable_model_test.cc b/multibody/plant/test/deformable_model_test.cc index 6569a1e85c7f..03737c56b159 100644 --- a/multibody/plant/test/deformable_model_test.cc +++ b/multibody/plant/test/deformable_model_test.cc @@ -3,6 +3,7 @@ #include #include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/multibody/plant/multibody_plant.h" #include "drake/systems/framework/diagram_builder.h" namespace drake { diff --git a/multibody/plant/test/discrete_update_manager_test.cc b/multibody/plant/test/discrete_update_manager_test.cc index 0793cf3794eb..53254f794db8 100644 --- a/multibody/plant/test/discrete_update_manager_test.cc +++ b/multibody/plant/test/discrete_update_manager_test.cc @@ -11,7 +11,6 @@ #include "drake/systems/framework/abstract_value_cloner.h" #include "drake/systems/primitives/pass_through.h" #include "drake/systems/primitives/zero_order_hold.h" - namespace drake { namespace multibody { namespace internal { @@ -91,7 +90,7 @@ class DummyDiscreteUpdateManager final : public DiscreteUpdateManager { /* Extracts information about the additional discrete state that DummyModel declares if one exists in the owning MultibodyPlant. */ - void ExtractModelInfo() final { + void DoExtractModelInfo() final { /* For unit testing we verify there is a single physical model of type DummyModel. */ DRAKE_DEMAND(this->plant().physical_models().size() == 1); diff --git a/multibody/plant/test/multibody_plant_scalar_conversion_test.cc b/multibody/plant/test/multibody_plant_scalar_conversion_test.cc index ceaa94f1cb09..07776849af5b 100644 --- a/multibody/plant/test/multibody_plant_scalar_conversion_test.cc +++ b/multibody/plant/test/multibody_plant_scalar_conversion_test.cc @@ -3,6 +3,7 @@ #include +#include "drake/multibody/plant/discrete_contact_data.h" #include "drake/multibody/plant/discrete_update_manager.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/plant/test/dummy_model.h" @@ -184,12 +185,12 @@ class DoubleOnlyDiscreteUpdateManager final const systems::Context&, internal::AccelerationKinematicsCache*) const final {} - void DoCalcDiscreteValues(const systems::Context&, - systems::DiscreteValues*) const final {} - void DoCalcContactResults(const systems::Context&, ContactResults*) const final {} + void DoCalcDiscreteValues(const systems::Context&, + systems::DiscreteValues*) const final {} + void DoCalcDiscreteUpdateMultibodyForces( const systems::Context& context, MultibodyForces* forces) const final {}