diff --git a/multibody/mpm/particle_data.cc b/multibody/mpm/particle_data.cc index eeb088484944..e44a4134bdc9 100644 --- a/multibody/mpm/particle_data.cc +++ b/multibody/mpm/particle_data.cc @@ -26,7 +26,7 @@ ConstitutiveModelVariant MakeConstitutiveModel( } template -DeformationGradientDataVariant MakeStrainData( +DeformationGradientDataVariant MakeDeformationGradientData( const fem::DeformableBodyConfig& config) { switch (config.material_model()) { case fem::MaterialModel::kCorotated: @@ -53,12 +53,14 @@ T ParticleData::ComputeTotalEnergy(const std::vector>& F, [&, this](auto&& model) { const Matrix3& F_p = F[p]; const Matrix3& F0_p = F0[p]; - using StrainDataType = typename std::decay_t::Data; - StrainDataType& strain_data_p = - std::get(strain_data_[p]); - strain_data_p.UpdateData(F_p, F0_p); + using DeformationGradientDataType = + typename std::remove_reference_t::Data; + DeformationGradientDataType& deformation_gradient_data_p = + std::get( + deformation_gradient_data_[p]); + deformation_gradient_data_p.UpdateData(F_p, F0_p); T Psi; - model.CalcElasticEnergyDensity(strain_data_p, &Psi); + model.CalcElasticEnergyDensity(deformation_gradient_data_p, &Psi); result += Psi * volume_[p]; }, constitutive_models_[p]); @@ -80,13 +82,16 @@ void ParticleData::ComputeKirchhoffStress( [&, this](auto& model) { const Matrix3& F_p = F[p]; const Matrix3& F0_p = F0[p]; - using StrainDataType = typename std::decay_t::Data; - StrainDataType& strain_data_p = - std::get(strain_data_[p]); - strain_data_p.UpdateData(F_p, F0_p); + using DeformationGradientDataType = + typename std::decay_t::Data; + DeformationGradientDataType& deformation_gradient_data_p = + std::get( + deformation_gradient_data_[p]); + deformation_gradient_data_p.UpdateData(F_p, F0_p); auto& tau_volume_p = (*volume_scaled_stress)[p]; const Matrix3& particle_F = F_[p]; - model.CalcFirstPiolaStress(strain_data_p, &tau_volume_p); + model.CalcFirstPiolaStress(deformation_gradient_data_p, + &tau_volume_p); tau_volume_p *= volume_[p] * particle_F.transpose(); }, constitutive_models_[p]); @@ -107,12 +112,15 @@ void ParticleData::ComputePK1StressDerivatives( [&, this](auto& model) { const Matrix3& F_p = F[p]; const Matrix3& F0_p = F0[p]; - using StrainDataType = typename std::decay_t::Data; - StrainDataType& strain_data_p = - std::get(strain_data_[p]); - strain_data_p.UpdateData(F_p, F0_p); + using DeformationGradientDataType = + typename std::decay_t::Data; + DeformationGradientDataType& deformation_gradient_data_p = + std::get( + deformation_gradient_data_[p]); + deformation_gradient_data_p.UpdateData(F_p, F0_p); auto& dPdF_volume_p = (*dPdF_volume)[p]; - model.CalcFirstPiolaStressDerivative(strain_data_p, &dPdF_volume_p); + model.CalcFirstPiolaStressDerivative(deformation_gradient_data_p, + &dPdF_volume_p); dPdF_volume_p.mutable_data() *= volume_[p]; }, constitutive_models_[p]); @@ -120,9 +128,9 @@ void ParticleData::ComputePK1StressDerivatives( } template -void ParticleData::Sample(const std::vector>& positions, - double total_volume, - const fem::DeformableBodyConfig& config) { +void ParticleData::AddParticles( + const std::vector>& positions, double total_volume, + const fem::DeformableBodyConfig& config) { DRAKE_THROW_UNLESS(total_volume > 0); DRAKE_THROW_UNLESS(!positions.empty()); const int num_new_particles = ssize(positions); @@ -130,21 +138,24 @@ void ParticleData::Sample(const std::vector>& positions, const double volume_per_particle = total_volume / num_new_particles; const ConstitutiveModelVariant constitutive_model = MakeConstitutiveModel(config); - const DeformationGradientDataVariant strain_data = - MakeStrainData(config); + const DeformationGradientDataVariant deformation_gradient_data = + MakeDeformationGradientData(config); for (int i = 0; i < num_new_particles; ++i) { - m_.push_back(mass_density * volume_per_particle); x_.emplace_back(positions[i].cast()); - v_.emplace_back(Vector3::Zero()); - F_.emplace_back(Matrix3::Identity()); - F0_.emplace_back(Matrix3::Identity()); - C_.emplace_back(Matrix3::Zero()); - volume_.push_back(volume_per_particle); - constitutive_models_.push_back(constitutive_model); - tau_volume_.emplace_back(Matrix3::Zero()); - in_constraint_.push_back(false); - strain_data_.push_back(strain_data); } + m_.insert(m_.end(), num_new_particles, mass_density * volume_per_particle); + v_.insert(v_.end(), num_new_particles, Vector3::Zero()); + F_.insert(F_.end(), num_new_particles, Matrix3::Identity()); + F0_.insert(F0_.end(), num_new_particles, Matrix3::Identity()); + C_.insert(C_.end(), num_new_particles, Matrix3::Zero()); + volume_.insert(volume_.end(), num_new_particles, volume_per_particle); + constitutive_models_.insert(constitutive_models_.end(), num_new_particles, + constitutive_model); + tau_volume_.insert(tau_volume_.end(), num_new_particles, Matrix3::Zero()); + in_constraint_.insert(in_constraint_.end(), num_new_particles, false); + deformation_gradient_data_.insert(deformation_gradient_data_.end(), + num_new_particles, + deformation_gradient_data); } } // namespace internal diff --git a/multibody/mpm/particle_data.h b/multibody/mpm/particle_data.h index 1965f1f16399..8e93fffe2941 100644 --- a/multibody/mpm/particle_data.h +++ b/multibody/mpm/particle_data.h @@ -29,9 +29,9 @@ using DeformationGradientDataVariant = fem::internal::LinearCorotatedModelData, fem::internal::LinearConstitutiveModelData>; -/* The collection of all physical attributes we care about for all particles. - All quantities are measured and expressed in the world frame (when - applicable). +/* The collection of all physical attributes we care about for all particles in + a full MPM model. All quantities are measured and expressed in the world frame + (when applicable). @tparam T The scalar type, can be a double, float, or AutoDiffXd. */ template class ParticleData { @@ -59,17 +59,12 @@ class ParticleData { const std::vector& in_constraint() const { return in_constraint_; } const std::vector>& tau_volume() const { return tau_volume_; } - /* Mutators */ - std::vector& mutable_m() { return m_; } + /* Mutable accessors */ std::vector>& mutable_x() { return x_; } std::vector>& mutable_v() { return v_; } std::vector>& mutable_F() { return F_; } std::vector>& mutable_F0() { return F0_; } std::vector>& mutable_C() { return C_; } - std::vector& mutable_volume() { return volume_; } - std::vector>& mutable_constitutive_models() { - return constitutive_models_; - } std::vector& mutable_in_constraint() { return in_constraint_; } std::vector>& mutable_tau_volume() { return tau_volume_; } @@ -89,7 +84,7 @@ class ParticleData { @param[in] F The deformation gradients of the particles. @param[in] F0 The deformation gradients at the previous time step. @param[out] tau_volume The Kirchhoff stress of each particle scaled by the - volume of the particle. + reference volume of the particle. @param[in] parallelism Specifies the degree of parallelism to use. @pre F and F0 have the same size and ordering as this ParticleData. @pre volume_scaled_stress != nullptr. @@ -125,35 +120,42 @@ class ParticleData { // TODO(xuchenhan-tri): Support Rayleigh damping for MPM. /* Appends default particle data to `this` ParticleData using the given parameters. The velocities of the particles are set to zeros. The deformation - gradients are set to identities. + gradients are set to identities. This function can be called repeatedly on a + single ParticleData object to add particles in multiple passes. @param positions The positions of the new particles in the world frame. @param total_volume The per particle volume of the new particles is given by total_volume divided by the number of new particles. + This is using the assumption that the new particles are + evenly distributed across the domain. @param config Provides the constitutive models and the mass densities of the new particles. @pre total_volume > 0. */ - void Sample(const std::vector>& positions, - double total_volume, - const fem::DeformableBodyConfig& config); + void AddParticles(const std::vector>& positions, + double total_volume, + const fem::DeformableBodyConfig& config); /* Per particle state and data. All of the following fields have the same size and ordering. */ - std::vector m_; // mass + /* State */ std::vector> x_; // positions std::vector> v_; // velocity std::vector> F_; // deformation gradient std::vector> F0_; // deformation gradient at the previous time step std::vector> C_; // affine velocity field - std::vector volume_; // reference volume + /* Constant data. */ + std::vector m_; // mass + std::vector volume_; // reference volume std::vector> constitutive_models_; + /* State dependent data. */ std::vector in_constraint_; // whether the particle is participating // in a constraint std::vector> tau_volume_; // Kirchhoff stress scaled by reference volume mutable std::vector> - strain_data_; // Deformation gradient dependent data that is used to - // calculate the energy density and its derivatives. + deformation_gradient_data_; // Deformation gradient dependent data that + // is used to calculate the energy density + // and its derivatives. }; } // namespace internal diff --git a/multibody/mpm/test/particle_data_test.cc b/multibody/mpm/test/particle_data_test.cc index 1dd9963d261b..222df6160ed8 100644 --- a/multibody/mpm/test/particle_data_test.cc +++ b/multibody/mpm/test/particle_data_test.cc @@ -27,6 +27,7 @@ using ScalarTypes = ::testing::Types; TYPED_TEST_SUITE(ParticleDataTest, ScalarTypes); TYPED_TEST(ParticleDataTest, Constructor) { + using T = TypeParam; EXPECT_EQ(this->dut_.num_particles(), 0); EXPECT_TRUE(this->dut_.m().empty()); EXPECT_TRUE(this->dut_.x().empty()); @@ -38,9 +39,11 @@ TYPED_TEST(ParticleDataTest, Constructor) { EXPECT_TRUE(this->dut_.constitutive_models().empty()); EXPECT_TRUE(this->dut_.in_constraint().empty()); EXPECT_TRUE(this->dut_.tau_volume().empty()); + EXPECT_TRUE((std::is_same_v::Scalar, T>)); } -TYPED_TEST(ParticleDataTest, Sample) { +TYPED_TEST(ParticleDataTest, AddParticles) { + using T = TypeParam; const Vector3d x0(1.0, 2.0, 3.0); const Vector3d x1(0.0, 2.1, 3.2); const std::vector positions = {x0, x1}; @@ -49,28 +52,57 @@ TYPED_TEST(ParticleDataTest, Sample) { fem::DeformableBodyConfig config; config.set_mass_density(2.0); - this->dut_.Sample(positions, total_volume, config); + this->dut_.AddParticles(positions, total_volume, config); EXPECT_EQ(this->dut_.num_particles(), 2); EXPECT_EQ(this->dut_.m().size(), 2); EXPECT_EQ(this->dut_.x().size(), 2); EXPECT_EQ(this->dut_.volume().size(), 2); + EXPECT_EQ(this->dut_.v().size(), 2); + EXPECT_EQ(this->dut_.C().size(), 2); + EXPECT_EQ(this->dut_.F().size(), 2); + EXPECT_EQ(this->dut_.F0().size(), 2); + EXPECT_EQ(this->dut_.tau_volume().size(), 2); + EXPECT_EQ(this->dut_.in_constraint().size(), 2); + EXPECT_EQ(this->dut_.constitutive_models().size(), 2); for (int i = 0; i < 2; ++i) { EXPECT_EQ(this->dut_.m()[i], 1.0); + EXPECT_EQ(this->dut_.x()[i], positions[i].cast()); EXPECT_EQ(this->dut_.volume()[i], 0.5); - - EXPECT_EQ(this->dut_.v()[i], Vector3::Zero()); - EXPECT_EQ(this->dut_.C()[i], Matrix3::Zero()); - EXPECT_EQ(this->dut_.F()[i], Matrix3::Identity()); + EXPECT_EQ(this->dut_.v()[i], Vector3::Zero()); + EXPECT_EQ(this->dut_.C()[i], Matrix3::Zero()); + EXPECT_EQ(this->dut_.F()[i], Matrix3::Identity()); + EXPECT_EQ(this->dut_.F0()[i], Matrix3::Identity()); + EXPECT_EQ(this->dut_.tau_volume()[i], Matrix3::Zero()); + EXPECT_EQ(this->dut_.in_constraint()[i], false); + EXPECT_TRUE(std::holds_alternative>( + this->dut_.constitutive_models()[i])); } + + /* Test mutable accessors. */ + this->dut_.mutable_x()[0] = Vector3::Ones(); + this->dut_.mutable_v()[0] = 2.0 * Vector3::Ones(); + this->dut_.mutable_F()[0] = 3.0 * Matrix3::Ones(); + this->dut_.mutable_F0()[0] = 4.0 * Matrix3::Ones(); + this->dut_.mutable_C()[0] = 5.0 * Matrix3::Ones(); + this->dut_.mutable_tau_volume()[0] = 6.0 * Matrix3::Ones(); + this->dut_.mutable_in_constraint()[0] = true; + EXPECT_EQ(this->dut_.x()[0], Vector3::Ones()); + EXPECT_EQ(this->dut_.v()[0], 2.0 * Vector3::Ones()); + EXPECT_EQ(this->dut_.F()[0], 3.0 * Matrix3::Ones()); + EXPECT_EQ(this->dut_.F0()[0], 4.0 * Matrix3::Ones()); + EXPECT_EQ(this->dut_.C()[0], 5.0 * Matrix3::Ones()); + EXPECT_EQ(this->dut_.tau_volume()[0], 6.0 * Matrix3::Ones()); + EXPECT_EQ(this->dut_.in_constraint()[0], true); } TYPED_TEST(ParticleDataTest, ComputeTotalEnergy) { - const std::vector> F = {Matrix3::Identity(), - Matrix3::Identity()}; - const std::vector> F0 = {Matrix3::Identity(), - Matrix3::Identity()}; + using T = TypeParam; + const std::vector> F = {Matrix3::Identity(), + Matrix3::Identity()}; + const std::vector> F0 = {Matrix3::Identity(), + Matrix3::Identity()}; const Vector3d x0(1.0, 2.0, 3.0); const Vector3d x1(0.0, 2.1, 3.2); @@ -79,41 +111,39 @@ TYPED_TEST(ParticleDataTest, ComputeTotalEnergy) { fem::DeformableBodyConfig config; config.set_mass_density(1.2); - this->dut_.Sample(positions, total_volume, config); + this->dut_.AddParticles(positions, total_volume, config); - const TypeParam zero_energy = this->dut_.ComputeTotalEnergy(F, F0); + const T zero_energy = this->dut_.ComputeTotalEnergy(F, F0); EXPECT_EQ(zero_energy, 0); - const std::vector> nontrivial_F = { - 2.0 * Matrix3::Identity(), - 0.5 * Matrix3::Identity()}; - const TypeParam nonzero_energy = - this->dut_.ComputeTotalEnergy(nontrivial_F, F0); + const std::vector> nontrivial_F = {2.0 * Matrix3::Identity(), + 0.5 * Matrix3::Identity()}; + const T nonzero_energy = this->dut_.ComputeTotalEnergy(nontrivial_F, F0); EXPECT_GT(nonzero_energy, 0); } TYPED_TEST(ParticleDataTest, ComputeKirchhoffStress) { - const std::vector> F = {Matrix3::Identity(), - Matrix3::Identity()}; - const std::vector> F0 = {Matrix3::Identity(), - Matrix3::Identity()}; + using T = TypeParam; + const std::vector> F = {Matrix3::Identity(), + Matrix3::Identity()}; + const std::vector> F0 = {Matrix3::Identity(), + Matrix3::Identity()}; const Vector3d x0(1.0, 2.0, 3.0); const Vector3d x1(0.0, 2.1, 3.2); const std::vector positions = {x0, x1}; const double total_volume = 1.0; const fem::DeformableBodyConfig config; - this->dut_.Sample(positions, total_volume, config); + this->dut_.AddParticles(positions, total_volume, config); - std::vector> tau_volume(2); + std::vector> tau_volume(2); this->dut_.ComputeKirchhoffStress(F, F0, &tau_volume); for (const auto& tau : tau_volume) { EXPECT_TRUE(tau.isZero()); } - const std::vector> nontrivial_F = { - 2.0 * Matrix3::Identity(), - 0.5 * Matrix3::Identity()}; + const std::vector> nontrivial_F = {2.0 * Matrix3::Identity(), + 0.5 * Matrix3::Identity()}; this->dut_.ComputeKirchhoffStress(nontrivial_F, F0, &tau_volume); for (const auto& tau : tau_volume) { EXPECT_GT(tau.norm(), 1.0); @@ -121,10 +151,11 @@ TYPED_TEST(ParticleDataTest, ComputeKirchhoffStress) { } TYPED_TEST(ParticleDataTest, ComputePK1StressDerivatives) { - const std::vector> F = {Matrix3::Identity(), - Matrix3::Identity()}; - const std::vector> F0 = {Matrix3::Identity(), - Matrix3::Identity()}; + using T = TypeParam; + const std::vector> F = {Matrix3::Identity(), + Matrix3::Identity()}; + const std::vector> F0 = {Matrix3::Identity(), + Matrix3::Identity()}; const Vector3d x0(1.0, 2.0, 3.0); const Vector3d x1(0.0, 2.1, 3.2); const std::vector positions = {x0, x1}; @@ -132,14 +163,14 @@ TYPED_TEST(ParticleDataTest, ComputePK1StressDerivatives) { fem::DeformableBodyConfig config; config.set_mass_density(1.2); - this->dut_.Sample(positions, total_volume, config); + this->dut_.AddParticles(positions, total_volume, config); - std::vector> dPdF_volume(2); + std::vector> dPdF_volume(2); this->dut_.ComputePK1StressDerivatives(F, F0, &dPdF_volume); /* Comfirm that the stress derivative is symmetric positive-definite. Here, we perform double contraction T : dPdF_volume : T where T is a test matrix and confirm the result is positive. */ - Eigen::Vector test_matrix; + Eigen::Vector test_matrix; for (int i = 0; i < 9; ++i) { test_matrix.setZero(); test_matrix(i) = 1.0; @@ -199,13 +230,13 @@ GTEST_TEST(ParticleDataDerivativeTest, FirstDerivative) { config.set_mass_density(1.2); ParticleData particle_data_ad; - particle_data_ad.Sample(positions, total_volume, config); + particle_data_ad.AddParticles(positions, total_volume, config); const AutoDiffXd energy = particle_data_ad.ComputeTotalEnergy(F_ad, F0_ad); const std::vector F_double = {math::DiscardGradient(F_ad[0])}; const std::vector F0_double = {MakeArbitraryMatrix3()}; ParticleData particle_data_double; - particle_data_double.Sample(positions, total_volume, config); + particle_data_double.AddParticles(positions, total_volume, config); /* We scale by an arbitrary factor of 1.23 so that particle_F is not the same as F0 to make the test more general. */ const std::vector particle_F = {1.23 * @@ -240,7 +271,7 @@ GTEST_TEST(ParticleDataDerivativeTest, SecondDerivative) { config.set_mass_density(1.2); ParticleData particle_data_ad; - particle_data_ad.Sample(positions, total_volume, config); + particle_data_ad.AddParticles(positions, total_volume, config); particle_data_ad.mutable_F() = {Matrix3::Identity()}; std::vector> tau_volume_ad(1); /* Now tau_volume_ad should store volume * P. */ @@ -249,7 +280,7 @@ GTEST_TEST(ParticleDataDerivativeTest, SecondDerivative) { const std::vector F_double = {math::DiscardGradient(F_ad[0])}; const std::vector F0_double = {MakeArbitraryMatrix3()}; ParticleData particle_data_double; - particle_data_double.Sample(positions, total_volume, config); + particle_data_double.AddParticles(positions, total_volume, config); std::vector> dPdF_volume_double(1); particle_data_double.ComputePK1StressDerivatives(F_double, F0_double, &dPdF_volume_double);