[fem] Allow additional quadrature points for external force integration

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/BUILD.bazel

@@ -42,6 +42,7 @@ drake_cc_package_library(
         ":quadrature",
         ":schur_complement",
         ":simplex_gaussian_quadrature",
+        ":tet_subdivision_quadrature",
         ":velocity_newmark_scheme",
         ":volumetric_element",
         ":volumetric_model",
@@ -453,6 +454,19 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "tet_subdivision_quadrature",
+    srcs = [
+        "tet_subdivision_quadrature.cc",
+    ],
+    hdrs = [
+        "tet_subdivision_quadrature.h",
+    ],
+    deps = [
+        ":quadrature",
+    ],
+)
+
 drake_cc_library(
     name = "velocity_newmark_scheme",
     srcs = [
@@ -751,6 +765,13 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "tet_subdivision_quadrature_test",
+    deps = [
+        ":tet_subdivision_quadrature",
+    ],
+)
+
 drake_cc_googletest(
     name = "volumetric_element_test",
     deps = [
@@ -765,6 +786,19 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "volumetric_element_external_force_test",
+    deps = [
+        ":corotated_model",
+        ":linear_simplex_element",
+        ":simplex_gaussian_quadrature",
+        ":tet_subdivision_quadrature",
+        ":volumetric_element",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//multibody/plant",
+    ],
+)
+
 drake_cc_googletest(
     name = "velocity_newmark_scheme_test",
     deps = [

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

@@ -132,6 +132,20 @@ template class LinearSimplexElement<double, 2, 3, 4>;
 template class LinearSimplexElement<double, 3, 3, 1>;
 template class LinearSimplexElement<double, 3, 3, 2>;
 template class LinearSimplexElement<double, 3, 3, 5>;
+
+// TODO(xuchenhan-tri): With the addition of TetSubdivisionQuadrature (which is
+// currently an open template), we need to either make LinearSimplexElement an
+// open template as well, or, make TetSubdivisionQuadrature a closed template
+// (and move stuff to cc file), and limit the number of subdivisions. As it
+// stands now, if we subdivide more than 4 times, the number of quadrature
+// points will exceed 128, and we will miss the necessary explicit instantiation
+// of LinearSimplexElement.
+/* Explicit instantiations for subdivions 1,2,3,4. */
+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, 256>;
+
 template class LinearSimplexElement<AutoDiffXd, 2, 2, 2>;
 template class LinearSimplexElement<AutoDiffXd, 2, 2, 4>;
 template class LinearSimplexElement<AutoDiffXd, 2, 3, 4>;

multibody/fem/test/tet_subdivision_quadrature_test.cc

@@ -0,0 +1,99 @@
+#include "drake/multibody/fem/tet_subdivision_quadrature.h"
+
+#include <gtest/gtest.h>
+
+namespace drake {
+namespace multibody {
+namespace fem {
+namespace internal {
+namespace {
+
+using Eigen::Vector3d;
+
+/* Arbitrary polynomial constants used in this file. */
+static constexpr double a0 = 1.0;
+static constexpr double a1 = 3.0;
+static constexpr double a2 = -2.0;
+static constexpr double a3 = -4.0;
+
+struct LinearTestFunction3D {
+  /* Returns the value of function
+       y(x₀, x₁, x₂) = a₀ + a₁ * x₀+ a₂ * x₁+ a₃ * x₂
+   evaluated at input x. */
+  static double Eval(const Vector3d& x) {
+    return a0 + a1 * x(0) + a2 * x(1) + a3 * x(2);
+  }
+
+  static double EvalAnalyticalIntegralOverUnitTet() {
+    /* The integral of the monomial 1 and x over the unit tetrahedron with end
+     points at (0,0,0), (1,0,0), (0,1,0) and (0,0,1) is
+         ∫₀¹∫₀¹⁻ˣ∫₀¹⁻ˣ⁻ʸ 1 dzdydx =  1/6.
+         ∫₀¹∫₀¹⁻ˣ∫₀¹⁻ˣ⁻ʸ x dzdydx =  1/24.
+     So the integral of f(x) = a₀ + a₁*x₀ + a₂*x₁ + a₃*x₂ is equal to
+         1/6 * a₀ + 1/24 * a₁ + 1/24 * a₂ + 1/24 * a₃. */
+    return 1.0 / 6.0 * a0 + 1.0 / 24.0 * a1 + 1.0 / 24.0 * a2 + 1.0 / 24.0 * a3;
+  }
+};
+
+template <typename Quad>
+double Integrate(const Quad& quadrature, double (*f)(const Vector3d&)) {
+  double numerical_integral = 0;
+  for (int i = 0; i < quadrature.num_quadrature_points; ++i) {
+    numerical_integral += quadrature.get_weight(i) * f(quadrature.get_point(i));
+  }
+  return numerical_integral;
+}
+
+class TetSubdivisionQuadratureTest : public ::testing::Test {
+ protected:
+  const TetSubdivisionQuadrature<0> subd_0_quadrature_;
+  const TetSubdivisionQuadrature<1> subd_1_quadrature_;
+  const TetSubdivisionQuadrature<2> subd_2_quadrature_;
+  const TetSubdivisionQuadrature<3> subd_3_quadrature_;
+};
+
+TEST_F(TetSubdivisionQuadratureTest, Weights) {
+  EXPECT_EQ(subd_0_quadrature_.num_quadrature_points, 1);
+  EXPECT_EQ(subd_1_quadrature_.num_quadrature_points, 4);
+  EXPECT_EQ(subd_2_quadrature_.num_quadrature_points, 16);
+  EXPECT_EQ(subd_3_quadrature_.num_quadrature_points, 64);
+  for (int i = 0; i < subd_0_quadrature_.num_quadrature_points; ++i) {
+    EXPECT_EQ(subd_0_quadrature_.get_weight(i), 1.0 / 6.0);
+  }
+  for (int i = 0; i < subd_1_quadrature_.num_quadrature_points; ++i) {
+    EXPECT_EQ(subd_1_quadrature_.get_weight(i), 1.0 / 24.0);
+  }
+  for (int i = 0; i < subd_2_quadrature_.num_quadrature_points; ++i) {
+    EXPECT_EQ(subd_2_quadrature_.get_weight(i), 1.0 / 96.0);
+  }
+  for (int i = 0; i < subd_3_quadrature_.num_quadrature_points; ++i) {
+    EXPECT_EQ(subd_3_quadrature_.get_weight(i), 1.0 / 384.0);
+  }
+}
+
+TEST_F(TetSubdivisionQuadratureTest, Locations) {
+  for (int i = 0; i < subd_0_quadrature_.num_quadrature_points; ++i) {
+    const Vector3d& p = subd_0_quadrature_.get_point(i);
+    EXPECT_EQ(p, Vector3d(0.25, 0.25, 0.25));
+  }
+  // TODO(xuchenhan-tri): Add more tests for non-zero subdivisions.
+}
+
+TEST_F(TetSubdivisionQuadratureTest, ReproducesLinearFunctions) {
+  const double analytical_integral =
+      LinearTestFunction3D::EvalAnalyticalIntegralOverUnitTet();
+  EXPECT_DOUBLE_EQ(analytical_integral,
+                   Integrate(subd_0_quadrature_, LinearTestFunction3D::Eval));
+  EXPECT_DOUBLE_EQ(analytical_integral,
+                   Integrate(subd_1_quadrature_, LinearTestFunction3D::Eval));
+  EXPECT_DOUBLE_EQ(analytical_integral,
+                   Integrate(subd_2_quadrature_, LinearTestFunction3D::Eval));
+  EXPECT_DOUBLE_EQ(analytical_integral,
+                   Integrate(subd_3_quadrature_, LinearTestFunction3D::Eval));
+}
+
+}  // namespace
+}  // namespace internal
+}  // namespace fem
+}  // namespace multibody
+}  // namespace drake