Add isoparametric element for FEM. (RobotLocomotion#14213)

multibody/fem/dev/BUILD.bazel

@@ -6,14 +6,39 @@ load(
     "drake_cc_googletest",
     "drake_cc_library",
 )
-load(
-    "@drake//tools/vector_gen:vector_gen.bzl",
-    "drake_cc_vector_gen_library",
-)
 load("//tools/lint:lint.bzl", "add_lint_tests")
 
 package(
-    default_visibility = ["//visibility:public"],
+    default_visibility = ["//visibility:private"],
+)
+
+drake_cc_library(
+    name = "isoparametric_element",
+    srcs = [
+        "isoparametric_element.cc",
+    ],
+    hdrs = [
+        "isoparametric_element.h",
+    ],
+    deps = [
+        "//common:default_scalars",
+        "//common:essential",
+    ],
+)
+
+drake_cc_library(
+    name = "linear_simplex_element",
+    srcs = [
+        "linear_simplex_element.cc",
+    ],
+    hdrs = [
+        "linear_simplex_element.h",
+    ],
+    deps = [
+        ":isoparametric_element",
+        "//common:default_scalars",
+        "//common:essential",
+    ],
 )
 
 drake_cc_library(
@@ -27,6 +52,22 @@ drake_cc_library(
     ],
 )
 
+drake_cc_googletest(
+    name = "isoparametric_element_test",
+    deps = [
+        ":isoparametric_element",
+        ":linear_simplex_element",
+        "//common/test_utilities:eigen_matrix_compare",
+    ],
+)
+
+drake_cc_googletest(
+    name = "linear_simplex_element_test",
+    deps = [
+        ":linear_simplex_element",
+    ],
+)
+
 drake_cc_googletest(
     name = "simplex_gaussian_quadrature_test",
     deps = [

multibody/fem/dev/isoparametric_element.cc

@@ -0,0 +1,88 @@
+#include "drake/multibody/fem/dev/isoparametric_element.h"
+
+#include <vector>
+
+#include "drake/common/default_scalars.h"
+#include "drake/common/eigen_types.h"
+
+namespace drake {
+namespace multibody {
+namespace fem {
+template <typename T, int NaturalDim>
+std::vector<MatrixX<T>>
+IsoparametricElement<T, NaturalDim>::CalcJacobian(
+    const Eigen::Ref<const MatrixX<T>>& xa) const {
+  DRAKE_DEMAND(xa.cols() == num_nodes());
+  std::vector<MatrixX<T>> dxdxi(num_sample_locations());
+  const std::vector<MatrixX<T>>& dSdxi = CalcGradientInParentCoordinates();
+  for (int q = 0; q < num_sample_locations(); ++q) {
+    dxdxi[q] = xa * dSdxi[q];
+  }
+  return dxdxi;
+}
+
+template <typename T, int NaturalDim>
+std::vector<MatrixX<T>>
+IsoparametricElement<T, NaturalDim>::CalcJacobianInverse(
+    const Eigen::Ref<const MatrixX<T>>& xa) const {
+  DRAKE_DEMAND(xa.cols() == num_nodes());
+  // Number of spatial dimension.
+  const int nsd = xa.rows();
+  DRAKE_ASSERT(nsd >= NaturalDim);
+  std::vector<MatrixX<T>> dxdxi = CalcJacobian(xa);
+  return CalcJacobianInverse(dxdxi);
+}
+
+template <typename T, int NaturalDim>
+std::vector<MatrixX<T>>
+IsoparametricElement<T, NaturalDim>::CalcJacobianInverse(
+    const std::vector<MatrixX<T>>& dxdxi) const {
+  DRAKE_DEMAND(static_cast<int>(dxdxi.size()) == num_sample_locations());
+  std::vector<MatrixX<T>> dxidx(num_sample_locations());
+  for (int q = 0; q < num_sample_locations(); ++q) {
+    Eigen::HouseholderQR<MatrixX<T>> qr(dxdxi[q]);
+    const int nsd = dxdxi[q].rows();
+    DRAKE_DEMAND(dxdxi[q].cols() == NaturalDim);
+    DRAKE_DEMAND(nsd >= NaturalDim);
+    MatrixX<T> dxdxi_rotated(NaturalDim, NaturalDim);
+    // J = QR and we need to solve R*x = I which is equivalent to Jx = Q*I.
+    auto rhs = qr.householderQ() * MatrixX<T>::Identity(nsd, NaturalDim);
+    auto dxidx_rotated = qr.solve(rhs);
+    MatrixX<T> local_dxidx = MatrixX<T>::Zero(NaturalDim, nsd);
+    local_dxidx.topLeftCorner(NaturalDim, NaturalDim) = dxidx_rotated;
+    dxidx[q] = local_dxidx * qr.householderQ().transpose();
+  }
+  return dxidx;
+}
+
+template <typename T, int NaturalDim>
+std::vector<T> IsoparametricElement<T, NaturalDim>::InterpolateScalar(
+    const Eigen::Ref<const VectorX<T>>& ua) const {
+  DRAKE_DEMAND(ua.size() == num_nodes());
+  const std::vector<VectorX<T>>& S = CalcShapeFunctions();
+  std::vector<T> interpolated_value(num_sample_locations());
+  for (int q = 0; q < num_sample_locations(); ++q) {
+    interpolated_value[q] = ua.dot(S[q]);
+  }
+  return interpolated_value;
+}
+
+template <typename T, int NaturalDim>
+std::vector<VectorX<T>> IsoparametricElement<T, NaturalDim>::InterpolateVector(
+    const Eigen::Ref<const MatrixX<T>>& ua) const {
+  DRAKE_DEMAND(ua.cols() == num_nodes());
+  const std::vector<VectorX<T>>& S = CalcShapeFunctions();
+  std::vector<VectorX<T>> interpolated_value(num_sample_locations());
+  for (int q = 0; q < num_sample_locations(); ++q) {
+    interpolated_value[q] = ua * S[q];
+  }
+  return interpolated_value;
+}
+
+template class IsoparametricElement<double, 2>;
+template class IsoparametricElement<double, 3>;
+template class IsoparametricElement<AutoDiffXd, 2>;
+template class IsoparametricElement<AutoDiffXd, 3>;
+}  // namespace fem
+}  // namespace multibody
+}  // namespace drake

multibody/fem/dev/isoparametric_element.h

@@ -0,0 +1,138 @@
+#pragma once
+
+#include <utility>
+#include <vector>
+
+#include "drake/common/eigen_types.h"
+
+namespace drake {
+namespace multibody {
+namespace fem {
+
+/** IsoparametricElement is a class that helps evaluate shape functions
+ and their derivatives at prescribed locations. The shape function
+ `S` located at a vertex `a` maps parent domain to a scalar. The reference
+ position `X` as well as `u`, the function we wish to interpolate, are
+ interpolated from the shape function, i.e.:
+
+ <pre>
+     X(ξ) = Sₐ(ξ)Xₐ, and
+     u(ξ) = Sₐ(ξ)uₐ,
+ </pre>
+
+ where ξ ∈ ℝᵈ and d is the natural dimension (dimension of the parent
+ domain), which may be different from the dimensions of X and u (e.g. 2D
+ membrane or shell element in 3D dynamics simulation). The constructor
+ for this class takes in a vector of locations at which we may
+ evaluate and/or interpolate various quantities. If you need to evaluate
+ and/or interpolate at other locations, construct another instance of
+ IsoparametricElement and pass the new locations into the constructor.
+ @tparam_nonsymbolic_scalar T.
+ @tparam NaturalDim The dimension of the parent domain. */
+template <typename T, int NaturalDim>
+class IsoparametricElement {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(IsoparametricElement);
+
+  using VectorD = Eigen::Matrix<T, NaturalDim, 1>;
+
+  /** Constructs an isoparametric element that performs calculations at the
+   locations specified by the input `locations`. */
+  explicit IsoparametricElement(std::vector<VectorD> locations)
+      : locations_(std::move(locations)) {}
+
+  virtual ~IsoparametricElement() = default;
+
+  /** The number of nodes in the element. E.g. 3 for linear triangles, 9 for
+   quadratic quadrilaterals and 4 for linear tetrahedrons. */
+  virtual int num_nodes() const = 0;
+
+  /** The number of locations to evaluate various quantities in the
+   element. */
+  int num_sample_locations() const { return locations_.size(); }
+
+  /** Returns a vector of locations ξ in the parent domain at which this class
+  evaluates elemental quantities, as provided at construction. */
+  const std::vector<VectorD>& locations() const { return locations_; }
+
+  /** Computes the shape function vector
+      S(ξ) = [S₀(ξ); S₁(ξ); ... Sₐ(ξ); ... Sₙ₋₁(ξ)]
+   at each ξ in the parent domain provided at construction. The a-th component
+   of S(ξ) corresponds to the shape function Sₐ(ξ) for node a.
+   @returns S Vector of size equal to 'num_sample_locations()`. The q-th entry
+   of the output contains vector S(ξ), of size num_nodes(), evaluated at the
+   q-th ξ in the parent domain provided at construction. */
+  virtual const std::vector<VectorX<T>>& CalcShapeFunctions() const = 0;
+
+  /** Computes dS/dξ, a matrix of size `num_nodes()` by `NaturalDim` evaluated
+   at each ξ in the parent domain provided at construction.
+   @returns dSdxi The gradient of the shape function with respect to the
+   parent coordinates evaluated at each ξ in the parent domain provided at
+   construction. dSdxi is a vector of size `num_sample_locations()`. The q-th
+   entry contains dS/dξ evaluated at the q-th ξ in the parent domain provided at
+   construction. */
+  virtual const std::vector<MatrixX<T>>& CalcGradientInParentCoordinates()
+      const = 0;
+
+  // TODO(xuchenhan-tri): Implement CalcGradientInSpatialCoordinates().
+
+  /** Computes dx/dξ, a matrix of size `nsd` by `NaturalDim` at each ξ in the
+   parent domain provided at construction. `nsd` is the number of spatial
+   dimensions, defined by the number of rows in `xa`.
+   @param xa Spatial coordinates for each element node.
+   @returns jacobian The Jacobian of the spatial coordinates with respect to
+   parent coordinates evaluated at each ξ in the parent domain provided at
+   construction. 'jacobian' is represented by a vector of size
+   `num_sample_locations()`. The q-th entry contains the dx/dξ evaluated at the
+   q-th ξ in the parent domain provided at construction.
+   @pre `xa` must have `num_nodes()` columns. */
+  std::vector<MatrixX<T>> CalcJacobian(
+      const Eigen::Ref<const MatrixX<T>>& xa) const;
+
+  /** Computes dξ/dx, a matrix of size `NaturalDim` by `nsd`, at each ξ in the
+   parent domain provided at construction. `nsd` is the "number of spatial
+   dimensions, defined by the number of rows in `xa`.
+   @param xa Spatial coordinates for each element node.
+   @returns The gradient of the shape function with respect to the spatial
+   coordinates evaluated at each ξ in the parent domain provided at
+   construction represented by a vector
+   of size `num_quad()`. The q-th entry contains dξ/dx evaluated at the q-th ξ
+   in the parent domain provided at construction.
+   @pre `xa` must have `num_nodes()` columns.
+   @see CalcJacobian().  */
+  std::vector<MatrixX<T>> CalcJacobianInverse(
+      const Eigen::Ref<const MatrixX<T>>& xa) const;
+
+  /** Preferred signature for computing dξ/dx when the Jacobians are
+   available so as to avoid recomputing the Jacobians.
+   @param jacobian A vector of size `num_sample_locations()` with the q-th entry
+   containing the element Jacobian evaluated at the q-th ξ
+   in the parent domain provided at construction.
+   @pre `jacobian.size()` must be equal to `num_sample_locations()`. Each entry
+   in `jacobian` must have `NaturalDim` columns.
+   @see CalcJacobian(). */
+  std::vector<MatrixX<T>> CalcJacobianInverse(
+      const std::vector<MatrixX<T>>& jacobian) const;
+
+  /** Interpolates scalar nodal values `ua` into u(ξ) at each ξ
+   in the parent domain provided at construction.
+   @param ua The value of scalar function u at each element node.
+   @pre `ua` must be a vector of size num_nodes(). */
+  std::vector<T> InterpolateScalar(
+      const Eigen::Ref<const VectorX<T>>& ua) const;
+
+  /** Interpolates vector nodal values `ua` into u(ξ) at each ξ
+   in the parent domain provided at construction.
+   @param ua The value of vector function u at each element node.
+   @pre `ua` must have size nsd by num_nodes(). */
+  std::vector<VectorX<T>> InterpolateVector(
+      const Eigen::Ref<const MatrixX<T>>& ua) const;
+
+ private:
+  // The locations at which to evaluate various quantities of this
+  // element.
+  std::vector<VectorD> locations_;
+};
+}  // namespace fem
+}  // namespace multibody
+}  // namespace drake

multibody/fem/dev/linear_simplex_element.cc

@@ -0,0 +1,41 @@
+#include "drake/multibody/fem/dev/linear_simplex_element.h"
+
+namespace drake {
+namespace multibody {
+namespace fem {
+template <typename T, int NaturalDim>
+std::vector<VectorX<T>>
+LinearSimplexElement<T, NaturalDim>::CalcShapeFunctionsHelper() const {
+  std::vector<VectorX<T>> S(this->num_sample_locations());
+  const auto& locations = this->locations();
+  for (int q = 0; q < this->num_sample_locations(); ++q) {
+    VectorX<T> Sq = VectorX<T>::Zero(num_nodes());
+    // Sₐ = ξₐ₋₁ for a = 1, ..., num_nodes() - 1
+    for (int a = 1; a < num_nodes(); ++a) {
+      Sq(a) = locations[q](a - 1);
+    }
+    // S₀ = 1−ξ₀ − ... − ξₙ₋₂
+    Sq(0) = 1 - Sq.sum();
+    S[q] = Sq;
+  }
+  return S;
+}
+
+template <typename T, int NaturalDim>
+std::vector<MatrixX<T>> LinearSimplexElement<
+    T, NaturalDim>::CalcGradientInParentCoordinatesHelper() const {
+  MatrixX<T> dSdxi_q = MatrixX<T>::Zero(num_nodes(), NaturalDim);
+  dSdxi_q.template topRows<1>() = -1.0 * VectorX<T>::Ones(NaturalDim);
+  dSdxi_q.template bottomRows<NaturalDim>() =
+      MatrixX<T>::Identity(NaturalDim, NaturalDim);
+  std::vector<MatrixX<T>> dSdxi(this->num_sample_locations(), dSdxi_q);
+  return dSdxi;
+}
+
+template class LinearSimplexElement<double, 2>;
+template class LinearSimplexElement<double, 3>;
+template class LinearSimplexElement<AutoDiffXd, 2>;
+template class LinearSimplexElement<AutoDiffXd, 3>;
+}  // namespace fem
+}  // namespace multibody
+}  // namespace drake

multibody/fem/dev/linear_simplex_element.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <utility>
+#include <vector>
+
+#include "drake/common/default_scalars.h"
+#include "drake/common/eigen_types.h"
+#include "drake/multibody/fem/dev/isoparametric_element.h"
+
+namespace drake {
+namespace multibody {
+namespace fem {
+/** A concrete isoparametric element for linear simplex elements (triangles and
+ tetrahedrons). In parent domain, the simplex's node are laid out in the
+ following way: the 0-th node is placed at the origin and the i-th node for 0 <
+ i <= NaturalDim is placed at the point whose i-th coordinate is 1 and all other
+ coordinates are 0. */
+// TODO(xuchenhan-tri) Also support segments. Need to add instantiations as
+// well as unit tests.
+template <typename T, int NaturalDim>
+class LinearSimplexElement : public IsoparametricElement<T, NaturalDim> {
+ public:
+  using typename IsoparametricElement<T, NaturalDim>::VectorD;
+  explicit LinearSimplexElement(std::vector<VectorD> locations)
+      : IsoparametricElement<T, NaturalDim>(std::move(locations)) {
+    S_ = CalcShapeFunctionsHelper();
+    dSdxi_ = CalcGradientInParentCoordinatesHelper();
+  }
+
+  int num_nodes() const final { return NaturalDim + 1; }
+
+  const std::vector<VectorX<T>>& CalcShapeFunctions() const { return S_; }
+
+  const std::vector<MatrixX<T>>& CalcGradientInParentCoordinates() const {
+    return dSdxi_;
+  }
+
+ private:
+  std::vector<VectorX<T>> CalcShapeFunctionsHelper() const;
+
+  std::vector<MatrixX<T>> CalcGradientInParentCoordinatesHelper() const;
+
+  // Shape functions evaluated at points specified at construction.
+  std::vector<VectorX<T>> S_;
+  // Shape function derivatives evaluated at points specified at construction.
+  std::vector<MatrixX<T>> dSdxi_;
+};
+}  // namespace fem
+}  // namespace multibody
+}  // namespace drake