Propagate change to DeformableModel

examples/multibody/deformable/deformable_torus.cc

@@ -90,18 +90,18 @@ ModelInstanceIndex AddSuctionGripper(
                                      Vector4d(0.9, 0.1, 0.1, 0.8));
   plant->RegisterVisualGeometry(body, RigidTransformd::Identity(), capsule,
                                 "cup_visual", cup_illustration_props);
-  /* Add a visual hint for the center of the suction force source. */
-  const Sphere sphere{0.0075};
+  /* Add a visual hint for the support of the suction force source. */
+  const Sphere sphere{0.01};
   IllustrationProperties source_illustration_props;
   source_illustration_props.AddProperty("phong", "diffuse",
                                         Vector4d(0.1, 0.9, 0.1, 0.8));
-  plant->RegisterVisualGeometry(body, RigidTransformd(Vector3d(0, 0, -0.07)),
+  plant->RegisterVisualGeometry(body, RigidTransformd(Vector3d(0, 0, -0.065)),
                                 sphere, "source_visual",
                                 source_illustration_props);
   plant->RegisterCollisionGeometry(body, RigidTransformd::Identity(), capsule,
                                    "cup_collision", proximity_props);
   /* Adds an actuated joint between the suction cup body and the world. */
-  const RigidTransformd X_WF(Vector3d(0.04, 0, -0.05));
+  const RigidTransformd X_WF(Vector3d(0.05, 0, -0.05));
   const auto& prismatic_joint = plant->AddJoint<PrismaticJoint>(
       "translate_z_joint", plant->world_body(), X_WF, body, std::nullopt,
       Vector3d::UnitZ());
@@ -188,6 +188,9 @@ int do_main() {
   deformable_config.set_poissons_ratio(FLAGS_nu);
   deformable_config.set_mass_density(FLAGS_density);
   deformable_config.set_stiffness_damping_coefficient(FLAGS_beta);
+  /* Subdivide the element for suction force integration; otherwise, the mesh is
+   too coarse for the support of the suction force field (1cm radius). */
+  deformable_config.set_element_subdivision(2);
 
   /* Load the geometry and scale it down to 65% (to showcase the scaling
    capability and to make the torus suitable for grasping by the gripper). */
@@ -206,7 +209,7 @@ int do_main() {
   const PointSourceForceField* suction_force_ptr{nullptr};
   if (use_suction) {
     auto suction_force = std::make_unique<PointSourceForceField>(
-        plant, plant.GetBodyByName("cup_body"), Vector3d(0, 0, -0.07), 0.1);
+        plant, plant.GetBodyByName("cup_body"), Vector3d(0, 0, -0.065), 0.01);
     suction_force_ptr = suction_force.get();
     deformable_model.AddExternalForce(std::move(suction_force));
   }

examples/multibody/deformable/suction_cup_controller.cc

@@ -60,9 +60,9 @@ void SuctionCupController::CalcMaxForceDensity(
     BasicVector<double>* max_force_density) const {
   const double time = context.get_time();
   if (time >= start_suction_time_ && time <= release_suction_time_) {
-    // An arbitrary value that's reasonable for picking up the deformable torus
-    // in the example with density comparable to water.
-    (*max_force_density)[0] = 2.0e5;
+    /* An arbitrary value that's reasonable for picking up the deformable torus
+     in the example with density comparable to water. */
+    (*max_force_density)[0] = 2.0e8;
   } else {
     (*max_force_density)[0] = 0.0;
   }

multibody/fem/deformable_body_config.h

@@ -2,8 +2,8 @@
 
 #include <utility>
 
-#include "drake/common/drake_assert.h"
 #include "drake/common/drake_copyable.h"
+#include "drake/common/drake_throw.h"
 
 namespace drake {
 namespace multibody {
@@ -63,38 +63,45 @@ class DeformableBodyConfig {
 
   /** @pre youngs_modulus > 0. */
   void set_youngs_modulus(T youngs_modulus) {
-    DRAKE_DEMAND(youngs_modulus > 0);
+    DRAKE_THROW_UNLESS(youngs_modulus > 0);
     youngs_modulus_ = std::move(youngs_modulus);
   }
 
   /** @pre -1 < poissons_ratio < 0.5. */
   void set_poissons_ratio(T poissons_ratio) {
-    DRAKE_DEMAND(-1 < poissons_ratio && poissons_ratio < 0.5);
+    DRAKE_THROW_UNLESS(-1 < poissons_ratio && poissons_ratio < 0.5);
     poissons_ratio_ = std::move(poissons_ratio);
   }
 
   /** @pre mass_damping_coefficient >= 0. */
   void set_mass_damping_coefficient(T mass_damping_coefficient) {
-    DRAKE_DEMAND(mass_damping_coefficient_ >= 0);
+    DRAKE_THROW_UNLESS(mass_damping_coefficient_ >= 0);
     mass_damping_coefficient_ = std::move(mass_damping_coefficient);
   }
 
   /**  @pre stiffness_damping_coefficient >= 0. */
   void set_stiffness_damping_coefficient(T stiffness_damping_coefficient) {
-    DRAKE_DEMAND(stiffness_damping_coefficient_ >= 0);
+    DRAKE_THROW_UNLESS(stiffness_damping_coefficient_ >= 0);
     stiffness_damping_coefficient_ = std::move(stiffness_damping_coefficient);
   }
 
   /**  @pre mass_density > 0. */
   void set_mass_density(T mass_density) {
-    DRAKE_DEMAND(mass_density > 0);
+    DRAKE_THROW_UNLESS(mass_density > 0);
     mass_density_ = std::move(mass_density);
   }
 
   void set_material_model(MaterialModel material_model) {
     material_model_ = material_model;
   }
 
+  /** @pre 0 <= element_subdivision <= 4. */
+  void set_element_subdivision(int element_subdivision) {
+    DRAKE_THROW_UNLESS(element_subdivision >= 0);
+    DRAKE_THROW_UNLESS(element_subdivision <= 4);
+    element_subdivision_ = element_subdivision;
+  }
+
   /** Returns the Young's modulus, with unit of N/m². */
   const T& youngs_modulus() const { return youngs_modulus_; }
   /** Returns the Poisson's ratio, unitless. */
@@ -111,6 +118,10 @@ class DeformableBodyConfig {
   const T& mass_density() const { return mass_density_; }
   /** Returns the constitutive model of the material. */
   MaterialModel material_model() const { return material_model_; }
+  /** Returns the number of times each element is subdivided when evaluating the
+   external forces. Useful when elements are too coarse to resolve external
+   force fields. */
+  int element_subdivision() const { return element_subdivision_; }
 
  private:
   T youngs_modulus_{1e8};
@@ -119,6 +130,7 @@ class DeformableBodyConfig {
   T stiffness_damping_coefficient_{0};
   T mass_density_{1.5e3};
   MaterialModel material_model_{MaterialModel::kLinearCorotated};
+  int element_subdivision_{0};
 };
 
 }  // namespace fem

multibody/fem/linear_simplex_element.cc

@@ -144,7 +144,7 @@ template class LinearSimplexElement<double, 3, 3, 5>;
 template class LinearSimplexElement<double, 3, 3, 4>;
 template class LinearSimplexElement<double, 3, 3, 16>;
 template class LinearSimplexElement<double, 3, 3, 64>;
-template class LinearSimplexElement<double, 3, 3, 128>;
+template class LinearSimplexElement<double, 3, 3, 256>;
 
 template class LinearSimplexElement<AutoDiffXd, 2, 2, 2>;
 template class LinearSimplexElement<AutoDiffXd, 2, 2, 4>;

multibody/fem/test/volumetric_element_external_force_test.cc

@@ -55,8 +55,6 @@ using DeformationGradientDataType =
 const double kYoungsModulus{1};
 const double kPoissonRatio{0.25};
 const double kDensity{1.23};
-const double kMassDamping{1e-4};
-const double kStiffnessDamping{1e-3};
 
 class VolumetricElementTest : public ::testing::Test {
  protected:

multibody/fem/volumetric_model.h

@@ -33,7 +33,9 @@ class VolumetricModel : public FemModelImpl<Element> {
   static_assert(
       std::is_same_v<
           VolumetricElement<typename Element::IsoparametricElement,
-                            typename Element::Quadrature, ConstitutiveModel>,
+                            typename Element::Quadrature, ConstitutiveModel,
+                            typename Element::SubdIsoparametricElement,
+                            typename Element::SubdQuadrature>,
           Element>,
       "The template parameter `Element` must be of type VolumetricElement.");
 

multibody/plant/deformable_model.cc

@@ -10,6 +10,7 @@
 #include "drake/multibody/fem/linear_corotated_model.h"
 #include "drake/multibody/fem/linear_simplex_element.h"
 #include "drake/multibody/fem/simplex_gaussian_quadrature.h"
+#include "drake/multibody/fem/tet_subdivision_quadrature.h"
 #include "drake/multibody/fem/volumetric_model.h"
 #include "drake/multibody/plant/multibody_plant.h"
 
@@ -352,24 +353,48 @@ DeformableModel<T>::BuildLinearVolumetricModel(
   }
   switch (config.material_model()) {
     case MaterialModel::kLinear:
-      BuildLinearVolumetricModelHelper<fem::internal::LinearConstitutiveModel>(
-          id, mesh, config);
+      ConstitutiveModelHelper<fem::internal::LinearConstitutiveModel>(id, mesh,
+                                                                      config);
       break;
     case MaterialModel::kCorotated:
-      BuildLinearVolumetricModelHelper<fem::internal::CorotatedModel>(id, mesh,
-                                                                      config);
+      ConstitutiveModelHelper<fem::internal::CorotatedModel>(id, mesh, config);
       break;
     case MaterialModel::kLinearCorotated:
-      BuildLinearVolumetricModelHelper<fem::internal::LinearCorotatedModel>(
-          id, mesh, config);
+      ConstitutiveModelHelper<fem::internal::LinearCorotatedModel>(id, mesh,
+                                                                   config);
       break;
   }
 }
 
 template <typename T>
 template <template <typename> class Model, typename T1>
 typename std::enable_if_t<std::is_same_v<T1, double>, void>
-DeformableModel<T>::BuildLinearVolumetricModelHelper(
+DeformableModel<T>::ConstitutiveModelHelper(
+    DeformableBodyId id, const geometry::VolumeMesh<double>& mesh,
+    const fem::DeformableBodyConfig<T>& config) {
+  switch (config.element_subdivision()) {
+    case 0:
+      SubdElementlHelper<Model, 0>(id, mesh, config);
+      break;
+    case 1:
+      SubdElementlHelper<Model, 1>(id, mesh, config);
+      break;
+    case 2:
+      SubdElementlHelper<Model, 2>(id, mesh, config);
+      break;
+    case 3:
+      SubdElementlHelper<Model, 3>(id, mesh, config);
+      break;
+    case 4:
+      SubdElementlHelper<Model, 4>(id, mesh, config);
+      break;
+  }
+}
+
+template <typename T>
+template <template <typename> class Model, int num_subd, typename T1>
+typename std::enable_if_t<std::is_same_v<T1, double>, void>
+DeformableModel<T>::SubdElementlHelper(
     DeformableBodyId id, const geometry::VolumeMesh<double>& mesh,
     const fem::DeformableBodyConfig<T>& config) {
   constexpr int kNaturalDimension = 3;
@@ -383,16 +408,24 @@ DeformableModel<T>::BuildLinearVolumetricModelHelper(
       fem::internal::LinearSimplexElement<T, kNaturalDimension,
                                           kSpatialDimension, kNumQuads>;
   using ConstitutiveModelType = Model<T>;
+  using SubdQuadratureType =
+      std::conditional_t<(num_subd <= 0), QuadratureType,
+                         fem::internal::TetSubdivisionQuadrature<num_subd>>;
+  using SubdIsoparametricElementType =
+      std::conditional_t<(num_subd <= 0), IsoparametricElementType,
+                         fem::internal::LinearSimplexElement<
+                             T, kNaturalDimension, kSpatialDimension,
+                             SubdQuadratureType::num_quadrature_points>>;
   static_assert(
       std::is_base_of_v<
           fem::internal::ConstitutiveModel<
               ConstitutiveModelType, typename ConstitutiveModelType::Traits>,
           ConstitutiveModelType>,
       "The template parameter 'Model' must be derived from "
       "ConstitutiveModel.");
-  using FemElementType =
-      fem::internal::VolumetricElement<IsoparametricElementType, QuadratureType,
-                                       ConstitutiveModelType>;
+  using FemElementType = fem::internal::VolumetricElement<
+      IsoparametricElementType, QuadratureType, ConstitutiveModelType,
+      SubdIsoparametricElementType, SubdQuadratureType>;
   using FemModelType = fem::internal::VolumetricModel<FemElementType>;
 
   const fem::DampingModel<T> damping_model(

multibody/plant/deformable_model.h

@@ -344,11 +344,21 @@ class DeformableModel final : public multibody::PhysicalModel<T> {
                              const geometry::VolumeMesh<double>& mesh,
                              const fem::DeformableBodyConfig<T>& config);
 
+  /* Helper function for BuildLinearVolumetricModel that turns
+   element_subdivision to a template parameter. */
+  template <template <class> class Model, int num_subd, typename T1 = T>
+  typename std::enable_if_t<std::is_same_v<T1, double>, void>
+  SubdElementlHelper(DeformableBodyId id,
+                     const geometry::VolumeMesh<double>& mesh,
+                     const fem::DeformableBodyConfig<T>& config);
+
+  /* Helper function for BuildLinearVolumetricModel that turns
+   constitutive model to a template parameter. */
   template <template <class> class Model, typename T1 = T>
   typename std::enable_if_t<std::is_same_v<T1, double>, void>
-  BuildLinearVolumetricModelHelper(DeformableBodyId id,
-                                   const geometry::VolumeMesh<double>& mesh,
-                                   const fem::DeformableBodyConfig<T>& config);
+  ConstitutiveModelHelper(DeformableBodyId id,
+                          const geometry::VolumeMesh<double>& mesh,
+                          const fem::DeformableBodyConfig<T>& config);
 
   /* Copies the vertex positions of all deformable bodies to the output port
    value which is guaranteed to be of type GeometryConfigurationVector. */