From d1064415757d36c7571ffd308f62345c48ba0007 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 21 Nov 2024 15:25:51 -0800 Subject: [PATCH] Address feature review comments --- math/fourth_order_tensor.cc | 37 +++++++++ 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, 198 insertions(+), 108 deletions(-) diff --git a/math/fourth_order_tensor.cc b/math/fourth_order_tensor.cc index 57b66448cbe2..1e3dd393dce2 100644 --- a/math/fourth_order_tensor.cc +++ b/math/fourth_order_tensor.cc @@ -24,6 +24,43 @@ 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; + result.data_ = half_scale * 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; + 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(