From 9ba321d6795d74fdc272b82cb819af6ebb8c890f Mon Sep 17 00:00:00 2001 From: xuchenhan-tri <68254008+xuchenhan-tri@users.noreply.github.com> Date: Tue, 10 Nov 2020 13:00:56 -0800 Subject: [PATCH] Remove usage of unintialized values. (#14313) * Remove usage of unintialized values. Remove usage of uninitialized values in FemState and LinearElasticityModelCacheTest. - Initialize states in FemState's constructor. - Initialize strains in DeformationGradient's constructor. Fixes issue #14312. --- multibody/fem/dev/BUILD.bazel | 3 --- .../fem/dev/deformation_gradient_cache.h | 2 +- multibody/fem/dev/fem_state.h | 24 ++++++++++--------- .../fem/dev/linear_elasticity_model_cache.h | 4 ++-- multibody/fem/dev/test/fem_elasticity_test.cc | 21 ++++++++-------- multibody/fem/dev/test/fem_state_test.cc | 18 ++++---------- .../linear_elasticity_model_cache_test.cc | 13 ++++++++-- 7 files changed, 42 insertions(+), 43 deletions(-) diff --git a/multibody/fem/dev/BUILD.bazel b/multibody/fem/dev/BUILD.bazel index 8fd75ad45afb..f2e23f41ecad 100644 --- a/multibody/fem/dev/BUILD.bazel +++ b/multibody/fem/dev/BUILD.bazel @@ -191,7 +191,6 @@ drake_cc_library( drake_cc_googletest( name = "fem_elasticity_test", - tags = ["manual"], # Currently fails valgrind. deps = [ ":fem_elasticity", ":fem_state", @@ -206,7 +205,6 @@ drake_cc_googletest( drake_cc_googletest( name = "fem_state_test", - tags = ["manual"], # Currently fails valgrind. deps = [ ":element_cache", ":fem_state", @@ -225,7 +223,6 @@ drake_cc_googletest( drake_cc_googletest( name = "linear_elasticity_model_cache_test", - tags = ["manual"], # Currently fails valgrind. deps = [ ":linear_elasticity_model_cache", "//common/test_utilities:expect_throws_message", diff --git a/multibody/fem/dev/deformation_gradient_cache.h b/multibody/fem/dev/deformation_gradient_cache.h index 5b492824f80b..38f2a699eae5 100644 --- a/multibody/fem/dev/deformation_gradient_cache.h +++ b/multibody/fem/dev/deformation_gradient_cache.h @@ -75,7 +75,7 @@ class DeformationGradientCache { DeformationGradientCache(ElementIndex element_index, int num_quads) : element_index_(element_index), num_quads_(num_quads), - deformation_gradient_(num_quads) { + deformation_gradient_(num_quads, Matrix3::Identity()) { DRAKE_ASSERT(element_index.is_valid()); DRAKE_ASSERT(num_quads > 0); } diff --git a/multibody/fem/dev/fem_state.h b/multibody/fem/dev/fem_state.h index 345e2330f2b3..d9f8091dbe07 100644 --- a/multibody/fem/dev/fem_state.h +++ b/multibody/fem/dev/fem_state.h @@ -27,11 +27,15 @@ class FemState { another %FemState. */ DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(FemState); - FemState() = default; - - /** Constructs an %FemState with prescribed number of states. */ - explicit FemState(int num_generalized_positions) - : qdot_(num_generalized_positions), q_(num_generalized_positions) {} + /** Constructs an %FemState with prescribed states. + @param[in] q The prescribed generalized positions. + @param[in] qdot The prescribed time derivatives of generalized positions. + @pre `q` and `qdot` must have the same size. */ + FemState(const Eigen::Ref>& q, + const Eigen::Ref>& qdot) + : q_(q), qdot_(qdot) { + DRAKE_DEMAND(q.size() == qdot.size()); + } /** Takes ownership of the input `element_cache` and set it as the element cache that `this` %FemState owns. This method is usually called right after @@ -48,8 +52,8 @@ class FemState { /** Creates a deep identical copy of `this` %FemState with all its states and cache. Returns a unique pointer to the copy. */ std::unique_ptr> Clone() const { - auto clone = std::make_unique>(num_generalized_positions()); - clone->SetFrom(*this); + auto clone = std::make_unique>(q(), qdot()); + clone->element_cache_ = element_cache_; return clone; } @@ -86,13 +90,11 @@ class FemState { @{ */ void set_qdot(const Eigen::Ref>& value) { DRAKE_ASSERT(value.size() == qdot_.size()); - if (value == qdot_) return; mutable_qdot() = value; } void set_q(const Eigen::Ref>& value) { DRAKE_ASSERT(value.size() == q_.size()); - if (value == q_) return; mutable_q() = value; } /** @} */ @@ -128,10 +130,10 @@ class FemState { int num_generalized_positions() const { return q_.size(); } private: - // Time derivatives of generalized node positions. - VectorX qdot_; // Generalized node positions. VectorX q_; + // Time derivatives of generalized node positions. + VectorX qdot_; // Owned element cache quantities. std::vector>> element_cache_; }; diff --git a/multibody/fem/dev/linear_elasticity_model_cache.h b/multibody/fem/dev/linear_elasticity_model_cache.h index d5e258e13148..9209cd1b6069 100644 --- a/multibody/fem/dev/linear_elasticity_model_cache.h +++ b/multibody/fem/dev/linear_elasticity_model_cache.h @@ -38,8 +38,8 @@ class LinearElasticityModelCache : public DeformationGradientCache { @pre `num_quads` must be positive. */ LinearElasticityModelCache(ElementIndex element_index, int num_quads) : DeformationGradientCache(element_index, num_quads), - strain_(num_quads), - trace_strain_(num_quads) {} + strain_(num_quads, Matrix3::Zero()), + trace_strain_(num_quads, 0) {} /** Returns the infinitesimal strains evaluated at the quadrature locations for the associated element. */ diff --git a/multibody/fem/dev/test/fem_elasticity_test.cc b/multibody/fem/dev/test/fem_elasticity_test.cc index 2d3587aae210..89de9d5cebec 100644 --- a/multibody/fem/dev/test/fem_elasticity_test.cc +++ b/multibody/fem/dev/test/fem_elasticity_test.cc @@ -26,7 +26,10 @@ class ElasticityElementTest : public ::testing::Test { using QuadratureType = SimplexGaussianQuadrature; using ShapeType = LinearSimplexElement; - void SetUp() override { SetupElement(); } + void SetUp() override { + SetupElement(); + SetupState(); + } void SetupElement() { std::vector node_indices = {NodeIndex(0), NodeIndex(1), @@ -42,8 +45,6 @@ class ElasticityElementTest : public ::testing::Test { } void SetupState() { - state_.Resize(kDof); - state_.set_qdot(VectorX::Zero(kDof)); // Set arbitrary node positions and the gradient. Eigen::Matrix x; x << 0.18, 0.63, 0.54, 0.13, 0.92, 0.17, 0.03, 0.86, 0.85, 0.25, 0.53, 0.67; @@ -51,7 +52,8 @@ class ElasticityElementTest : public ::testing::Test { MatrixX::Identity(kDof, kDof); const VectorX x_autodiff = math::initializeAutoDiffGivenGradientMatrix(x, gradient); - state_.set_q(x_autodiff); + const VectorX v = VectorX::Zero(kDof); + state_ = std::make_unique>(x_autodiff, v); // Set up the element cache. auto linear_elasticity_cache = std::make_unique>( @@ -61,7 +63,7 @@ class ElasticityElementTest : public ::testing::Test { // ElementCache. cache.emplace_back(std::make_unique>( kDummyElementIndex, kNumQuads, std::move(linear_elasticity_cache))); - state_.ResetElementCache(std::move(cache)); + state_->ResetElementCache(std::move(cache)); } // Set an arbitrary reference positions such that the tetrahedron is not @@ -76,14 +78,14 @@ class ElasticityElementTest : public ::testing::Test { // Calculates the negative elastic force at state_. VectorX CalcNegativeElasticForce() const { VectorX neg_force(kDof); - fem_elasticity_->CalcNegativeElasticForce(state_, &neg_force); + fem_elasticity_->CalcNegativeElasticForce(*state_, &neg_force); return neg_force; } std::unique_ptr> fem_elasticity_; std::unique_ptr> linear_elasticity_; - FemState state_; + std::unique_ptr> state_; }; namespace { @@ -94,15 +96,14 @@ TEST_F(ElasticityElementTest, Basic) { } TEST_F(ElasticityElementTest, ElasticForceIsNegativeEnergyDerivative) { - SetupState(); - AutoDiffXd energy = fem_elasticity_->CalcElasticEnergy(state_); + AutoDiffXd energy = fem_elasticity_->CalcElasticEnergy(*state_); VectorX neg_force = CalcNegativeElasticForce(); EXPECT_TRUE(CompareMatrices(energy.derivatives(), neg_force, std::numeric_limits::epsilon())); // TODO(xuchenhan-tri) Modify this to account for damping forces and inertia // terms. VectorX residual(kDof); - fem_elasticity_->CalcResidual(state_, &residual); + fem_elasticity_->CalcResidual(*state_, &residual); EXPECT_TRUE(CompareMatrices(residual, neg_force)); } // TODO(xuchenhan-tri): Add TEST_F as needed for damping and inertia terms diff --git a/multibody/fem/dev/test/fem_state_test.cc b/multibody/fem/dev/test/fem_state_test.cc index e2f17958baff..994787bd3ab9 100644 --- a/multibody/fem/dev/test/fem_state_test.cc +++ b/multibody/fem/dev/test/fem_state_test.cc @@ -29,24 +29,15 @@ class MyElementCache final : public ElementCache { class FemStateTest : public ::testing::Test { protected: void SetUp() { - state_ = std::make_unique>(kDof); + state_ = std::make_unique>(q(), qdot()); // Create a couple of cache entries. state_->ResetElementCache(MakeElementCache()); - // Set default states. - SetStates(); } // Default values for the first kDof state entries. VectorX qdot() const { return Vector3(0.3, 0.4, 0.5); } VectorX q() const { return Vector3(0.7, 0.8, 0.9); } - // Set the states to default values. - void SetStates() { - // Set all states to their default values. - state_->set_qdot(qdot()); - state_->set_q(q()); - } - std::vector>> MakeElementCache() const { std::vector>> cache; std::vector data = {3, 5}; @@ -62,15 +53,13 @@ class FemStateTest : public ::testing::Test { }; // Verify setters and getters are working properly. -TEST_F(FemStateTest, SetAndGet) { - SetStates(); +TEST_F(FemStateTest, GetStates) { EXPECT_EQ(state_->qdot(), qdot()); EXPECT_EQ(state_->q(), q()); } // Verify resizing does not thrash existing values. TEST_F(FemStateTest, ConservativeResize) { - SetStates(); state_->Resize(2 * kDof); // The first kDof entres should remain unchanged. EXPECT_EQ(state_->qdot().head(kDof), qdot()); @@ -93,7 +82,8 @@ TEST_F(FemStateTest, Clone) { } TEST_F(FemStateTest, SetFrom) { - FemState dest; + // Set up an arbitrary state. + FemState dest(VectorX::Zero(1), VectorX::Zero(1)); dest.SetFrom(*state_); EXPECT_EQ(dest.qdot(), qdot()); EXPECT_EQ(dest.q(), q()); diff --git a/multibody/fem/dev/test/linear_elasticity_model_cache_test.cc b/multibody/fem/dev/test/linear_elasticity_model_cache_test.cc index b66b083b2d32..becd9c7350d4 100644 --- a/multibody/fem/dev/test/linear_elasticity_model_cache_test.cc +++ b/multibody/fem/dev/test/linear_elasticity_model_cache_test.cc @@ -11,8 +11,18 @@ constexpr int kNumQuads = 1; class LinearElasticityCacheTest : public ::testing::Test { protected: + void SetUp() { + linear_elasticity_cache_.UpdateCache({MakeDeformationGradient()}); + } LinearElasticityModelCache linear_elasticity_cache_{kElementIndex, kNumQuads}; + + // Make an arbitrary deformation gradient. + Matrix3 MakeDeformationGradient() { + Matrix3 F; + F << 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8, 8.9, 9.0; + return F; + } }; TEST_F(LinearElasticityCacheTest, LinearElasticityCacheInitialization) { @@ -24,8 +34,7 @@ TEST_F(LinearElasticityCacheTest, LinearElasticityCacheInitialization) { } TEST_F(LinearElasticityCacheTest, UpdateCache) { - const Matrix3 F = Matrix3::Random(); - linear_elasticity_cache_.UpdateCache({F}); + const Matrix3 F = MakeDeformationGradient(); const Matrix3 strain = 0.5 * (F + F.transpose()) - Matrix3::Identity(); const double trace_strain = strain.trace();