diff --git a/multibody/plant/BUILD.bazel b/multibody/plant/BUILD.bazel index 9e5f61a90537..661699d303d5 100644 --- a/multibody/plant/BUILD.bazel +++ b/multibody/plant/BUILD.bazel @@ -87,17 +87,6 @@ drake_cc_library( ], ) -drake_cc_library( - name = "dummy_physical_model", - testonly = 1, - srcs = ["dummy_physical_model.cc"], - hdrs = ["dummy_physical_model.h"], - visibility = ["//visibility:private"], - deps = [ - ":multibody_plant_core", - ], -) - drake_cc_library( name = "multibody_plant_core", srcs = [ @@ -105,9 +94,11 @@ drake_cc_library( "deformable_driver.cc", "deformable_model.cc", "discrete_update_manager.cc", + "dummy_physical_model.cc", "make_discrete_update_manager.cc", "multibody_plant.cc", "physical_model.cc", + "physical_model_collection.cc", "sap_driver.cc", "tamsi_driver.cc", ], @@ -117,11 +108,13 @@ drake_cc_library( "deformable_ids.h", "deformable_model.h", "discrete_update_manager.h", + "dummy_physical_model.h", "make_discrete_update_manager.h", "multibody_plant.h", "multibody_plant_discrete_update_manager_attorney.h", "multibody_plant_model_attorney.h", "physical_model.h", + "physical_model_collection.h", "sap_driver.h", "scalar_convertible_component.h", "tamsi_driver.h", @@ -1174,7 +1167,7 @@ drake_cc_googletest( drake_cc_googletest( name = "multibody_plant_scalar_conversion_test", deps = [ - ":dummy_physical_model", + ":multibody_plant_core", ":plant", ], ) @@ -1198,6 +1191,15 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "physical_model_collection_test", + deps = [ + ":multibody_plant_config_functions", + ":multibody_plant_core", + "//common/test_utilities:expect_throws_message", + ], +) + drake_cc_googletest( name = "wing_test", data = ["//multibody:models"], @@ -1213,7 +1215,6 @@ drake_cc_googletest( drake_cc_googletest( name = "discrete_update_manager_test", deps = [ - ":dummy_physical_model", ":multibody_plant_config_functions", ":multibody_plant_core", "//common/test_utilities:eigen_matrix_compare", @@ -1230,7 +1231,6 @@ drake_cc_googletest( drake_cc_googletest( name = "physical_model_test", deps = [ - ":dummy_physical_model", ":multibody_plant_core", "//common/test_utilities:eigen_matrix_compare", "//common/test_utilities:expect_throws_message", diff --git a/multibody/plant/physical_model.h b/multibody/plant/physical_model.h index 064ec7f30041..b39c998c4620 100644 --- a/multibody/plant/physical_model.h +++ b/multibody/plant/physical_model.h @@ -138,10 +138,13 @@ class PhysicalModel : public internal::ScalarConvertibleComponent { return DoToPhysicalModelPointerVariant(); } - protected: /* Returns the back pointer to the MultibodyPlant owning `this` PhysicalModel pre-finalize and nullptr post-finalize. */ const MultibodyPlant* plant() const { return owning_plant_; } + + protected: + /* Returns the mutable back pointer to the MultibodyPlant owning `this` + PhysicalModel pre-finalize and nullptr post-finalize. */ MultibodyPlant* mutable_plant() { return owning_plant_; } /* Derived classes must override this function to return their specific model diff --git a/multibody/plant/physical_model_collection.cc b/multibody/plant/physical_model_collection.cc new file mode 100644 index 000000000000..9d4856498808 --- /dev/null +++ b/multibody/plant/physical_model_collection.cc @@ -0,0 +1,71 @@ +#include "drake/multibody/plant/physical_model_collection.h" + +namespace drake { +namespace multibody { +namespace internal { + +template +DeformableModel& PhysicalModelCollection::AddDeformableModel( + std::unique_ptr> model) { + DRAKE_THROW_UNLESS(deformable_model_ == nullptr); + DRAKE_THROW_UNLESS(model != nullptr); + DRAKE_THROW_UNLESS(model->plant() == plant()); + deformable_model_ = model.get(); + owned_models_.emplace_back(std::move(model)); + return *deformable_model_; +} + +template +DummyPhysicalModel& PhysicalModelCollection::AddDummyModel( + std::unique_ptr> model) { + DRAKE_THROW_UNLESS(dummy_model_ == nullptr); + DRAKE_THROW_UNLESS(model != nullptr); + DRAKE_THROW_UNLESS(model->plant() == plant()); + dummy_model_ = model.get(); + owned_models_.emplace_back(std::move(model)); + return *dummy_model_; +} +template +bool PhysicalModelCollection::is_cloneable_to_double() const { + for (const auto& model : owned_models_) { + if (!model->is_cloneable_to_double()) { + return false; + } + } + return true; +} + +template +bool PhysicalModelCollection::is_cloneable_to_autodiff() const { + for (const auto& model : owned_models_) { + if (!model->is_cloneable_to_autodiff()) { + return false; + } + } + return true; +} + +template +bool PhysicalModelCollection::is_cloneable_to_symbolic() const { + for (const auto& model : owned_models_) { + if (!model->is_cloneable_to_symbolic()) { + return false; + } + } + return true; +} + +template +void PhysicalModelCollection::DeclareSystemResources() { + for (auto& model : owned_models_) { + model->DeclareSystemResources(); + } + owning_plant_ = nullptr; +} + +} // namespace internal +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::multibody::internal::PhysicalModelCollection) diff --git a/multibody/plant/physical_model_collection.h b/multibody/plant/physical_model_collection.h new file mode 100644 index 000000000000..a490461ff867 --- /dev/null +++ b/multibody/plant/physical_model_collection.h @@ -0,0 +1,147 @@ +#pragma once + +#include +#include +#include + +#include "drake/multibody/plant/deformable_model.h" +#include "drake/multibody/plant/dummy_physical_model.h" +#include "drake/multibody/plant/physical_model.h" + +namespace drake { +namespace multibody { + +template +class MultibodyPlant; + +namespace internal { + +/* A PhysicalModelCollection is a scalar convertible component of a + MultibodyPlant. Each MultibodyPlant owns exactly one PhysicalModelCollection. + It helps its owning MultibodyPlant manage its PhysicalModel components and + ensures that each type of PhysicalModel appears at most once in the + MultibodyPlant. PhysicalModels can be added prior to the call to + `DeclareSystemResources`. Once `DeclareSystemResources` is called, a + PhysicalModelCollection is considered to be "finalized" and new PhysicalModels + can no longer be added. The collection of PhysicalModel types is closed and + currently the only two types are DeformableModel and DummyPhysicalModel. */ +template +class PhysicalModelCollection : public ScalarConvertibleComponent { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PhysicalModelCollection); + + /* Constructs a PhysicalModelCollection owned by the given `plant`. The given + `plant` must outlive `this` PhysicalModelCollection. + @pre plant != nullptr */ + explicit PhysicalModelCollection(const MultibodyPlant* plant) + : owning_plant_(plant) { + DRAKE_DEMAND(plant != nullptr); + } + + ~PhysicalModelCollection() = default; + + /* Returns the back pointer to the MultibodyPlant owning `this` PhysicalModel + before DeclareSystemResources() are called and nullptr after. */ + const MultibodyPlant* plant() const { return owning_plant_; } + + /* Adds a DeformableModel to the collection and returns the added model if + successful. + @throws std::exception if a DeformableModel is already in the collection. + @throws std::exception if model is nullptr. + @throws std::exception if model->plant() != plant(). */ + DeformableModel& AddDeformableModel( + std::unique_ptr> model); + + /* (For testing only) Adds a DummyPhysicalModel to the collection and returns + the added model if successful. + @throws std::exception if a DummyPhysicalModel is already in the collection. + @throws std::exception if model is nullptr. + @throws std::exception if model->plant() != plant(). */ + DummyPhysicalModel& AddDummyModel( + std::unique_ptr> model); + + /* Returns a pointer to the stored DeformableModel if one exists and nullptr + otherwise. */ + const DeformableModel* deformable_model() const { + return deformable_model_; + } + DeformableModel* mutable_deformable_model() { return deformable_model_; } + + /* Returns a pointer to the stored DummyPhysicalModel if one exists and + nullptr otherwise. */ + const internal::DummyPhysicalModel* dummy_model() const { + return dummy_model_; + } + internal::DummyPhysicalModel* mutable_dummy_model() { + return dummy_model_; + } + + const std::vector>>& owned_models() const { + return owned_models_; + } + + bool is_cloneable_to_double() const override; + + bool is_cloneable_to_autodiff() const override; + + bool is_cloneable_to_symbolic() const override; + + /* Declare system resources on all owned models and nulls the back pointer to + the owning plant. For each PhysicalModelCollection object, this function + should be called once and only once by the owning plant, typically in the + process of MultibodyPlant::Finalize(). After the call to this function, new + models can no longer be added to `this` PhysicalModelCollection. */ + void DeclareSystemResources(); + + /* Creates a clone of `this` PhysicalModelCollection with the scalar type + `ScalarType`. The clone should be a deep scalar conversion of the original + PhysicalModelCollection with the exception of back pointer to the owning + plant. This function should only called from the scalar conversion + constructor of MultibodyPlant, with `new_plant` being the scalar converted + MultibodyPlant under construction. + @tparam ScalarType double, AutoDiffXd, or symbolic::Expression. + @param[in] new_plant pointer to the MultibodyPlant that will own the cloned + PhysicalModelCollection. Note that we take a mutable pointer to + MultibodyPlant here as required by cloning each individual PhysicalModel. + @throw std::exception if plant is nullptr. + @throw std::exception if DeclareSystemResources() has not been called on + `this` PhysicalModelCollection + @note `DeclareSystemResources()` is not called on the clone and needs to be + called from the plant owning the clone. */ + template + std::unique_ptr> CloneToScalar( + MultibodyPlant* new_plant) const { + DRAKE_THROW_UNLESS(owning_plant_ == nullptr); + std::unique_ptr> clone = + std::make_unique>(new_plant); + if (deformable_model_) { + auto deformable_model_clone = + std::unique_ptr>( + static_cast*>( + deformable_model_ + ->template CloneToScalar(new_plant) + .release())); + clone->AddDeformableModel(std::move(deformable_model_clone)); + } + if (dummy_model_) { + auto dummy_model_clone = std::unique_ptr>( + static_cast*>( + dummy_model_->template CloneToScalar(new_plant) + .release())); + clone->AddDummyModel(std::move(dummy_model_clone)); + } + return clone; + } + + private: + const MultibodyPlant* owning_plant_{nullptr}; + /* We maintain the invariant such that each `model` in `owned_models_` + satisfies `model->plant() == owning_plant_`. */ + std::vector>> owned_models_; + DeformableModel* deformable_model_{nullptr}; + DummyPhysicalModel* dummy_model_{nullptr}; +}; + +} // namespace internal +} // namespace multibody +} // namespace drake diff --git a/multibody/plant/test/physical_model_collection_test.cc b/multibody/plant/test/physical_model_collection_test.cc new file mode 100644 index 000000000000..004724662e79 --- /dev/null +++ b/multibody/plant/test/physical_model_collection_test.cc @@ -0,0 +1,153 @@ +#include "drake/multibody/plant/physical_model_collection.h" + +#include + +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/multibody/plant/deformable_model.h" +#include "drake/multibody/plant/dummy_physical_model.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/plant/multibody_plant_config_functions.h" + +namespace drake { +namespace multibody { +namespace internal { +namespace { + +GTEST_TEST(PhysicalModelCollectionTest, EmptyCollection) { + MultibodyPlant plant(0.01); + PhysicalModelCollection model_collection(&plant); + model_collection.DeclareSystemResources(); + + EXPECT_EQ(model_collection.dummy_model(), nullptr); + EXPECT_EQ(model_collection.mutable_dummy_model(), nullptr); + EXPECT_EQ(model_collection.deformable_model(), nullptr); + EXPECT_EQ(model_collection.mutable_deformable_model(), nullptr); + + EXPECT_TRUE(model_collection.is_cloneable_to_double()); + EXPECT_TRUE(model_collection.is_cloneable_to_autodiff()); + EXPECT_TRUE(model_collection.is_cloneable_to_symbolic()); + + EXPECT_EQ(model_collection.owned_models().size(), 0); + + /* Target owning plants. */ + MultibodyPlant double_plant(0.01); + MultibodyPlant autodiff_plant(0.01); + MultibodyPlant symbolic_plant(0.01); + + EXPECT_NO_THROW(model_collection.CloneToScalar(&double_plant)); + EXPECT_NO_THROW(model_collection.CloneToScalar(&autodiff_plant)); + EXPECT_NO_THROW( + model_collection.CloneToScalar(&symbolic_plant)); +} + +GTEST_TEST(PhysicalModelCollectionTest, AddEmptyModels) { + MultibodyPlant plant(0.01); + PhysicalModelCollection model_collection(&plant); + + auto dummy_model = std::make_unique>(&plant); + DummyPhysicalModel* dummy_model_ptr = dummy_model.get(); + model_collection.AddDummyModel(std::move(dummy_model)); + + EXPECT_EQ(model_collection.dummy_model(), dummy_model_ptr); + EXPECT_EQ(model_collection.mutable_dummy_model(), dummy_model_ptr); + EXPECT_EQ(model_collection.deformable_model(), nullptr); + EXPECT_EQ(model_collection.mutable_deformable_model(), nullptr); + EXPECT_EQ(model_collection.owned_models().size(), 1); + + auto deformable_model = std::make_unique>(&plant); + DeformableModel* deformable_model_ptr = deformable_model.get(); + model_collection.AddDeformableModel(std::move(deformable_model)); + + EXPECT_EQ(model_collection.dummy_model(), dummy_model_ptr); + EXPECT_EQ(model_collection.mutable_dummy_model(), dummy_model_ptr); + EXPECT_EQ(model_collection.deformable_model(), deformable_model_ptr); + EXPECT_EQ(model_collection.mutable_deformable_model(), deformable_model_ptr); + EXPECT_EQ(model_collection.owned_models().size(), 2); + + /* Mimic Finalizing the plant after all PhysicalModels have been added. */ + model_collection.DeclareSystemResources(); + + /* Neither the dummy model or the empty deformable model prevents scalar + conversion. */ + EXPECT_TRUE(model_collection.is_cloneable_to_double()); + EXPECT_TRUE(model_collection.is_cloneable_to_autodiff()); + EXPECT_TRUE(model_collection.is_cloneable_to_symbolic()); + /* Target owning plants. */ + MultibodyPlant double_plant(0.01); + MultibodyPlant autodiff_plant(0.01); + MultibodyPlant symbolic_plant(0.01); + + EXPECT_NO_THROW(model_collection.CloneToScalar(&double_plant)); + EXPECT_NO_THROW(model_collection.CloneToScalar(&autodiff_plant)); + EXPECT_NO_THROW( + model_collection.CloneToScalar(&symbolic_plant)); +} + +/* Adding a body to the deformable model prevents the model from scalar + converting to anything but double. Consequently, this prevents scalar + conversion to anything but double for the collection too. */ +GTEST_TEST(PhysicalModelCollectionTest, NonEmptyDeformableModel) { + systems::DiagramBuilder builder; + /* Use discrete plant and contact approximation that allows us to add + deformable bodies. */ + MultibodyPlantConfig plant_config; + plant_config.time_step = 0.01; + plant_config.discrete_contact_approximation = "sap"; + auto [plant, _] = AddMultibodyPlant(plant_config, &builder); + PhysicalModelCollection model_collection(&plant); + + DeformableModel& deformable_model = + model_collection.AddDeformableModel( + std::make_unique>(&plant)); + + /* Add a deformable body to the DeformableModel. */ + auto geometry = std::make_unique( + math::RigidTransformd{}, std::make_unique(1.0), + "sphere"); + deformable_model.RegisterDeformableBody( + std::move(geometry), fem::DeformableBodyConfig{}, 1.0); + + /* Mimic Finalizing the plant after all PhysicalModels have been added. */ + model_collection.DeclareSystemResources(); + + EXPECT_TRUE(model_collection.is_cloneable_to_double()); + EXPECT_FALSE(model_collection.is_cloneable_to_autodiff()); + EXPECT_FALSE(model_collection.is_cloneable_to_symbolic()); + /* Target owning plants. */ + MultibodyPlant double_plant(0.01); + MultibodyPlant autodiff_plant(0.01); + MultibodyPlant symbolic_plant(0.01); + EXPECT_NO_THROW(model_collection.CloneToScalar(&double_plant)); + DRAKE_EXPECT_THROWS_MESSAGE( + model_collection.CloneToScalar(&autodiff_plant), + ".*CloneToAutoDiffXd.*is_empty.*failed.*"); + DRAKE_EXPECT_THROWS_MESSAGE( + model_collection.CloneToScalar(&symbolic_plant), + ".*CloneToSymbolic.*is_empty.*failed.*"); +} + +GTEST_TEST(PhysicalModelCollectionTest, IncompatibleModel) { + MultibodyPlant plant(0.01); + PhysicalModelCollection model_collection(&plant); + model_collection.AddDeformableModel( + std::make_unique>(&plant)); + + MultibodyPlant another_plant(0.01); + auto model_with_wrong_plant = + std::make_unique>(&another_plant); + auto finalized_model = std::make_unique>(&plant); + finalized_model->DeclareSystemResources(); + auto ok_model = std::make_unique>(&plant); + EXPECT_THROW( + model_collection.AddDummyModel(std::move(model_with_wrong_plant)), + std::exception); + DRAKE_EXPECT_THROWS_MESSAGE( + model_collection.AddDummyModel(std::move(finalized_model)), + ".*AddDummyModel.*model->plant.*==.*plant.*failed.*"); + EXPECT_NO_THROW(model_collection.AddDummyModel(std::move(ok_model))); +} + +} // namespace +} // namespace internal +} // namespace multibody +} // namespace drake