From c8fc6679ffb190cbe2380ea8354e22f59a3aee76 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 14 Nov 2024 17:58:17 -0800 Subject: [PATCH 1/2] Add math::FourthOrderTensor --- math/BUILD.bazel | 19 +++ math/fourth_order_tensor.cc | 33 ++++++ math/fourth_order_tensor.h | 108 ++++++++++++++++++ math/test/fourth_order_tensor_test.cc | 86 ++++++++++++++ multibody/fem/BUILD.bazel | 2 + multibody/fem/constitutive_model.h | 9 +- multibody/fem/corotated_model.cc | 7 +- multibody/fem/corotated_model.h | 4 +- multibody/fem/linear_constitutive_model.cc | 4 +- multibody/fem/linear_constitutive_model.h | 6 +- multibody/fem/linear_corotated_model.cc | 4 +- multibody/fem/linear_corotated_model.h | 4 +- multibody/fem/matrix_utilities.cc | 4 +- multibody/fem/matrix_utilities.h | 25 +--- multibody/fem/test/constitutive_model_test.cc | 2 +- .../test/constitutive_model_test_utilities.cc | 4 +- multibody/fem/test/matrix_utilities_test.cc | 6 +- multibody/fem/volumetric_element.h | 41 +------ 18 files changed, 281 insertions(+), 87 deletions(-) create mode 100644 math/fourth_order_tensor.cc create mode 100644 math/fourth_order_tensor.h create mode 100644 math/test/fourth_order_tensor_test.cc diff --git a/math/BUILD.bazel b/math/BUILD.bazel index bee2b3cbf8d1..46654f9a6697 100644 --- a/math/BUILD.bazel +++ b/math/BUILD.bazel @@ -23,6 +23,7 @@ drake_cc_package_library( ":discrete_lyapunov_equation", ":eigen_sparse_triplet", ":evenly_distributed_pts_on_sphere", + ":fourth_order_tensor", ":geometric_transform", ":gradient", ":gray_code", @@ -134,6 +135,16 @@ drake_cc_library( ], ) +drake_cc_library( + name = "fourth_order_tensor", + srcs = ["fourth_order_tensor.cc"], + hdrs = ["fourth_order_tensor.h"], + deps = [ + "//common:default_scalars", + "//common:essential", + ], +) + # TODO(jwnimmer-tri) Improved name for this library, "pose_representations"? drake_cc_library( name = "geometric_transform", @@ -433,6 +444,14 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "fourth_order_tensor_test", + deps = [ + ":fourth_order_tensor", + "//common/test_utilities:eigen_matrix_compare", + ], +) + drake_cc_googletest( name = "jacobian_test", deps = [ diff --git a/math/fourth_order_tensor.cc b/math/fourth_order_tensor.cc new file mode 100644 index 000000000000..57b66448cbe2 --- /dev/null +++ b/math/fourth_order_tensor.cc @@ -0,0 +1,33 @@ +#include "drake/math/fourth_order_tensor.h" + +#include "drake/common/default_scalars.h" + +namespace drake { +namespace math { +namespace internal { + +template +FourthOrderTensor::FourthOrderTensor(const MatrixType& data) : data_(data) {} + +template +FourthOrderTensor::FourthOrderTensor() = default; + +template +void FourthOrderTensor::ContractWithVectors( + const Eigen::Ref>& u, + const Eigen::Ref>& v, EigenPtr> B) const { + B->setZero(); + for (int l = 0; l < 3; ++l) { + for (int j = 0; j < 3; ++j) { + *B += data_.template block<3, 3>(3 * j, 3 * l) * u(j) * v(l); + } + } +} + +} // namespace internal +} // namespace math +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( + class ::drake::math::internal::FourthOrderTensor); +template class drake::math::internal::FourthOrderTensor; diff --git a/math/fourth_order_tensor.h b/math/fourth_order_tensor.h new file mode 100644 index 000000000000..54dba2f56830 --- /dev/null +++ b/math/fourth_order_tensor.h @@ -0,0 +1,108 @@ +#pragma once + +#include "drake/common/drake_throw.h" +#include "drake/common/eigen_types.h" + +namespace drake { +namespace math { +namespace internal { + +/* This class provides functionalities related to 4th-order tensors of + dimension 3*3*3*3. The tensor is represented using a a 9*9 matrix that is + organized as following + + l = 1 l = 2 l = 3 + ------------------------------------- + | | | | + j = 1 | Aᵢ₁ₖ₁ | Aᵢ₁ₖ₂ | Aᵢ₁ₖ₃ | + | | | | + ------------------------------------- + | | | | + j = 2 | Aᵢ₂ₖ₁ | Aᵢ₂ₖ₂ | Aᵢ₂ₖ₃ | + | | | | + ------------------------------------- + | | | | + j = 3 | Aᵢ₃ₖ₁ | Aᵢ₃ₖ₂ | Aᵢ₃ₖ₃ | + | | | | + ------------------------------------- + Namely the ik-th entry in the jl-th block corresponds to the value Aᵢⱼₖₗ. + @tparam float, double, AutoDiffXd. */ +template +class FourthOrderTensor { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(FourthOrderTensor) + using MatrixType = Eigen::Matrix; + + /* Constructs a 4th-order tensor represented by the given matrix using the + convention layed out in the class documentation. */ + explicit FourthOrderTensor(const MatrixType& data); + + /* Constructs a zero 4th-order tensor. */ + FourthOrderTensor(); + + /* Performs contraction between this 4th-order tensor A and two vectors u and + v and outputs 2nd order tensor B. In Einstein notation, the contraction being + done is Bᵢₖ = uⱼ Aᵢⱼₖₗ vₗ. */ + void ContractWithVectors(const Eigen::Ref>& u, + const Eigen::Ref>& v, + EigenPtr> B) const; + + /* Returns this 4th-order tensor encoded as a matrix according to the class + documentation. */ + const MatrixType& data() const { return data_; } + + /* Returns this 4th-order tensor encoded as a mutable matrix according to the + class documentation. */ + MatrixType& mutable_data() { return data_; } + + /* Returns the value of the 4th-order tensor at the given indices. + @pre 0 <= i, j, k, l < 3. */ + const T& operator()(int i, int j, int k, int l) const { + DRAKE_ASSERT(0 <= i && i < 3); + DRAKE_ASSERT(0 <= j && j < 3); + DRAKE_ASSERT(0 <= k && k < 3); + DRAKE_ASSERT(0 <= l && l < 3); + return data_(3 * j + i, 3 * l + k); + } + + /* Returns the value of the 4th-order tensor at the given indices. + @pre 0 <= i, j, k, l < 3. */ + T& operator()(int i, int j, int k, int l) { + DRAKE_ASSERT(0 <= i && i < 3); + DRAKE_ASSERT(0 <= j && j < 3); + DRAKE_ASSERT(0 <= k && k < 3); + DRAKE_ASSERT(0 <= l && l < 3); + return data_(3 * j + i, 3 * l + k); + } + + /* Returns the value of the 4th-order tensor at the given indices, + interpreted as indices into the 9x9 matrix, using the convention layed out in + the class documentation. + @pre 0 <= i, j < 9. */ + const T& operator()(int i, int j) const { + DRAKE_ASSERT(0 <= i && i < 9); + DRAKE_ASSERT(0 <= j && j < 9); + return data_(i, j); + } + + /* Returns the value of the 4th-order tensor at the given indices, + interpreted as indices into the 9x9 matrix, using the convention layed out in + the class documentation. + @pre 0 <= i, j < 9. */ + T& operator()(int i, int j) { + DRAKE_ASSERT(0 <= i && i < 9); + DRAKE_ASSERT(0 <= j && j < 9); + return data_(i, j); + } + + /* Sets the data of this 4th-order tensor to the given matrix, using the + convention layed out in the class documentation. */ + void set_data(const MatrixType& data) { data_ = data; } + + private: + MatrixType data_{MatrixType::Zero()}; +}; + +} // namespace internal +} // namespace math +} // namespace drake diff --git a/math/test/fourth_order_tensor_test.cc b/math/test/fourth_order_tensor_test.cc new file mode 100644 index 000000000000..9f3e02bd614d --- /dev/null +++ b/math/test/fourth_order_tensor_test.cc @@ -0,0 +1,86 @@ +#include "drake/math/fourth_order_tensor.h" + +#include "drake/common/eigen_types.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" + +using Eigen::Matrix3d; +using Eigen::Vector3d; + +namespace drake { +namespace math { +namespace internal { +namespace { + +Eigen::Matrix MakeArbitraryMatrix() { + Eigen::Matrix data; + for (int i = 0; i < 9; ++i) { + for (int j = 0; j < 9; ++j) { + data(i, j) = i + j; + } + } + return data; +} + +GTEST_TEST(FourthOrderTensorTest, DefaultConstructor) { + FourthOrderTensor tensor; + EXPECT_TRUE(tensor.data().isZero()); + const Vector3d u(1.0, 2.0, 3.0); + const Vector3d v(4.0, 5.0, 6.0); + Matrix3d B; + + tensor.ContractWithVectors(u, v, &B); + EXPECT_TRUE(B.isZero()); +} + +GTEST_TEST(FourthOrderTensorTest, ConstructWithData) { + const Eigen::Matrix data = MakeArbitraryMatrix(); + FourthOrderTensor tensor(data); + /* Getter and mutable getter. */ + EXPECT_EQ(tensor.data(), data); + EXPECT_EQ(tensor.data(), tensor.mutable_data()); + tensor.mutable_data() = data.transpose(); + EXPECT_EQ(tensor.data(), data.transpose()); + /* Settor */ + tensor.set_data(data); + EXPECT_EQ(tensor.data(), data); + /* Operator with four indices. */ + EXPECT_EQ(tensor(0, 0, 0, 0), data(0, 0)); + EXPECT_EQ(tensor(1, 1, 1, 1), data(4, 4)); + /* Operator with two indices. */ + EXPECT_EQ(tensor(0, 0), data(0, 0)); + EXPECT_EQ(tensor(3, 3), data(3, 3)); +} + +GTEST_TEST(FourthOrderTensorTest, ContractWithVectors) { + FourthOrderTensor tensor(MakeArbitraryMatrix()); + + /* If any vector input is zero, the contraction is zero. */ + Vector3d u = Vector3d::Zero(); + Vector3d v(4.0, 5.0, 6.0); + Matrix3d B; + tensor.ContractWithVectors(u, v, &B); + EXPECT_TRUE(B.isZero()); + tensor.ContractWithVectors(v, u, &B); + EXPECT_TRUE(B.isZero()); + + /* If the 9x9 representation of the tensor has a repeating pattern in the + blocks, the contraction is a multiple of that block. */ + Matrix3d block; + block << 1, 2, 3, 4, 5, 6, 7, 8, 9; + Eigen::Matrix data; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + data.block<3, 3>(3 * i, 3 * j) = block; + } + } + tensor = FourthOrderTensor(data); + u << 1.0, 2.0, 3.0; + v << 4.0, 5.0, 6.0; + tensor.ContractWithVectors(u, v, &B); + EXPECT_TRUE(CompareMatrices(B, block * (u * v.transpose()).sum())); +} + +} // namespace +} // namespace internal +} // namespace math +} // namespace drake diff --git a/multibody/fem/BUILD.bazel b/multibody/fem/BUILD.bazel index ea9a3ef84324..2c3b4608637f 100644 --- a/multibody/fem/BUILD.bazel +++ b/multibody/fem/BUILD.bazel @@ -83,6 +83,7 @@ drake_cc_library( deps = [ "//common:essential", "//common:nice_type_name", + "//math:fourth_order_tensor", ], ) @@ -407,6 +408,7 @@ drake_cc_library( deps = [ "//common:default_scalars", "//common:essential", + "//math:fourth_order_tensor", ], ) diff --git a/multibody/fem/constitutive_model.h b/multibody/fem/constitutive_model.h index 223f62f09285..68a9e01c63f4 100644 --- a/multibody/fem/constitutive_model.h +++ b/multibody/fem/constitutive_model.h @@ -6,6 +6,7 @@ #include "drake/common/eigen_types.h" #include "drake/common/nice_type_name.h" +#include "drake/math/fourth_order_tensor.h" namespace drake { namespace multibody { @@ -92,8 +93,8 @@ class ConstitutiveModel { | | | | ------------------------------------- @pre `dPdF != nullptr`. */ - void CalcFirstPiolaStressDerivative(const Data& data, - Eigen::Matrix* dPdF) const { + void CalcFirstPiolaStressDerivative( + const Data& data, math::internal::FourthOrderTensor* dPdF) const { DRAKE_ASSERT(dPdF != nullptr); derived().CalcFirstPiolaStressDerivativeImpl(data, dPdF); } @@ -123,8 +124,8 @@ class ConstitutiveModel { NiceTypeName::Get(derived()))); } - void CalcFirstPiolaStressDerivativeImpl(const Data& data, - Eigen::Matrix* dPdF) const { + void CalcFirstPiolaStressDerivativeImpl( + const Data& data, math::internal::FourthOrderTensor* dPdF) const { throw std::logic_error( fmt::format("The derived class {} must provide a shadow definition of " "CalcFirstPiolaStressDerivativeImpl() to be correct.", diff --git a/multibody/fem/corotated_model.cc b/multibody/fem/corotated_model.cc index 3f102e82afe1..1b170a3b9fb3 100644 --- a/multibody/fem/corotated_model.cc +++ b/multibody/fem/corotated_model.cc @@ -44,7 +44,7 @@ void CorotatedModel::CalcFirstPiolaStressImpl(const Data& data, template void CorotatedModel::CalcFirstPiolaStressDerivativeImpl( - const Data& data, Eigen::Matrix* dPdF) const { + const Data& data, math::internal::FourthOrderTensor* dPdF) const { const T& Jm1 = data.Jm1(); const Matrix3& F = data.deformation_gradient(); const Matrix3& R = data.R(); @@ -54,9 +54,10 @@ void CorotatedModel::CalcFirstPiolaStressDerivativeImpl( Eigen::Map>(JFinvT.data(), 3 * 3); auto& local_dPdF = (*dPdF); /* The contribution from derivatives of Jm1. */ - local_dPdF.noalias() = lambda_ * flat_JFinvT * flat_JFinvT.transpose(); + local_dPdF.mutable_data().noalias() = + lambda_ * flat_JFinvT * flat_JFinvT.transpose(); /* The contribution from derivatives of F. */ - local_dPdF.diagonal().array() += 2.0 * mu_; + local_dPdF.mutable_data().diagonal().array() += 2.0 * mu_; /* The contribution from derivatives of R. */ internal::AddScaledRotationalDerivative(R, S, -2.0 * mu_, &local_dPdF); /* The contribution from derivatives of JFinvT. */ diff --git a/multibody/fem/corotated_model.h b/multibody/fem/corotated_model.h index 9e3673fe3be7..00129fdcc1e3 100644 --- a/multibody/fem/corotated_model.h +++ b/multibody/fem/corotated_model.h @@ -69,8 +69,8 @@ class CorotatedModel final /* Shadows ConstitutiveModel::CalcFirstPiolaStressDerivativeImpl() as required by the CRTP base class. */ - void CalcFirstPiolaStressDerivativeImpl(const Data& data, - Eigen::Matrix* dPdF) const; + void CalcFirstPiolaStressDerivativeImpl( + const Data& data, math::internal::FourthOrderTensor* dPdF) const; T E_; // Young's modulus, N/m². T nu_; // Poisson's ratio. diff --git a/multibody/fem/linear_constitutive_model.cc b/multibody/fem/linear_constitutive_model.cc index b0383816efe0..655596fd5cf4 100644 --- a/multibody/fem/linear_constitutive_model.cc +++ b/multibody/fem/linear_constitutive_model.cc @@ -29,7 +29,7 @@ LinearConstitutiveModel::LinearConstitutiveModel(const T& youngs_modulus, Keep in mind that the indices are laid out such that the ik-th entry in the jl-th block corresponds to the value dPᵢⱼ/dFₖₗ. */ /* First term. */ - dPdF_ = mu_ * Eigen::Matrix::Identity(); + dPdF_.set_data(mu_ * Eigen::Matrix::Identity()); for (int k = 0; k < 3; ++k) { /* Second term. */ for (int l = 0; l < 3; ++l) { @@ -65,7 +65,7 @@ void LinearConstitutiveModel::CalcFirstPiolaStressImpl(const Data& data, template void LinearConstitutiveModel::CalcFirstPiolaStressDerivativeImpl( - const Data&, Eigen::Matrix* dPdF) const { + const Data&, math::internal::FourthOrderTensor* dPdF) const { *dPdF = dPdF_; } diff --git a/multibody/fem/linear_constitutive_model.h b/multibody/fem/linear_constitutive_model.h index b986f85ddb89..0639127d538c 100644 --- a/multibody/fem/linear_constitutive_model.h +++ b/multibody/fem/linear_constitutive_model.h @@ -71,14 +71,14 @@ class LinearConstitutiveModel final /* Shadows ConstitutiveModel::CalcFirstPiolaStressDerivativeImpl() as required by the CRTP base class. */ - void CalcFirstPiolaStressDerivativeImpl(const Data& data, - Eigen::Matrix* dPdF) const; + void CalcFirstPiolaStressDerivativeImpl( + const Data& data, math::internal::FourthOrderTensor* dPdF) const; T E_; // Young's modulus, N/m². T nu_; // Poisson's ratio. T mu_; // Lamé's second parameter/Shear modulus, N/m². T lambda_; // Lamé's first parameter, N/m². - Eigen::Matrix + math::internal::FourthOrderTensor dPdF_; // The First Piola stress derivative is constant and precomputed. }; diff --git a/multibody/fem/linear_corotated_model.cc b/multibody/fem/linear_corotated_model.cc index a3fed81e495c..e24ee3e67145 100644 --- a/multibody/fem/linear_corotated_model.cc +++ b/multibody/fem/linear_corotated_model.cc @@ -82,11 +82,11 @@ void LinearCorotatedModel::CalcFirstPiolaStressImpl(const Data& data, */ template void LinearCorotatedModel::CalcFirstPiolaStressDerivativeImpl( - const Data& data, Eigen::Matrix* dPdF) const { + const Data& data, math::internal::FourthOrderTensor* dPdF) const { const Matrix3& R0 = data.R0(); auto& local_dPdF = (*dPdF); /* Add in μ * δₐᵢδⱼᵦ. */ - local_dPdF = mu_ * Eigen::Matrix::Identity(); + local_dPdF.set_data(mu_ * Eigen::Matrix::Identity()); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { for (int alpha = 0; alpha < 3; ++alpha) { diff --git a/multibody/fem/linear_corotated_model.h b/multibody/fem/linear_corotated_model.h index c561a73ffdbe..2a8f29f3cb33 100644 --- a/multibody/fem/linear_corotated_model.h +++ b/multibody/fem/linear_corotated_model.h @@ -71,8 +71,8 @@ class LinearCorotatedModel final /* Shadows ConstitutiveModel::CalcFirstPiolaStressDerivativeImpl() as required by the CRTP base class. */ - void CalcFirstPiolaStressDerivativeImpl(const Data& data, - Eigen::Matrix* dPdF) const; + void CalcFirstPiolaStressDerivativeImpl( + const Data& data, math::internal::FourthOrderTensor* dPdF) const; T E_; // Young's modulus, N/m². T nu_; // Poisson's ratio. diff --git a/multibody/fem/matrix_utilities.cc b/multibody/fem/matrix_utilities.cc index b0258785cc4e..76bbbe279388 100644 --- a/multibody/fem/matrix_utilities.cc +++ b/multibody/fem/matrix_utilities.cc @@ -52,7 +52,7 @@ void PolarDecompose(const Matrix3& F, EigenPtr> R, template void AddScaledRotationalDerivative( const Matrix3& R, const Matrix3& S, const T& scale, - EigenPtr> scaled_dRdF) { + math::internal::FourthOrderTensor* scaled_dRdF) { /* Some notes on derivation on the derivative of the rotation matrix from polar decomposition: we start with the result from section 2 of [McAdams, 2011] about the differential of the rotation matrix, which states that δR = @@ -118,7 +118,7 @@ void CalcCofactorMatrix(const Matrix3& M, EigenPtr> cofactor) { template void AddScaledCofactorMatrixDerivative( const Matrix3& M, const T& scale, - EigenPtr> scaled_dCdM) { + math::internal::FourthOrderTensor* scaled_dCdM) { /* See the convention for ordering the 9-by-9 derivatives at the top of the header file. */ const Matrix3 A = scale * M; diff --git a/multibody/fem/matrix_utilities.h b/multibody/fem/matrix_utilities.h index 56be4822a62c..9147c9c76ad7 100644 --- a/multibody/fem/matrix_utilities.h +++ b/multibody/fem/matrix_utilities.h @@ -5,32 +5,13 @@ #include #include "drake/common/eigen_types.h" +#include "drake/math/fourth_order_tensor.h" namespace drake { namespace multibody { namespace fem { namespace internal { -/* Some of the following functions involve calculations about a 4th order tensor -(call it A) of dimension 3*3*3*3. We follow the following convention to flatten -the 4th order tensor into 9*9 matrices that are organized as follows: - - l = 1 l = 2 l = 3 - ------------------------------------- - | | | | - j = 1 | Aᵢ₁ₖ₁ | Aᵢ₁ₖ₂ | Aᵢ₁ₖ₃ | - | | | | - ------------------------------------- - | | | | - j = 2 | Aᵢ₂ₖ₁ | Aᵢ₂ₖ₂ | Aᵢ₂ₖ₃ | - | | | | - ------------------------------------- - | | | | - j = 3 | Aᵢ₃ₖ₁ | Aᵢ₃ₖ₂ | Aᵢ₃ₖ₃ | - | | | | - ------------------------------------- -Namely the ik-th entry in the jl-th block corresponds to the value Aᵢⱼₖₗ. */ - /* Calculates the condition number for the given matrix A. The condition number is calculated via k(A) = σₘₐₓ / σₘᵢₙ @@ -67,7 +48,7 @@ void PolarDecompose(const Matrix3& F, EigenPtr> R, template void AddScaledRotationalDerivative( const Matrix3& R, const Matrix3& S, const T& scale, - EigenPtr> scaled_dRdF); + math::internal::FourthOrderTensor* scaled_dRdF); /* Calculates the cofactor matrix of the given input 3-by-3 matrix M. @pre cofactor != nullptr. @@ -85,7 +66,7 @@ void CalcCofactorMatrix(const Matrix3& M, EigenPtr> cofactor); template void AddScaledCofactorMatrixDerivative( const Matrix3& M, const T& scale, - EigenPtr> scaled_dCdM); + math::internal::FourthOrderTensor* scaled_dCdM); /* Given a size 3N vector with block structure with size 3 block entries Bᵢ where i ∈ V = {0, ..., N-1} and a permutation P on V, this function builds the diff --git a/multibody/fem/test/constitutive_model_test.cc b/multibody/fem/test/constitutive_model_test.cc index 1e47d75a621c..63918fb26a74 100644 --- a/multibody/fem/test/constitutive_model_test.cc +++ b/multibody/fem/test/constitutive_model_test.cc @@ -48,7 +48,7 @@ GTEST_TEST(ConstitutiveModelTest, InvalidModel) { "CalcFirstPiolaStressImpl.. to be correct.", NiceTypeName::Get(model))); - Eigen::Matrix dPdF; + math::internal::FourthOrderTensor dPdF; DRAKE_EXPECT_THROWS_MESSAGE( model.CalcFirstPiolaStressDerivative(data, &dPdF), fmt::format("The derived class {} must provide a shadow definition of " diff --git a/multibody/fem/test/constitutive_model_test_utilities.cc b/multibody/fem/test/constitutive_model_test_utilities.cc index df206effaf63..d0a77e206ea0 100644 --- a/multibody/fem/test/constitutive_model_test_utilities.cc +++ b/multibody/fem/test/constitutive_model_test_utilities.cc @@ -160,14 +160,14 @@ void TestdPdFIsDerivativeOfP() { data.UpdateData(deformation_gradients, previous_step_deformation_gradients); Matrix3 P; model.CalcFirstPiolaStress(data, &P); - Eigen::Matrix dPdF; + math::internal::FourthOrderTensor dPdF; model.CalcFirstPiolaStressDerivative(data, &dPdF); for (int i = 0; i < kSpaceDimension; ++i) { for (int j = 0; j < kSpaceDimension; ++j) { Matrix3d dPijdF; for (int k = 0; k < kSpaceDimension; ++k) { for (int l = 0; l < kSpaceDimension; ++l) { - dPijdF(k, l) = dPdF(3 * j + i, 3 * l + k).value(); + dPijdF(k, l) = dPdF(i, j, k, l).value(); } } EXPECT_TRUE(CompareMatrices( diff --git a/multibody/fem/test/matrix_utilities_test.cc b/multibody/fem/test/matrix_utilities_test.cc index ff49160a0ad3..0f638ecc0b64 100644 --- a/multibody/fem/test/matrix_utilities_test.cc +++ b/multibody/fem/test/matrix_utilities_test.cc @@ -68,8 +68,7 @@ GTEST_TEST(MatrixUtilitiesTest, AddScaledRotationalDerivative) { const Matrix3 F = MakeAutoDiffMatrix(3, 3); Matrix3 R, S; PolarDecompose(F, &R, &S); - Eigen::Matrix scaled_dRdF = - Eigen::Matrix::Zero(); + math::internal::FourthOrderTensor scaled_dRdF; AutoDiffXd scale = 1.23; AddScaledRotationalDerivative(R, S, scale, &scaled_dRdF); for (int i = 0; i < 3; ++i) { @@ -100,8 +99,7 @@ GTEST_TEST(MatrixUtilitiesTest, AddScaledCofactorMatrixDerivative) { const Matrix3 A = MakeAutoDiffMatrix(3, 3); Matrix3 C; CalcCofactorMatrix(A, &C); - Eigen::Matrix scaled_dCdA = - Eigen::Matrix::Zero(); + math::internal::FourthOrderTensor scaled_dCdA; AutoDiffXd scale = 1.23; AddScaledCofactorMatrixDerivative(A, scale, &scaled_dCdA); for (int i = 0; i < 3; ++i) { diff --git a/multibody/fem/volumetric_element.h b/multibody/fem/volumetric_element.h index c48563c1438f..c21f5e0e91e5 100644 --- a/multibody/fem/volumetric_element.h +++ b/multibody/fem/volumetric_element.h @@ -14,41 +14,6 @@ namespace multibody { namespace fem { namespace internal { -// TODO(xuchenhan-tri): Encapsulate the the memory layout of 4th order tensors -// and the contraction operation in a FourthOrderTensor class. -/* Helper function that performs a contraction between a 4th order tensor A - and two vectors u and v and returns a matrix B. In Einstein notation, the - contraction is: Bᵢₖ = uⱼ Aᵢⱼₖₗ vₗ. The 4th order tensor A of dimension - 3*3*3*3 is flattened to a 9*9 matrix that is organized as following - - l = 1 l = 2 l = 3 - ------------------------------------- - | | | | - j = 1 | Aᵢ₁ₖ₁ | Aᵢ₁ₖ₂ | Aᵢ₁ₖ₃ | - | | | | - ------------------------------------- - | | | | - j = 2 | Aᵢ₂ₖ₁ | Aᵢ₂ₖ₂ | Aᵢ₂ₖ₃ | - | | | | - ------------------------------------- - | | | | - j = 3 | Aᵢ₃ₖ₁ | Aᵢ₃ₖ₂ | Aᵢ₃ₖ₃ | - | | | | - ------------------------------------- -Namely the ik-th entry in the jl-th block corresponds to the value Aᵢⱼₖₗ. */ -template -void PerformDoubleTensorContraction( - const Eigen::Ref>& A, - const Eigen::Ref>& u, - const Eigen::Ref>& v, EigenPtr> B) { - B->setZero(); - for (int l = 0; l < 3; ++l) { - for (int j = 0; j < 3; ++j) { - *B += A.template block<3, 3>(3 * j, 3 * l) * u(j) * v(l); - } - } -} - /* The data struct that stores per element data for VolumetricElement. See FemElement for the requirement. We define it here instead of nesting it in the traits class below due to #17109. */ @@ -74,7 +39,7 @@ struct VolumetricElementData { std::array, num_quadrature_points> P; /* The derivative of first Piola stress with respect to the deformation gradient evaluated at quadrature points. */ - std::array, num_quadrature_points> dPdF; + std::array, num_quadrature_points> dPdF; }; /* Forward declaration needed for defining the traits below. */ @@ -354,8 +319,8 @@ class VolumetricElement /* Note that the scale is negated here because the tensor contraction gives the second derivative of energy, which is the opposite of the force derivative. */ - PerformDoubleTensorContraction( - data.dPdF[q], dSdX_transpose_[q].col(a), + data.dPdF[q].ContractWithVectors( + dSdX_transpose_[q].col(a), dSdX_transpose_[q].col(b) * reference_volume_[q] * -scale, &K_ab); AccumulateMatrixBlock(K_ab, a, b, K); } From 6f2c5922bc12e7279a83d1a335b16428b3213e39 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 21 Nov 2024 15:25:51 -0800 Subject: [PATCH 2/2] Address feature review comments --- math/fourth_order_tensor.cc | 38 +++++++++ math/fourth_order_tensor.h | 87 +++++++++++---------- math/test/fourth_order_tensor_test.cc | 73 ++++++++++++++++- multibody/fem/corotated_model.cc | 6 +- multibody/fem/linear_constitutive_model.cc | 19 +---- multibody/fem/linear_corotated_model.cc | 4 +- multibody/fem/matrix_utilities.cc | 74 +++++++++--------- multibody/fem/test/matrix_utilities_test.cc | 6 +- 8 files changed, 199 insertions(+), 108 deletions(-) diff --git a/math/fourth_order_tensor.cc b/math/fourth_order_tensor.cc index 57b66448cbe2..b769950c420c 100644 --- a/math/fourth_order_tensor.cc +++ b/math/fourth_order_tensor.cc @@ -24,6 +24,44 @@ void FourthOrderTensor::ContractWithVectors( } } +template +void FourthOrderTensor::SetAsOuterProduct( + const Eigen::Ref>& M, + const Eigen::Ref>& N) { + const auto M_vec = Eigen::Map>(M.data(), 9); + const auto N_vec = Eigen::Map>(N.data(), 9); + data_.noalias() = M_vec * N_vec.transpose(); +} + +template +FourthOrderTensor FourthOrderTensor::MakeSymmetricIdentity(T scale) { + T half_scale = 0.5 * scale; + FourthOrderTensor result; + /* The δᵢₖδⱼₗ term. */ + result.data_ = half_scale * Eigen::Matrix::Identity(); + for (int j = 0; j < 3; ++j) { + /* The δᵢₗδⱼₖ term. */ + for (int i = 0; i < 3; ++i) { + const int l = i; + const int k = j; + result.data_(3 * j + i, 3 * l + k) += half_scale; + } + } + return result; +} + +template +FourthOrderTensor FourthOrderTensor::MakeMajorIdentity(T scale) { + return FourthOrderTensor(scale * Eigen::Matrix::Identity()); +} + +template +FourthOrderTensor& FourthOrderTensor::operator+=( + const FourthOrderTensor& other) { + data_.noalias() += other.data_; + return *this; +} + } // namespace internal } // namespace math } // namespace drake diff --git a/math/fourth_order_tensor.h b/math/fourth_order_tensor.h index 54dba2f56830..e41be80ccee3 100644 --- a/math/fourth_order_tensor.h +++ b/math/fourth_order_tensor.h @@ -8,25 +8,7 @@ namespace math { namespace internal { /* This class provides functionalities related to 4th-order tensors of - dimension 3*3*3*3. The tensor is represented using a a 9*9 matrix that is - organized as following - - l = 1 l = 2 l = 3 - ------------------------------------- - | | | | - j = 1 | Aᵢ₁ₖ₁ | Aᵢ₁ₖ₂ | Aᵢ₁ₖ₃ | - | | | | - ------------------------------------- - | | | | - j = 2 | Aᵢ₂ₖ₁ | Aᵢ₂ₖ₂ | Aᵢ₂ₖ₃ | - | | | | - ------------------------------------- - | | | | - j = 3 | Aᵢ₃ₖ₁ | Aᵢ₃ₖ₂ | Aᵢ₃ₖ₃ | - | | | | - ------------------------------------- - Namely the ik-th entry in the jl-th block corresponds to the value Aᵢⱼₖₗ. - @tparam float, double, AutoDiffXd. */ + dimension 3*3*3*3. @tparam float, double, AutoDiffXd. */ template class FourthOrderTensor { public: @@ -47,12 +29,45 @@ class FourthOrderTensor { const Eigen::Ref>& v, EigenPtr> B) const; - /* Returns this 4th-order tensor encoded as a matrix according to the class - documentation. */ + /* Sets this 4th-order tensor as the outer product of the matrices (2nd-order + tensor) M and N. More specifically, in Einstein notion, sets Aᵢⱼₖₗ = MᵢⱼNₖₗ. + @warn This function assumes the input matrices are aliasing the data in this + 4th-order tensor. */ + void SetAsOuterProduct(const Eigen::Ref>& M, + const Eigen::Ref>& N); + + /* Returns a a scaled symmetric identity 4th-order tensor. In Einstein + notation, the result is scale * 1/2 * (δᵢₖδⱼₗ + δᵢₗδⱼₖ). */ + static FourthOrderTensor MakeSymmetricIdentity(T scale); + + /* Returns a a scaled major identity 4th-order tensor. In Einstein + notation, the result is scale * δᵢₖδⱼₗ. */ + static FourthOrderTensor MakeMajorIdentity(T scale); + + FourthOrderTensor& operator+=(const FourthOrderTensor& other); + + /* Returns this 4th-order tensor encoded as a 2D matrix. + The tensor is represented using a a 9*9 matrix organized as following + + l = 1 l = 2 l = 3 + ------------------------------------- + | | | | + j = 1 | Aᵢ₁ₖ₁ | Aᵢ₁ₖ₂ | Aᵢ₁ₖ₃ | + | | | | + ------------------------------------- + | | | | + j = 2 | Aᵢ₂ₖ₁ | Aᵢ₂ₖ₂ | Aᵢ₂ₖ₃ | + | | | | + ------------------------------------- + | | | | + j = 3 | Aᵢ₃ₖ₁ | Aᵢ₃ₖ₂ | Aᵢ₃ₖ₃ | + | | | | + ------------------------------------- + Namely the ik-th entry in the jl-th block corresponds to the value Aᵢⱼₖₗ. */ const MatrixType& data() const { return data_; } - /* Returns this 4th-order tensor encoded as a mutable matrix according to the - class documentation. */ + /* Returns this 4th-order tensor encoded as a mutable 2D matrix. + @see data() for the layout of the matrix. */ MatrixType& mutable_data() { return data_; } /* Returns the value of the 4th-order tensor at the given indices. @@ -75,26 +90,6 @@ class FourthOrderTensor { return data_(3 * j + i, 3 * l + k); } - /* Returns the value of the 4th-order tensor at the given indices, - interpreted as indices into the 9x9 matrix, using the convention layed out in - the class documentation. - @pre 0 <= i, j < 9. */ - const T& operator()(int i, int j) const { - DRAKE_ASSERT(0 <= i && i < 9); - DRAKE_ASSERT(0 <= j && j < 9); - return data_(i, j); - } - - /* Returns the value of the 4th-order tensor at the given indices, - interpreted as indices into the 9x9 matrix, using the convention layed out in - the class documentation. - @pre 0 <= i, j < 9. */ - T& operator()(int i, int j) { - DRAKE_ASSERT(0 <= i && i < 9); - DRAKE_ASSERT(0 <= j && j < 9); - return data_(i, j); - } - /* Sets the data of this 4th-order tensor to the given matrix, using the convention layed out in the class documentation. */ void set_data(const MatrixType& data) { data_ = data; } @@ -103,6 +98,12 @@ class FourthOrderTensor { MatrixType data_{MatrixType::Zero()}; }; +template +FourthOrderTensor operator+(const FourthOrderTensor& t1, + const FourthOrderTensor& t2) { + return FourthOrderTensor(t1) += t2; +} + } // namespace internal } // namespace math } // namespace drake diff --git a/math/test/fourth_order_tensor_test.cc b/math/test/fourth_order_tensor_test.cc index 9f3e02bd614d..4f6ac6a5105f 100644 --- a/math/test/fourth_order_tensor_test.cc +++ b/math/test/fourth_order_tensor_test.cc @@ -15,7 +15,7 @@ Eigen::Matrix MakeArbitraryMatrix() { Eigen::Matrix data; for (int i = 0; i < 9; ++i) { for (int j = 0; j < 9; ++j) { - data(i, j) = i + j; + data(i, j) = i + 2 * j; } } return data; @@ -46,9 +46,6 @@ GTEST_TEST(FourthOrderTensorTest, ConstructWithData) { /* Operator with four indices. */ EXPECT_EQ(tensor(0, 0, 0, 0), data(0, 0)); EXPECT_EQ(tensor(1, 1, 1, 1), data(4, 4)); - /* Operator with two indices. */ - EXPECT_EQ(tensor(0, 0), data(0, 0)); - EXPECT_EQ(tensor(3, 3), data(3, 3)); } GTEST_TEST(FourthOrderTensorTest, ContractWithVectors) { @@ -80,6 +77,74 @@ GTEST_TEST(FourthOrderTensorTest, ContractWithVectors) { EXPECT_TRUE(CompareMatrices(B, block * (u * v.transpose()).sum())); } +GTEST_TEST(FourthOrderTensorTest, SetAsOuterProduct) { + FourthOrderTensor t1, t2; + Matrix3d M, N; + M << 1, 0, 3, 0, 5, 0, 7, 0, 9; + N << 0, 2, 0, 4, 0, 6, 0, 8, 0; + t1.SetAsOuterProduct(M, N); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + for (int k = 0; k < 3; ++k) { + for (int l = 0; l < 3; ++l) { + EXPECT_EQ(t1(i, j, k, l), M(i, j) * N(k, l)); + } + } + } + } + t2.SetAsOuterProduct(N, M); + EXPECT_TRUE(CompareMatrices(t1.data(), t2.data().transpose())); +} + +GTEST_TEST(FourthOrderTensorTest, MakeSymmetricIdentity) { + const double scale = 1.23; + const FourthOrderTensor tensor = + FourthOrderTensor::MakeSymmetricIdentity(scale); + /* The expected result is scale * 1/2 * (δᵢₖδⱼₗ + δᵢₗδⱼₖ). */ + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + for (int k = 0; k < 3; ++k) { + for (int l = 0; l < 3; ++l) { + double expected_entry = 0.0; + if (i == k && j == l) expected_entry += 0.5 * scale; + if (i == l && j == k) expected_entry += 0.5 * scale; + EXPECT_EQ(tensor(i, j, k, l), expected_entry); + } + } + } + } +} + +GTEST_TEST(FourthOrderTensorTest, MakeMajorIdentity) { + const double scale = 1.23; + const FourthOrderTensor tensor = + FourthOrderTensor::MakeMajorIdentity(scale); + /* The expected result is scale * δᵢₖδⱼₗ. */ + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + for (int k = 0; k < 3; ++k) { + for (int l = 0; l < 3; ++l) { + double expected_entry = 0.0; + if (i == k && j == l) expected_entry += scale; + EXPECT_EQ(tensor(i, j, k, l), expected_entry); + } + } + } + } +} + +GTEST_TEST(FourthOrderTensorTest, Addition) { + const Eigen::Matrix data1 = MakeArbitraryMatrix(); + const Eigen::Matrix data2 = MakeArbitraryMatrix().transpose(); + FourthOrderTensor tensor1(data1); + const FourthOrderTensor tensor2(data2); + tensor1 += tensor2; + EXPECT_EQ(tensor1.data(), data1 + data2); + + const FourthOrderTensor tensor3 = tensor1 + tensor2; + EXPECT_EQ(tensor3.data(), data1 + 2.0 * data2); +} + } // namespace } // namespace internal } // namespace math diff --git a/multibody/fem/corotated_model.cc b/multibody/fem/corotated_model.cc index 1b170a3b9fb3..99c8f8a2c291 100644 --- a/multibody/fem/corotated_model.cc +++ b/multibody/fem/corotated_model.cc @@ -50,12 +50,8 @@ void CorotatedModel::CalcFirstPiolaStressDerivativeImpl( const Matrix3& R = data.R(); const Matrix3& S = data.S(); const Matrix3& JFinvT = data.JFinvT(); - const Vector& flat_JFinvT = - Eigen::Map>(JFinvT.data(), 3 * 3); auto& local_dPdF = (*dPdF); - /* The contribution from derivatives of Jm1. */ - local_dPdF.mutable_data().noalias() = - lambda_ * flat_JFinvT * flat_JFinvT.transpose(); + local_dPdF.SetAsOuterProduct(lambda_ * JFinvT, JFinvT); /* The contribution from derivatives of F. */ local_dPdF.mutable_data().diagonal().array() += 2.0 * mu_; /* The contribution from derivatives of R. */ diff --git a/multibody/fem/linear_constitutive_model.cc b/multibody/fem/linear_constitutive_model.cc index 655596fd5cf4..f3b318f6694b 100644 --- a/multibody/fem/linear_constitutive_model.cc +++ b/multibody/fem/linear_constitutive_model.cc @@ -29,21 +29,10 @@ LinearConstitutiveModel::LinearConstitutiveModel(const T& youngs_modulus, Keep in mind that the indices are laid out such that the ik-th entry in the jl-th block corresponds to the value dPᵢⱼ/dFₖₗ. */ /* First term. */ - dPdF_.set_data(mu_ * Eigen::Matrix::Identity()); - for (int k = 0; k < 3; ++k) { - /* Second term. */ - for (int l = 0; l < 3; ++l) { - const int i = l; - const int j = k; - dPdF_(3 * j + i, 3 * l + k) += mu_; - } - /* Third term. */ - for (int i = 0; i < 3; ++i) { - const int l = k; - const int j = i; - dPdF_(3 * j + i, 3 * l + k) += lambda_; - } - } + dPdF_.SetAsOuterProduct(Matrix3::Identity(), + lambda_ * Matrix3::Identity()); + dPdF_ += + math::internal::FourthOrderTensor::MakeSymmetricIdentity(2.0 * mu_); } template diff --git a/multibody/fem/linear_corotated_model.cc b/multibody/fem/linear_corotated_model.cc index e24ee3e67145..55c8190e4cea 100644 --- a/multibody/fem/linear_corotated_model.cc +++ b/multibody/fem/linear_corotated_model.cc @@ -86,13 +86,13 @@ void LinearCorotatedModel::CalcFirstPiolaStressDerivativeImpl( const Matrix3& R0 = data.R0(); auto& local_dPdF = (*dPdF); /* Add in μ * δₐᵢδⱼᵦ. */ - local_dPdF.set_data(mu_ * Eigen::Matrix::Identity()); + local_dPdF = math::internal::FourthOrderTensor::MakeMajorIdentity(mu_); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { for (int alpha = 0; alpha < 3; ++alpha) { for (int beta = 0; beta < 3; ++beta) { /* Add in μ * Rᵢᵦ Rₐⱼ + λ * Rₐᵦ * Rᵢⱼ. */ - local_dPdF(3 * j + i, 3 * beta + alpha) += + local_dPdF.mutable_data()(3 * j + i, 3 * beta + alpha) += mu_ * R0(i, beta) * R0(alpha, j) + lambda_ * R0(alpha, beta) * R0(i, j); } diff --git a/multibody/fem/matrix_utilities.cc b/multibody/fem/matrix_utilities.cc index 76bbbe279388..bc2c053f93c1 100644 --- a/multibody/fem/matrix_utilities.cc +++ b/multibody/fem/matrix_utilities.cc @@ -94,7 +94,7 @@ void AddScaledRotationalDerivative( for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { const int row_index = 3 * j + i; - (*scaled_dRdF)(row_index, column_index) += + scaled_dRdF->mutable_data()(row_index, column_index) += sRART(i, a) * A(j, b) - sRA(i, b) * RA(a, j); } } @@ -122,42 +122,42 @@ void AddScaledCofactorMatrixDerivative( /* See the convention for ordering the 9-by-9 derivatives at the top of the header file. */ const Matrix3 A = scale * M; - (*scaled_dCdM)(4, 0) += A(2, 2); - (*scaled_dCdM)(5, 0) += -A(1, 2); - (*scaled_dCdM)(7, 0) += -A(2, 1); - (*scaled_dCdM)(8, 0) += A(1, 1); - (*scaled_dCdM)(3, 1) += -A(2, 2); - (*scaled_dCdM)(5, 1) += A(0, 2); - (*scaled_dCdM)(6, 1) += A(2, 1); - (*scaled_dCdM)(8, 1) += -A(0, 1); - (*scaled_dCdM)(3, 2) += A(1, 2); - (*scaled_dCdM)(4, 2) += -A(0, 2); - (*scaled_dCdM)(6, 2) += -A(1, 1); - (*scaled_dCdM)(7, 2) += A(0, 1); - (*scaled_dCdM)(1, 3) += -A(2, 2); - (*scaled_dCdM)(2, 3) += A(1, 2); - (*scaled_dCdM)(7, 3) += A(2, 0); - (*scaled_dCdM)(8, 3) += -A(1, 0); - (*scaled_dCdM)(0, 4) += A(2, 2); - (*scaled_dCdM)(2, 4) += -A(0, 2); - (*scaled_dCdM)(6, 4) += -A(2, 0); - (*scaled_dCdM)(8, 4) += A(0, 0); - (*scaled_dCdM)(0, 5) += -A(1, 2); - (*scaled_dCdM)(1, 5) += A(0, 2); - (*scaled_dCdM)(6, 5) += A(1, 0); - (*scaled_dCdM)(7, 5) += -A(0, 0); - (*scaled_dCdM)(1, 6) += A(2, 1); - (*scaled_dCdM)(2, 6) += -A(1, 1); - (*scaled_dCdM)(4, 6) += -A(2, 0); - (*scaled_dCdM)(5, 6) += A(1, 0); - (*scaled_dCdM)(0, 7) += -A(2, 1); - (*scaled_dCdM)(2, 7) += A(0, 1); - (*scaled_dCdM)(3, 7) += A(2, 0); - (*scaled_dCdM)(5, 7) += -A(0, 0); - (*scaled_dCdM)(0, 8) += A(1, 1); - (*scaled_dCdM)(1, 8) += -A(0, 1); - (*scaled_dCdM)(3, 8) += -A(1, 0); - (*scaled_dCdM)(4, 8) += A(0, 0); + scaled_dCdM->mutable_data()(4, 0) += A(2, 2); + scaled_dCdM->mutable_data()(5, 0) += -A(1, 2); + scaled_dCdM->mutable_data()(7, 0) += -A(2, 1); + scaled_dCdM->mutable_data()(8, 0) += A(1, 1); + scaled_dCdM->mutable_data()(3, 1) += -A(2, 2); + scaled_dCdM->mutable_data()(5, 1) += A(0, 2); + scaled_dCdM->mutable_data()(6, 1) += A(2, 1); + scaled_dCdM->mutable_data()(8, 1) += -A(0, 1); + scaled_dCdM->mutable_data()(3, 2) += A(1, 2); + scaled_dCdM->mutable_data()(4, 2) += -A(0, 2); + scaled_dCdM->mutable_data()(6, 2) += -A(1, 1); + scaled_dCdM->mutable_data()(7, 2) += A(0, 1); + scaled_dCdM->mutable_data()(1, 3) += -A(2, 2); + scaled_dCdM->mutable_data()(2, 3) += A(1, 2); + scaled_dCdM->mutable_data()(7, 3) += A(2, 0); + scaled_dCdM->mutable_data()(8, 3) += -A(1, 0); + scaled_dCdM->mutable_data()(0, 4) += A(2, 2); + scaled_dCdM->mutable_data()(2, 4) += -A(0, 2); + scaled_dCdM->mutable_data()(6, 4) += -A(2, 0); + scaled_dCdM->mutable_data()(8, 4) += A(0, 0); + scaled_dCdM->mutable_data()(0, 5) += -A(1, 2); + scaled_dCdM->mutable_data()(1, 5) += A(0, 2); + scaled_dCdM->mutable_data()(6, 5) += A(1, 0); + scaled_dCdM->mutable_data()(7, 5) += -A(0, 0); + scaled_dCdM->mutable_data()(1, 6) += A(2, 1); + scaled_dCdM->mutable_data()(2, 6) += -A(1, 1); + scaled_dCdM->mutable_data()(4, 6) += -A(2, 0); + scaled_dCdM->mutable_data()(5, 6) += A(1, 0); + scaled_dCdM->mutable_data()(0, 7) += -A(2, 1); + scaled_dCdM->mutable_data()(2, 7) += A(0, 1); + scaled_dCdM->mutable_data()(3, 7) += A(2, 0); + scaled_dCdM->mutable_data()(5, 7) += -A(0, 0); + scaled_dCdM->mutable_data()(0, 8) += A(1, 1); + scaled_dCdM->mutable_data()(1, 8) += -A(0, 1); + scaled_dCdM->mutable_data()(3, 8) += -A(1, 0); + scaled_dCdM->mutable_data()(4, 8) += A(0, 0); } template diff --git a/multibody/fem/test/matrix_utilities_test.cc b/multibody/fem/test/matrix_utilities_test.cc index 0f638ecc0b64..d5f44f9feec2 100644 --- a/multibody/fem/test/matrix_utilities_test.cc +++ b/multibody/fem/test/matrix_utilities_test.cc @@ -76,7 +76,8 @@ GTEST_TEST(MatrixUtilitiesTest, AddScaledRotationalDerivative) { Matrix3 scaled_dRijdF; for (int k = 0; k < 3; ++k) { for (int l = 0; l < 3; ++l) { - scaled_dRijdF(k, l) = scaled_dRdF(3 * j + i, 3 * l + k).value(); + scaled_dRijdF(k, l) = + scaled_dRdF.data()(3 * j + i, 3 * l + k).value(); } } EXPECT_TRUE( @@ -107,7 +108,8 @@ GTEST_TEST(MatrixUtilitiesTest, AddScaledCofactorMatrixDerivative) { Matrix3 scaled_dCijdA; for (int k = 0; k < 3; ++k) { for (int l = 0; l < 3; ++l) { - scaled_dCijdA(k, l) = scaled_dCdA(3 * j + i, 3 * l + k).value(); + scaled_dCijdA(k, l) = + scaled_dCdA.data()(3 * j + i, 3 * l + k).value(); } } EXPECT_TRUE(