[FEM] Add SoftsimSystem and a large deformation demo (RobotLocomotion…

…#14741)

* Add SoftsimSystem and a large deformation demo

multibody/fixed_fem/dev/BUILD.bazel

@@ -64,6 +64,16 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "deformable_body_config",
+    hdrs = [
+        "deformable_body_config.h",
+    ],
+    deps = [
+        "//common:essential",
+    ],
+)
+
 drake_cc_library(
     name = "deformable_visualizer",
     srcs = ["deformable_visualizer.cc"],
@@ -146,6 +156,7 @@ drake_cc_library(
         ":isoparametric_element",
         ":quadrature",
         "//common:essential",
+        "//common:unused",
     ],
 )
 
@@ -205,6 +216,7 @@ drake_cc_library(
         ":fem_state",
         ":state_updater",
         "//common:essential",
+        "//common:unused",
     ],
 )
 
@@ -239,6 +251,30 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "softsim_system",
+    srcs = [
+        "softsim_system.cc",
+    ],
+    hdrs = [
+        "softsim_system.h",
+    ],
+    deps = [
+        ":corotated_model",
+        ":deformable_body_config",
+        ":dirichlet_boundary_condition",
+        ":dynamic_elasticity_element",
+        ":dynamic_elasticity_model",
+        ":fem_solver",
+        ":linear_constitutive_model",
+        ":linear_simplex_element",
+        ":simplex_gaussian_quadrature",
+        "//common:essential",
+        "//geometry/proximity:volume_mesh",
+        "//systems/framework:leaf_system",
+    ],
+)
+
 drake_cc_library(
     name = "fem_state",
     hdrs = [
@@ -384,6 +420,22 @@ drake_cc_binary(
     ],
 )
 
+drake_cc_binary(
+    name = "run_cantilever_beam",
+    srcs = [
+        "run_cantilever_beam.cc",
+    ],
+    deps = [
+        ":deformable_visualizer",
+        ":mesh_utilities",
+        ":softsim_system",
+        "//common:add_text_logging_gflags",
+        "//math:geometric_transform",
+        "//systems/analysis:simulator_gflags",
+        "//systems/framework:diagram_builder",
+    ],
+)
+
 drake_cc_library(
     name = "simplex_gaussian_quadrature",
     hdrs = [
@@ -401,6 +453,7 @@ drake_cc_library(
     ],
     deps = [
         ":fem_state",
+        "//common:unused",
     ],
 )
 
@@ -627,6 +680,16 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "softsim_system_test",
+    deps = [
+        ":softsim_system",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:expect_throws_message",
+        "//geometry/proximity:make_box_mesh",
+    ],
+)
+
 drake_cc_googletest(
     name = "static_elasticity_element_test",
     deps = [

multibody/fixed_fem/dev/deformable_body_config.h

@@ -0,0 +1,109 @@
+#pragma once
+
+#include <utility>
+
+#include "drake/common/drake_assert.h"
+#include "drake/common/drake_copyable.h"
+
+namespace drake {
+namespace multibody {
+namespace fixed_fem {
+/** Types of material models for the deformable body. */
+enum class MaterialModel {
+  /** Corotational model. Recommended for modeling large deformations. More
+   computationally expensive than the linear elasticity model. */
+  kCorotated,
+  /** Linear elasticity model. Recommended for modeling small deformations.
+   Less computationally expensive than non-linear models but is inaccurate
+   for large deformations. */
+  kLinear,
+};
+
+// TODO(xuchenhan-tri): Add unit tests for this class.
+/** %DeformableBodyConfig stores the physical parameters for a deformable body.
+ It contains the following fields with their corresponding valid ranges:
+ - Youngs modulus: Measures the stiffness of the material, has unit Pa. Must be
+   positive.
+ - Poisson ratio: Measures the Poisson effect (how much the material expands or
+   contracts in directions perpendicular to the direction of loading) of the
+   material, unitless. Must be greater than -1 and less than 0.5.
+ - Mass damping coefficient: Controls the strength of mass damping. The damping
+   ratio contributed by mass damping is inversely proportional to the frequency
+   of the motion. Must be non-negative.
+ - Stiffness damping coefficient: Controls the strength of stiffness damping.
+   The damping ratio contributed by stiffness damping is proportional to the
+   frequency of the motion. Must be non-negative.
+ - Mass density: Has unit kg/m³. Must be positive. Default to 1e3.
+ - Material model: The constitutive model that describes the stress-strain
+   relationship of the body, see MaterialModel. Must not be
+   MaterialModel::Invalid. Default to MaterialModel::kCorotated.
+
+ A default constructed configuration approximately represents a hard rubber
+ material (density, elasticity, and poisson's ratio) without any damping.
+ Damping coefficients are generally difficult to measure and we expect users
+ will typically start with zero damping and tune the values to achieve
+ reasonable dynamics. */
+template <typename T>
+class DeformableBodyConfig {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(DeformableBodyConfig);
+  DeformableBodyConfig() = default;
+
+  /* Setters. */
+  /** @pre youngs_modulus > 0. */
+  void set_youngs_modulus(T youngs_modulus) {
+    DRAKE_DEMAND(youngs_modulus > 0);
+    youngs_modulus_ = std::move(youngs_modulus);
+  }
+
+  /** @pre -1 < poisson_ratio < 0.5. */
+  void set_poisson_ratio(T poisson_ratio) {
+    DRAKE_DEMAND(-1 < poisson_ratio && poisson_ratio < 0.5);
+    poisson_ratio_ = std::move(poisson_ratio);
+  }
+
+  /** @pre mass_damping_coefficient >= 0. */
+  void set_mass_damping_coefficient(T mass_damping_coefficient) {
+    DRAKE_DEMAND(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);
+    stiffness_damping_coefficient_ = std::move(stiffness_damping_coefficient);
+  }
+
+  /**  @pre mass_density > 0. */
+  void set_mass_density(T mass_density) {
+    DRAKE_DEMAND(mass_density > 0);
+    mass_density_ = std::move(mass_density);
+  }
+
+  void set_material_model(MaterialModel material_model) {
+    material_model_ = material_model;
+  }
+
+  /* Getters. */
+  const T& youngs_modulus() const { return youngs_modulus_; }
+  const T& poisson_ratio() const { return poisson_ratio_; }
+  const T& mass_damping_coefficient() const {
+    return mass_damping_coefficient_;
+  }
+  const T& stiffness_damping_coefficient() const {
+    return stiffness_damping_coefficient_;
+  }
+  const T& mass_density() const { return mass_density_; }
+  MaterialModel material_model() const { return material_model_; }
+
+ private:
+  T youngs_modulus_{1e8};
+  T poisson_ratio_{0.49};
+  T mass_damping_coefficient_{0};
+  T stiffness_damping_coefficient_{0};
+  T mass_density_{1.5e3};
+  MaterialModel material_model_{MaterialModel::kCorotated};
+};
+}  // namespace fixed_fem
+}  // namespace multibody
+}  // namespace drake

multibody/fixed_fem/dev/dynamic_elasticity_element.h

@@ -129,7 +129,7 @@ class DynamicElasticityElement final
 
   /* Implements FemElement::CalcMassMatrix(). */
   void DoCalcMassMatrix(
-      const FemState<ElementType>& state,
+      const FemState<ElementType>&,
       EigenPtr<Eigen::Matrix<T, Traits::kNumDofs, Traits::kNumDofs>> M) const {
     *M = ElasticityElementType::mass_matrix();
   }

multibody/fixed_fem/dev/eigen_conjugate_gradient_solver.h

@@ -155,7 +155,9 @@ class EigenConjugateGradientSolver : public LinearSystemSolver<T> {
   iterations is reached.  */
   EigenConjugateGradientSolver(
       const contact_solvers::internal::LinearOperator<T>* A, double tol = 1e-4)
-      : LinearSystemSolver<T>(A), matrix_proxy_(A) {}
+      : LinearSystemSolver<T>(A), matrix_proxy_(A) {
+    set_tolerance(tol);
+  }
 
   ~EigenConjugateGradientSolver() {}
 

multibody/fixed_fem/dev/elasticity_element.h

@@ -4,6 +4,7 @@
 #include <type_traits>
 
 #include "drake/common/eigen_types.h"
+#include "drake/common/unused.h"
 #include "drake/multibody/fixed_fem/dev/constitutive_model.h"
 #include "drake/multibody/fixed_fem/dev/fem_element.h"
 #include "drake/multibody/fixed_fem/dev/fem_state.h"
@@ -156,6 +157,7 @@ class ElasticityElement : public FemElement<DerivedElement, DerivedTraits> {
       const FemState<DerivedElement>& state, const T& scale,
       EigenPtr<Vector<T, Traits::kNumDofs>> external_force) const {
     DRAKE_ASSERT(external_force != nullptr);
+    unused(state);
     /* So far, the only external force is gravity. */
     *external_force += scale * gravity_force_;
   }

multibody/fixed_fem/dev/fem_indexes.h

@@ -13,6 +13,9 @@ using NodeIndex = TypeSafeIndex<class NodeTag>;
 
 /** Index used to identify degrees of freedom (Dof) by index among FEM Dofs. */
 using DofIndex = TypeSafeIndex<class DofTag>;
+
+/** Index into a vector of deformable bodies. */
+using SoftBodyIndex = TypeSafeIndex<class BodyTag>;
 }  // namespace fixed_fem
 }  // namespace multibody
 }  // namespace drake

multibody/fixed_fem/dev/fem_model.h

@@ -10,6 +10,7 @@
 #include <Eigen/Sparse>
 
 #include "drake/common/eigen_types.h"
+#include "drake/common/unused.h"
 #include "drake/multibody/fixed_fem/dev/fem_element.h"
 #include "drake/multibody/fixed_fem/dev/fem_indexes.h"
 #include "drake/multibody/fixed_fem/dev/fem_model_base.h"
@@ -100,7 +101,9 @@ class FemModel : public FemModelBase<typename Element::Traits::T> {
    instead of accumulated from elements. The default implementation is a
    no-op. Derived classes must override this method if their residuals have
    per-vertex contribution. */
-  virtual void AddExplicitResidual(EigenPtr<VectorX<T>> residual) const {}
+  virtual void AddExplicitResidual(EigenPtr<VectorX<T>> residual) const {
+    unused(residual);
+  }
 
  private:
   int ode_order() const final { return Element::Traits::kOdeOrder; }
@@ -151,7 +154,12 @@ class FemModel : public FemModelBase<typename Element::Traits::T> {
     DRAKE_DEMAND(state.element_cache_size() == num_elements());
     /* The values are accumulated in the tangent_matrix, so it is important to
      clear the old data. */
-    tangent_matrix->setZero();
+    using Iterator = typename Eigen::SparseMatrix<T>::InnerIterator;
+    for (int k = 0; k < tangent_matrix->outerSize(); ++k) {
+      for (Iterator it(*tangent_matrix, k); it; ++it) {
+        it.valueRef() = 0;
+      }
+    }
     /* Aliases to improve readability. */
     constexpr int kNumDofs = Element::Traits::kNumDofs;
     constexpr int kNumNodes = Element::Traits::kNumNodes;
@@ -316,11 +324,10 @@ class FemModel : public FemModelBase<typename Element::Traits::T> {
           "FemModel.");
     }
     if (concrete_state_ptr->num_generalized_positions() != num_dofs()) {
-      throw std::logic_error(
-          fmt::format("{}(): The size of the FemState ({}) is incompatible "
-                      "with the size of the FemModel ({}).",
-                      func, concrete_state_ptr->num_generalized_positions(),
-                      num_dofs()));
+      throw std::logic_error(fmt::format(
+          "{}(): The size of the FemState ({}) is incompatible "
+          "with the size of the FemModel ({}).",
+          func, concrete_state_ptr->num_generalized_positions(), num_dofs()));
     }
   }
 

multibody/fixed_fem/dev/linear_system_solver.h

@@ -53,7 +53,7 @@ class LinearSystemSolver {
   }
 
   /* Sets the tolerance for iterative solvers. No-op for direct solvers. */
-  virtual void set_tolerance(const T& tolerance) {}
+  virtual void set_tolerance(const T&) {}
 
  protected:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(LinearSystemSolver);

multibody/fixed_fem/dev/matrix_utilities.h

@@ -32,7 +32,7 @@ Namely the ik-th entry in the jl-th block corresponds to the value Aᵢⱼₖₗ
 template <typename T>
 void PolarDecompose(const Matrix3<T>& F, EigenPtr<Matrix3<T>> R,
                     EigenPtr<Matrix3<T>> S) {
-  const Eigen::JacobiSVD<Matrix3<T>> svd(
+  const Eigen::JacobiSVD<Matrix3<T>, Eigen::HouseholderQRPreconditioner> svd(
       F, Eigen::ComputeFullU | Eigen::ComputeFullV);
   auto U = svd.matrixU();
   const auto& V = svd.matrixV();

multibody/fixed_fem/dev/mesh_utilities.h

@@ -13,7 +13,7 @@ namespace fixed_fem {
 rectangular cells (sharing a rectangular face) are subdivided in the patterns
 that are mirrored of each other so that the mesh is conforming.
 
- The following picture file shows example of the diamond cubic box volume mesh
+The following picture file shows example of the diamond cubic box volume mesh
 and demonstrates the mirrored subdivision pattern in adjacent cells. The file is
 distributed with the source code.