From 9d7c5562c5e087daaa41255b002f7948017e1d25 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 7 Jan 2025 11:15:19 -0800 Subject: [PATCH] Add a unit test for external forces with subd --- multibody/fem/BUILD.bazel | 13 ++ multibody/fem/linear_simplex_element.cc | 14 ++ .../volumetric_element_external_force_test.cc | 202 ++++++++++++++++++ multibody/fem/volumetric_element.h | 122 ++++------- 4 files changed, 275 insertions(+), 76 deletions(-) create mode 100644 multibody/fem/test/volumetric_element_external_force_test.cc diff --git a/multibody/fem/BUILD.bazel b/multibody/fem/BUILD.bazel index 9add4b0706bc..bfd9adfc6c4b 100644 --- a/multibody/fem/BUILD.bazel +++ b/multibody/fem/BUILD.bazel @@ -786,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 = [ diff --git a/multibody/fem/linear_simplex_element.cc b/multibody/fem/linear_simplex_element.cc index d28b9a60093c..0be71514097e 100644 --- a/multibody/fem/linear_simplex_element.cc +++ b/multibody/fem/linear_simplex_element.cc @@ -132,6 +132,20 @@ template class LinearSimplexElement; template class LinearSimplexElement; template class LinearSimplexElement; template class LinearSimplexElement; + +// 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; +template class LinearSimplexElement; +template class LinearSimplexElement; +template class LinearSimplexElement; + template class LinearSimplexElement; template class LinearSimplexElement; template class LinearSimplexElement; diff --git a/multibody/fem/test/volumetric_element_external_force_test.cc b/multibody/fem/test/volumetric_element_external_force_test.cc new file mode 100644 index 000000000000..c791ba22d487 --- /dev/null +++ b/multibody/fem/test/volumetric_element_external_force_test.cc @@ -0,0 +1,202 @@ +#include + +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/multibody/fem/corotated_model.h" +#include "drake/multibody/fem/fem_state.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_element.h" +#include "drake/multibody/plant/multibody_plant.h" + +namespace drake { +namespace multibody { +namespace fem { +namespace internal { + +/* A constant per current volume force density field used for testing. */ +class ConstantForceDensityField final : public ForceDensityField { + public: + explicit ConstantForceDensityField(const Vector3& f) : f_(f) {} + + private: + Vector3 DoEvaluateAt(const systems::Context& context, + const Vector3& p_WQ) const final { + return f_; + }; + + std::unique_ptr> DoClone() const final { + return std::make_unique(*this); + } + + const Vector3 f_; +}; + +constexpr int kNaturalDimension = 3; +constexpr int kSpatialDimension = 3; +constexpr int kQuadratureOrder = 1; +constexpr double kEpsilon = 1e-14; +constexpr int kSubdivisions = 2; +using QuadratureType = + internal::SimplexGaussianQuadrature; +static constexpr int kNumQuads = QuadratureType::num_quadrature_points; +using SubdQuadratureType = TetSubdivisionQuadrature; +static constexpr int kNumSubdQuads = SubdQuadratureType::num_quadrature_points; +using IsoparametricElementType = + internal::LinearSimplexElement; +using SubdIsoparametricElementType = + internal::LinearSimplexElement; +using ConstitutiveModelType = CorotatedModel; +using DeformationGradientDataType = + std::array, kNumQuads>; + +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: + using ElementType = + VolumetricElement; + using Data = typename ElementType::Data; + static constexpr int kNumDofs = ElementType::num_dofs; + static constexpr int kNumNodes = ElementType::num_nodes; + const std::array kNodeIndices = { + {FemNodeIndex(0), FemNodeIndex(1), FemNodeIndex(2), FemNodeIndex(3)}}; + + void SetUp() override { + SetupElement(); + fem_state_system_ = std::make_unique>( + VectorX::Zero(kNumDofs), VectorX::Zero(kNumDofs), + VectorX::Zero(kNumDofs)); + /* Set up element data. */ + std::function&, std::vector*)> + calc_element_data = [this](const systems::Context& context, + std::vector* element_data) { + /* There's only one element in the system. */ + element_data->resize(1); + const FemState fem_state(fem_state_system_.get(), &context); + (*element_data)[0] = (this->elements_)[0].ComputeData(fem_state); + }; + + cache_index_ = + fem_state_system_ + ->DeclareCacheEntry("Element data", + systems::ValueProducer(calc_element_data)) + .cache_index(); + } + + void SetupElement() { + const Eigen::Matrix X = + reference_positions(); + ConstitutiveModelType constitutive_model(kYoungsModulus, kPoissonRatio); + DampingModel damping_model(0, 0); + elements_.emplace_back(kNodeIndices, std::move(constitutive_model), X, + kDensity, std::move(damping_model)); + } + + /* Makes an FemState to be consumed by the unit tests with the given q, and + zeros for v and a as the state values. */ + std::unique_ptr> MakeFemState(const VectorX& q) { + DRAKE_DEMAND(q.size() == kNumDofs); + auto fem_state = + std::make_unique>(fem_state_system_.get()); + fem_state->SetPositions(q); + fem_state->SetVelocities(Vector::Zero()); + fem_state->SetAccelerations(Vector::Zero()); + return fem_state; + } + + /* Sets arbitrary reference positions with the requirement that the + tetrahedron is not inverted. */ + Eigen::Matrix reference_positions() + const { + Eigen::Matrix X(kSpatialDimension, + kNumNodes); + // clang-format off + X << -0.10, 0.90, 0.02, 0.10, + 1.33, 0.23, 0.04, 0.01, + 0.20, 0.03, 2.31, -0.12; + // clang-format on + return X; + } + + /* Returns the one and only element. */ + const ElementType& element() const { + DRAKE_DEMAND(elements_.size() == 1); + return elements_[0]; + } + + /* Evaluates the element data of the sole element. */ + const Data& EvalElementData(const FemState& state) const { + const std::vector& all_data = + state.EvalElementData(cache_index_); + DRAKE_DEMAND(all_data.size() == 1); + return all_data[0]; + } + + /* Returns the volume evaluated at each quadrature point in the reference + configuration of the only element. */ + const std::array& reference_volume() const { + return element().reference_volume_; + } + + std::unique_ptr> fem_state_system_; + systems::CacheIndex cache_index_; + std::vector elements_; +}; + +namespace { + +/* Tests that when applying a per current volume force density field, the result + agrees with analytic solution. The only reason we use AutoDiffXd here is + because we are reusing an AutoDiffXd fixture previously used to compute + gradients. This unit test in particular does not use AutoDiffXd to + compute gradients. */ +TEST_F(VolumetricElementTest, PerCurrentVolumeExternalForce) { + /* We scale the reference positions to get the current positions so we have + precise control of the current volume. */ + const double scale = 1.23; + const Eigen::Matrix X_matrix = + reference_positions(); + const Eigen::Matrix x_matrix = + scale * X_matrix; + const Vector x(Eigen::Map>( + x_matrix.data(), x_matrix.size())); + std::unique_ptr> fem_state = MakeFemState(x); + const auto& data = EvalElementData(*fem_state); + + const Vector3 force_per_current_volume(4, 5, 6); + const ConstantForceDensityField external_force_field( + force_per_current_volume); + /* The constant force field doesn't depend on Context, but a Context is needed + formally. So we create a dummy Context that's otherwise unused. */ + MultibodyPlant plant(0.01); + plant.Finalize(); + auto context = plant.CreateDefaultContext(); + fem::FemPlantData plant_data{*context, {&external_force_field}}; + + VectorX external_force = VectorX::Zero(kNumDofs); + element().AddScaledExternalForces(data, plant_data, 1.0, &external_force); + Vector3 total_force = Vector3::Zero(); + for (int i = 0; i < kNumNodes; ++i) { + total_force += external_force.segment<3>(3 * i); + } + + const double current_volume = reference_volume()[0] * scale * scale * scale; + const Vector3 expected_force = + current_volume * force_per_current_volume; + EXPECT_TRUE(CompareMatrices(total_force, expected_force, kEpsilon)); +} + +} // namespace +} // namespace internal +} // namespace fem +} // namespace multibody +} // namespace drake diff --git a/multibody/fem/volumetric_element.h b/multibody/fem/volumetric_element.h index 9ae2b0146c1f..37d64ce3920f 100644 --- a/multibody/fem/volumetric_element.h +++ b/multibody/fem/volumetric_element.h @@ -19,7 +19,7 @@ namespace internal { FemElement for the requirement. We define it here instead of nesting it in the traits class below due to #17109. */ template + int num_quadrature_points, int num_external_force_quadrature_points> struct VolumetricElementData { using T = typename ConstitutiveModelType::T; /* The states evaluated at nodes of the element. */ @@ -27,18 +27,10 @@ struct VolumetricElementData { Vector element_q0; Vector element_v; Vector element_a; - /* The current locations of the quadrature points in the world frame. */ - std::array, num_quadrature_points> quadrature_positions; - /* The current locations of the quadrature points used to integrate external - forces in the world frame. */ - std::optional, num_subd_quadrature_points>> - subd_quadrature_positions; using DeformationGradientData = typename ConstitutiveModelType::Data; std::array deformation_gradient_data; - std::optional> - subd_deformation_gradient_determinant; /* The elastic energy density evaluated at quadrature points. Note that this is energy per unit of "reference" volume. */ std::array Psi; @@ -47,6 +39,13 @@ struct VolumetricElementData { /* The derivative of first Piola stress with respect to the deformation gradient evaluated at quadrature points. */ std::array, num_quadrature_points> dPdF; + + /* Quadrature point data used to integrate external forces in the world frame. + */ + std::array, num_external_force_quadrature_points> + external_force_quadrature; + std::array + external_force_quadrature_F_det; }; /* Forward declaration needed for defining the traits below. */ @@ -228,13 +227,16 @@ class VolumetricElement // previous block. Consider refactoring. subd_quadrature_ = SubdQuadrature{}; subd_isoparametric_element_ = - SubdIsoparametricElement(subd_quadrature_.get_points()); + SubdIsoparametricElement(subd_quadrature_.value().get_points()); /* Computes the Jacobian of the change of variable function X(ΞΎ) at subd quadrature locations. */ const std::array, num_subd_quadrature_points> subd_dXdxi = subd_isoparametric_element_.value().CalcJacobian( reference_positions); + subd_dxidX_ = + subd_isoparametric_element_.value().CalcJacobianPseudoinverse( + subd_dXdxi); /* Record the quadrature point volume in reference configuration for each quadrature location. */ std::array subd_reference_volume; @@ -421,14 +423,11 @@ class VolumetricElement const auto& element_q_reshaped = Eigen::Map>(data.element_q.data(), 3, num_nodes); - data.quadrature_positions = - isoparametric_element_.template InterpolateNodalValues<3>( - element_q_reshaped); - - std::array, num_quadrature_points> F = + const std::array, num_quadrature_points> F = CalcDeformationGradient(data.element_q, isoparametric_element_, dxidX_); - std::array, num_quadrature_points> F0 = CalcDeformationGradient( - data.element_q0, isoparametric_element_, dxidX_); + const std::array, num_quadrature_points> F0 = + CalcDeformationGradient(data.element_q0, isoparametric_element_, + dxidX_); for (int q = 0; q < num_quadrature_points; ++q) { data.deformation_gradient_data[q].UpdateData(F[q], F0[q]); @@ -440,73 +439,37 @@ class VolumetricElement data.deformation_gradient_data[q], &(data.dPdF[q])); } if constexpr (use_subd) { - data.subd_quadrature_positions = - subd_isoparametric_element_.template InterpolateNodalValues<3>( - element_q_reshaped); - std::array subd_determinant; - std::array, num_subd_quadrature_points> subd_F = - CalcDeformationGradient(data.element_q, subd_isoparametric_element_, - subd_dxidX_); + data.external_force_quadrature = + subd_isoparametric_element_.value() + .template InterpolateNodalValues<3>(element_q_reshaped); + const std::array, num_subd_quadrature_points> subd_F = + CalcDeformationGradient(data.element_q, + subd_isoparametric_element_.value(), + subd_dxidX_.value()); for (int q = 0; q < num_subd_quadrature_points; ++q) { - subd_determinant[q] = subd_F[q].determinant(); + data.external_force_quadrature_F_det[q] = subd_F[q].determinant(); + } + } else { + data.external_force_quadrature = + isoparametric_element_.template InterpolateNodalValues<3>( + element_q_reshaped); + for (int q = 0; q < num_quadrature_points; ++q) { + data.external_force_quadrature_F_det[q] = F[q].determinant(); } - data.subd_deformation_gradient_determinant = std::move(subd_determinant); } return data; } - // TODO(xuchenhan-tri): Refactor to reduce code duplication. void DoAddScaledExternalForces(const Data& data, const FemPlantData& plant_data, const T& scale, EigenPtr> result) const { - if constexpr (use_subd) { - AddScaledExternalForcesSubd(data, plant_data, scale, result); - } else { - AddScaledExternalForcesNoSubd(data, plant_data, scale, result); - } - } - - void AddScaledExternalForcesNoSubd( - const Data& data, const FemPlantData& plant_data, const T& scale, - EigenPtr> result) const { - const std::array, num_quadrature_points>& - quadrature_positions = data.quadrature_positions; - const std::array, num_quadrature_points>& S = - isoparametric_element_.GetShapeFunctions(); - for (int q = 0; q < num_quadrature_points; ++q) { - const Matrix3& deformation_gradient = - data.deformation_gradient_data[q].deformation_gradient(); - Vector3 scaled_force = Vector3::Zero(); - for (const multibody::ForceDensityField* force_density : - plant_data.force_density_fields) { - DRAKE_ASSERT(force_density != nullptr); - const T change_of_volume = - force_density->density_type() == - multibody::ForceDensityType::kPerReferenceVolume - ? 1.0 - : deformation_gradient.determinant(); - scaled_force += scale * - force_density->EvaluateAt(plant_data.plant_context, - quadrature_positions[q]) * - reference_volume_[q] * change_of_volume; - } - for (int n = 0; n < num_nodes; ++n) { - result->template segment<3>(3 * n) += scaled_force * S[q](n); - } - } - } - - void AddScaledExternalForcesSubd(const Data& data, - const FemPlantData& plant_data, - const T& scale, - EigenPtr> result) const { - const std::array, num_subd_quadrature_points>& - quadrature_positions = data.subd_quadrature_positions; - const std::array, num_subd_quadrature_points>& S = - subd_isoparametric_element_.GetShapeFunctions(); - for (int q = 0; q < num_quadrature_points; ++q) { - const T& det = data.subd_deformation_gradient_determinant[q]; + const auto& quadrature_positions = data.external_force_quadrature; + const int num_quads = quadrature_positions.size(); + for (int q = 0; q < num_quads; ++q) { + const T& reference_volume = + use_subd ? subd_reference_volume_.value()[q] : reference_volume_[q]; + const T& det = data.external_force_quadrature_F_det[q]; Vector3 scaled_force = Vector3::Zero(); for (const multibody::ForceDensityField* force_density : plant_data.force_density_fields) { @@ -519,10 +482,17 @@ class VolumetricElement scaled_force += scale * force_density->EvaluateAt(plant_data.plant_context, quadrature_positions[q]) * - subd_reference_volume_[q] * change_of_volume; + reference_volume * change_of_volume; } for (int n = 0; n < num_nodes; ++n) { - result->template segment<3>(3 * n) += scaled_force * S[q](n); + if (use_subd) { + const auto& S = + subd_isoparametric_element_.value().GetShapeFunctions(); + result->template segment<3>(3 * n) += scaled_force * S[q](n); + } else { + const auto& S = isoparametric_element_.GetShapeFunctions(); + result->template segment<3>(3 * n) += scaled_force * S[q](n); + } } } }