[FEM] Implement CorotatedModel and math helpers (RobotLocomotion#14599)

* Implement CorotatedModel and math helpers.

- Implement the fixed corotated model from Stomakhin, Alexey, et al.
 "Energetically consistent  invertible elasticity." Proceedings of the
 11th ACM SIGGRAPH/Eurographics conference on Computer Animation. 2012.

- Implement a few matrix calculation helpers to facilitate
 CorotatedModel.

- Refactor utility functions for St-Venant type constitutive models
 into free functions.

- Refactor LinearConstitutiveModelTest to test for both
 LinearConstitutiveModel and CorotatedModel.

multibody/fixed_fem/dev/BUILD.bazel

@@ -21,6 +21,41 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "constitutive_model_utilities",
+    hdrs = [
+        "constitutive_model_utilities.h",
+    ],
+)
+
+drake_cc_library(
+    name = "corotated_model",
+    hdrs = [
+        "corotated_model.h",
+    ],
+    deps = [
+        ":constitutive_model",
+        ":constitutive_model_utilities",
+        ":corotated_model_cache_entry",
+        ":matrix_utilities",
+        "//common:default_scalars",
+        "//common:essential",
+    ],
+)
+
+drake_cc_library(
+    name = "corotated_model_cache_entry",
+    hdrs = [
+        "corotated_model_cache_entry.h",
+    ],
+    deps = [
+        ":deformation_gradient_cache_entry",
+        ":matrix_utilities",
+        "//common:default_scalars",
+        "//common:essential",
+    ],
+)
+
 drake_cc_library(
     name = "damping_model",
     hdrs = [
@@ -184,6 +219,7 @@ drake_cc_library(
     ],
     deps = [
         ":constitutive_model",
+        ":constitutive_model_utilities",
         ":linear_constitutive_model_cache_entry",
         "//common:default_scalars",
         "//common:essential",
@@ -223,6 +259,16 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "matrix_utilities",
+    hdrs = [
+        "matrix_utilities.h",
+    ],
+    deps = [
+        "//common:essential",
+    ],
+)
+
 drake_cc_library(
     name = "newmark_scheme",
     hdrs = [
@@ -296,6 +342,15 @@ drake_cc_library(
 )
 # === test/ ===
 
+drake_cc_library(
+    name = "test_utilities",
+    testonly = 1,
+    hdrs = ["test/test_utilities.h"],
+    deps = [
+        "//common:essential",
+    ],
+)
+
 drake_cc_googletest(
     name = "dirichlet_boundary_condition_test",
     deps = [
@@ -387,22 +442,24 @@ drake_cc_googletest(
 )
 
 drake_cc_googletest(
-    name = "isoparametric_element_test",
+    name = "hyperelastic_constitutive_model_test",
     deps = [
-        ":isoparametric_element",
-        ":linear_simplex_element",
+        ":corotated_model",
+        ":linear_constitutive_model",
+        ":test_utilities",
         "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_throws_message",
+        "//math:gradient",
     ],
 )
 
 drake_cc_googletest(
-    name = "linear_constitutive_model_test",
+    name = "isoparametric_element_test",
     deps = [
-        ":linear_constitutive_model",
+        ":isoparametric_element",
+        ":linear_simplex_element",
         "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_throws_message",
-        "//math:gradient",
     ],
 )
 
@@ -421,6 +478,18 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "matrix_utilities_test",
+    deps = [
+        ":matrix_utilities",
+        ":test_utilities",
+        "//common:essential",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//math:geometric_transform",
+        "//math:gradient",
+    ],
+)
+
 drake_cc_googletest(
     name = "newmark_scheme_test",
     deps = [

multibody/fixed_fem/dev/constitutive_model_utilities.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <utility>
+
+namespace drake {
+namespace multibody {
+namespace fixed_fem {
+namespace internal {
+/* Verifies that the given Young's modulus and Poisson ratio are valid. If so,
+ calculates the Lamé parameters from the Young's modulus and the Poisson
+ ratio. If not, throw.
+ @tparam_nonsymbolic_scalar T.
+ @throw std::exception if `youngs_modulus` is negative or if `poisson_ratio` is
+ not in (-1, 0.5). */
+template <typename T>
+std::pair<T, T> CalcLameParameters(const T& youngs_modulus,
+                                   const T& poisson_ratio) {
+  if (youngs_modulus < 0.0) {
+    throw std::logic_error("Young's modulus must be nonnegative.");
+  }
+  if (poisson_ratio >= 0.5 || poisson_ratio <= -1) {
+    throw std::logic_error("Poisson ratio must be in (-1, 0.5).");
+  }
+  T mu = youngs_modulus / (2.0 * (1.0 + poisson_ratio));
+  T lambda = youngs_modulus * poisson_ratio /
+             ((1.0 + poisson_ratio) * (1.0 - 2.0 * poisson_ratio));
+  return std::make_pair(lambda, mu);
+}
+}  // namespace internal
+}  // namespace fixed_fem
+}  // namespace multibody
+}  // namespace drake

multibody/fixed_fem/dev/corotated_model.h

@@ -0,0 +1,135 @@
+#pragma once
+
+#include <array>
+#include <utility>
+
+#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/constitutive_model_utilities.h"
+#include "drake/multibody/fixed_fem/dev/corotated_model_cache_entry.h"
+
+namespace drake {
+namespace multibody {
+namespace fixed_fem {
+/* Forward declare the model to be referred to in the traits class. */
+template <typename T, int num_locations>
+class CorotatedModel;
+
+/** Traits for CorotatedModel. */
+template <typename T, int num_locations>
+struct CorotatedModelTraits {
+  using Scalar = T;
+  using ModelType = CorotatedModel<T, num_locations>;
+  using DeformationGradientCacheEntryType =
+      CorotatedModelCacheEntry<T, num_locations>;
+  static constexpr int kNumLocations = num_locations;
+};
+
+/** Implements the fixed corotated hyperelastic constitutive model as
+ described in [Stomakhin, 2012].
+ @tparam_nonsymbolic_scalar T.
+
+ [Stomakhin, 2012] Stomakhin, Alexey, et al. "Energetically consistent
+ invertible elasticity." Proceedings of the 11th ACM SIGGRAPH/Eurographics
+ conference on Computer Animation. 2012. */
+template <typename T, int num_locations>
+class CorotatedModel final
+    : public ConstitutiveModel<CorotatedModel<T, num_locations>,
+                               CorotatedModelTraits<T, num_locations>> {
+ public:
+  using Traits = CorotatedModelTraits<T, num_locations>;
+  using ModelType = typename Traits::ModelType;
+  using DeformationGradientCacheEntryType =
+      typename Traits::DeformationGradientCacheEntryType;
+  using Base = ConstitutiveModel<ModelType, Traits>;
+
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(CorotatedModel)
+
+  /** Constructs a %CorotatedModel constitutive model with the
+   prescribed Young's modulus and Poisson ratio.
+   @param youngs_modulus Young's modulus of the model, with unit N/m²
+   @param poisson_ratio Poisson ratio of the model, unitless.
+   @pre youngs_modulus must be non-negative.
+   @pre poisson_ratio must be strictly greater than -1 and strictly smaller than
+   0.5. */
+  CorotatedModel(const T& youngs_modulus, const T& poisson_ratio)
+      : E_(youngs_modulus), nu_(poisson_ratio) {
+    std::tie(lambda_, mu_) = internal::CalcLameParameters(E_, nu_);
+  }
+
+  ~CorotatedModel() = default;
+
+  const T& youngs_modulus() const { return E_; }
+
+  const T& poisson_ratio() const { return nu_; }
+
+  const T& shear_modulus() const { return mu_; }
+
+  const T& lame_first_parameter() const { return lambda_; }
+
+ private:
+  friend Base;
+
+  /* Implements the interface ConstitutiveModel::CalcElasticEnergyDensity() in
+   the CRTP base class. */
+  void DoCalcElasticEnergyDensity(
+      const CorotatedModelCacheEntry<T, num_locations>& cache_entry,
+      std::array<T, num_locations>* Psi) const {
+    for (int i = 0; i < num_locations; ++i) {
+      const T& Jm1 = cache_entry.Jm1()[i];
+      const Matrix3<T>& F = cache_entry.deformation_gradient()[i];
+      const Matrix3<T>& R = cache_entry.R()[i];
+      (*Psi)[i] = mu_ * (F - R).squaredNorm() + 0.5 * lambda_ * Jm1 * Jm1;
+    }
+  }
+
+  /* Implements the interface ConstitutiveModel::CalcFirstPiolaStress()
+   in the CRTP base class. */
+  void DoCalcFirstPiolaStress(
+      const DeformationGradientCacheEntryType& cache_entry,
+      std::array<Matrix3<T>, num_locations>* P) const {
+    for (int i = 0; i < num_locations; ++i) {
+      const T& Jm1 = cache_entry.Jm1()[i];
+      const Matrix3<T>& F = cache_entry.deformation_gradient()[i];
+      const Matrix3<T>& R = cache_entry.R()[i];
+      const Matrix3<T>& JFinvT = cache_entry.JFinvT()[i];
+      (*P)[i].noalias() = 2.0 * mu_ * (F - R) + lambda_ * Jm1 * JFinvT;
+    }
+  }
+
+  /* Implements the interface
+   ConstitutiveModel::CalcFirstPiolaStressDerivative() in the CRTP base class.
+  */
+  void DoCalcFirstPiolaStressDerivative(
+      const DeformationGradientCacheEntryType& cache_entry,
+      std::array<Eigen::Matrix<T, 9, 9>, num_locations>* dPdF) const {
+    for (int i = 0; i < num_locations; ++i) {
+      const T& Jm1 = cache_entry.Jm1()[i];
+      const Matrix3<T>& F = cache_entry.deformation_gradient()[i];
+      const Matrix3<T>& R = cache_entry.R()[i];
+      const Matrix3<T>& S = cache_entry.S()[i];
+      const Matrix3<T>& JFinvT = cache_entry.JFinvT()[i];
+      const Vector<T, 3 * 3>& flat_JFinvT =
+          Eigen::Map<const Vector<T, 3 * 3>>(JFinvT.data(), 3 * 3);
+      auto& local_dPdF = (*dPdF)[i];
+      /* The contribution from derivatives of Jm1. */
+      local_dPdF.noalias() = lambda_ * flat_JFinvT * flat_JFinvT.transpose();
+      /* The contribution from derivatives of F. */
+      local_dPdF.diagonal().array() += 2.0 * mu_;
+      /* The contribution from derivatives of R. */
+      internal::AddScaledRotationalDerivative<T>(R, S, -2.0 * mu_, &local_dPdF);
+      /* The contribution from derivatives of JFinvT. */
+      internal::AddScaledCofactorMatrixDerivative<T>(F, lambda_ * Jm1,
+                                                     &local_dPdF);
+    }
+  }
+
+  T E_;       // Young's modulus, N/m².
+  T nu_;      // Poisson ratio.
+  T mu_;      // Lamé's second parameter/Shear modulus, N/m².
+  T lambda_;  // Lamé's first parameter, N/m².
+};
+}  // namespace fixed_fem
+}  // namespace multibody
+}  // namespace drake

multibody/fixed_fem/dev/corotated_model_cache_entry.h

@@ -0,0 +1,77 @@
+#pragma once
+
+#include <array>
+
+#include "drake/common/eigen_types.h"
+#include "drake/multibody/fixed_fem/dev/deformation_gradient_cache_entry.h"
+#include "drake/multibody/fixed_fem/dev/fem_indexes.h"
+#include "drake/multibody/fixed_fem/dev/matrix_utilities.h"
+
+namespace drake {
+namespace multibody {
+namespace fixed_fem {
+/** Cache entry for the CorotatedModel constitutive model. See CorotatedModel
+ for how the cache entry is used. See DeformationGradientCacheEntry for more
+ about cached quantities for constitutive models.
+ @tparam_nonsymbolic_scalar T.
+ @tparam num_locations Number of locations at which the cached quantities are
+ evaluated. */
+template <typename T, int num_locations>
+class CorotatedModelCacheEntry
+    : public DeformationGradientCacheEntry<
+          CorotatedModelCacheEntry<T, num_locations>> {
+ public:
+  using Base =
+      DeformationGradientCacheEntry<CorotatedModelCacheEntry<T, num_locations>>;
+
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(CorotatedModelCacheEntry);
+
+  ~CorotatedModelCacheEntry() = default;
+
+  /** Constructs a %CorotatedModelCacheEntry with no deformation. */
+  CorotatedModelCacheEntry() {
+    std::fill(R_.begin(), R_.end(), Matrix3<T>::Identity());
+    std::fill(S_.begin(), S_.end(), Matrix3<T>::Identity());
+    std::fill(Jm1_.begin(), Jm1_.end(), 0);
+    std::fill(JFinvT_.begin(), JFinvT_.end(), Matrix3<T>::Identity());
+  }
+
+  const std::array<Matrix3<T>, num_locations>& R() const { return R_; }
+
+  const std::array<Matrix3<T>, num_locations>& S() const { return S_; }
+
+  const std::array<T, num_locations>& Jm1() const { return Jm1_; }
+
+  const std::array<Matrix3<T>, num_locations>& JFinvT() const {
+    return JFinvT_;
+  }
+
+ private:
+  friend Base;
+
+  /* Implements the interface DeformationGradientCacheEntry::UpdateCacheEntry().
+   @param F The up-to-date deformation gradients evaluated at the quadrature
+   locations for the associated element. */
+  void DoUpdateCacheEntry(const std::array<Matrix3<T>, num_locations>& F) {
+    for (int i = 0; i < num_locations; ++i) {
+      Matrix3<T>& local_R = R_[i];
+      Matrix3<T>& local_S = S_[i];
+      Matrix3<T>& local_JFinvT = JFinvT_[i];
+      internal::PolarDecompose<T>(F[i], &local_R, &local_S);
+      Jm1_[i] = F[i].determinant() - 1.0;
+      internal::CalcCofactorMatrix<T>(F[i], &local_JFinvT);
+    }
+  }
+
+  /* Let F = RS be the polar decomposition of the deformation gradient where R
+   is a rotation matrix and S is symmetric. */
+  std::array<Matrix3<T>, num_locations> R_;
+  std::array<Matrix3<T>, num_locations> S_;
+  /* The determinant of F minus 1, or J - 1. */
+  std::array<T, num_locations> Jm1_;
+  /* The cofactor matrix of F, or JF⁻ᵀ. */
+  std::array<Matrix3<T>, num_locations> JFinvT_;
+};
+}  // namespace fixed_fem
+}  // namespace multibody
+}  // namespace drake