Skip to content

Commit

Permalink
Address feature review comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@xuchenhan-tri
xuchenhan-tri committed Nov 21, 2024
1 parent c8fc667 commit d106441
Show file tree
Hide file tree
Showing 8 changed files with 198 additions and 108 deletions.
37 changes: 37 additions & 0 deletions math/fourth_order_tensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,43 @@ void FourthOrderTensor<T>::ContractWithVectors(
}
}

template <typename T>
void FourthOrderTensor<T>::SetAsOuterProduct(
const Eigen::Ref<const Matrix3<T>>& M,
const Eigen::Ref<const Matrix3<T>>& N) {
const auto M_vec = Eigen::Map<const Vector<T, 9>>(M.data(), 9);
const auto N_vec = Eigen::Map<const Vector<T, 9>>(N.data(), 9);
data_.noalias() = M_vec * N_vec.transpose();
}

template <typename T>
FourthOrderTensor<T> FourthOrderTensor<T>::MakeSymmetricIdentity(T scale) {
T half_scale = 0.5 * scale;
FourthOrderTensor<T> result;
result.data_ = half_scale * Eigen::Matrix<T, 9, 9>::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 <typename T>
FourthOrderTensor<T> FourthOrderTensor<T>::MakeMajorIdentity(T scale) {
return FourthOrderTensor<T>(scale * Eigen::Matrix<T, 9, 9>::Identity());
}

template <typename T>
FourthOrderTensor<T>& FourthOrderTensor<T>::operator+=(
const FourthOrderTensor<T>& other) {
data_.noalias() += other.data_;
return *this;
}

} // namespace internal
} // namespace math
} // namespace drake
Expand Down
87 changes: 44 additions & 43 deletions math/fourth_order_tensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <typename T>
class FourthOrderTensor {
public:
Expand All @@ -47,12 +29,45 @@ class FourthOrderTensor {
const Eigen::Ref<const Vector3<T>>& v,
EigenPtr<Matrix3<T>> 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<const Matrix3<T>>& M,
const Eigen::Ref<const Matrix3<T>>& N);

/* Returns a a scaled symmetric identity 4th-order tensor. In Einstein
notation, the result is scale * 1/2 * (δᵢₖδⱼₗ + δᵢₗδⱼₖ). */
static FourthOrderTensor<T> MakeSymmetricIdentity(T scale);

/* Returns a a scaled major identity 4th-order tensor. In Einstein
notation, the result is scale * δᵢₖδⱼₗ. */
static FourthOrderTensor<T> MakeMajorIdentity(T scale);

FourthOrderTensor<T>& operator+=(const FourthOrderTensor<T>& 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.
Expand All @@ -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; }
Expand All @@ -103,6 +98,12 @@ class FourthOrderTensor {
MatrixType data_{MatrixType::Zero()};
};

template <typename T>
FourthOrderTensor<T> operator+(const FourthOrderTensor<T>& t1,
const FourthOrderTensor<T>& t2) {
return FourthOrderTensor<T>(t1) += t2;
}

} // namespace internal
} // namespace math
} // namespace drake
73 changes: 69 additions & 4 deletions math/test/fourth_order_tensor_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ Eigen::Matrix<double, 9, 9> MakeArbitraryMatrix() {
Eigen::Matrix<double, 9, 9> 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;
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -80,6 +77,74 @@ GTEST_TEST(FourthOrderTensorTest, ContractWithVectors) {
EXPECT_TRUE(CompareMatrices(B, block * (u * v.transpose()).sum()));
}

GTEST_TEST(FourthOrderTensorTest, SetAsOuterProduct) {
FourthOrderTensor<double> 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<double> tensor =
FourthOrderTensor<double>::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<double> tensor =
FourthOrderTensor<double>::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<double, 9, 9> data1 = MakeArbitraryMatrix();
const Eigen::Matrix<double, 9, 9> data2 = MakeArbitraryMatrix().transpose();
FourthOrderTensor<double> tensor1(data1);
const FourthOrderTensor<double> tensor2(data2);
tensor1 += tensor2;
EXPECT_EQ(tensor1.data(), data1 + data2);

const FourthOrderTensor<double> tensor3 = tensor1 + tensor2;
EXPECT_EQ(tensor3.data(), data1 + 2.0 * data2);
}

} // namespace
} // namespace internal
} // namespace math
Expand Down
6 changes: 1 addition & 5 deletions multibody/fem/corotated_model.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,12 +50,8 @@ void CorotatedModel<T>::CalcFirstPiolaStressDerivativeImpl(
const Matrix3<T>& R = data.R();
const Matrix3<T>& S = data.S();
const Matrix3<T>& JFinvT = data.JFinvT();
const Vector<T, 3 * 3>& flat_JFinvT =
Eigen::Map<const Vector<T, 3 * 3>>(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. */
Expand Down
19 changes: 4 additions & 15 deletions multibody/fem/linear_constitutive_model.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,21 +29,10 @@ LinearConstitutiveModel<T>::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<T, 9, 9>::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<T>::Identity(),
lambda_ * Matrix3<T>::Identity());
dPdF_ +=
math::internal::FourthOrderTensor<T>::MakeSymmetricIdentity(2.0 * mu_);
}

template <typename T>
Expand Down
4 changes: 2 additions & 2 deletions multibody/fem/linear_corotated_model.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,13 +86,13 @@ void LinearCorotatedModel<T>::CalcFirstPiolaStressDerivativeImpl(
const Matrix3<T>& R0 = data.R0();
auto& local_dPdF = (*dPdF);
/* Add in μ * δₐᵢδⱼᵦ. */
local_dPdF.set_data(mu_ * Eigen::Matrix<T, 9, 9>::Identity());
local_dPdF = math::internal::FourthOrderTensor<T>::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);
}
Expand Down
74 changes: 37 additions & 37 deletions multibody/fem/matrix_utilities.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
Expand Down Expand Up @@ -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<T> 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 <typename T>
Expand Down
Loading

0 comments on commit d106441

Please sign in to comment.