PhysicalModel retains const access to MbP postfinalize (RobotLocomoti…

…on#22300)

multibody/plant/deformable_model.cc

@@ -42,7 +42,7 @@ DeformableBodyId DeformableModel<T>::RegisterDeformableBody(
   if constexpr (std::is_same_v<T, double>) {
     /* Register the geometry with SceneGraph. */
     SceneGraph<T>& scene_graph = this->mutable_scene_graph();
-    SourceId source_id = this->plant()->get_source_id().value();
+    SourceId source_id = this->plant().get_source_id().value();
     /* All deformable bodies are registered with the world frame at the moment.
      */
     const FrameId world_frame_id = scene_graph.world_frame_id();
@@ -127,7 +127,7 @@ MultibodyConstraintId DeformableModel<T>::AddFixedConstraint(
   ThrowIfNotDouble(__func__);
   this->ThrowIfSystemResourcesDeclared(__func__);
   ThrowUnlessRegistered(__func__, body_A_id);
-  if (&this->plant()->get_body(body_B.index()) != &body_B) {
+  if (&this->plant().get_body(body_B.index()) != &body_B) {
     throw std::logic_error(
         fmt::format("The rigid body with name {} is not registered with the "
                     "MultibodyPlant owning the deformable model.",
@@ -386,14 +386,14 @@ DeformableModel<T>::BuildLinearVolumetricModelHelper(
 template <typename T>
 void DeformableModel<T>::DoDeclareSystemResources() {
   if (!is_empty()) {
-    if (this->plant()->get_discrete_contact_solver() !=
+    if (this->plant().get_discrete_contact_solver() !=
         DiscreteContactSolver::kSap) {
       throw std::runtime_error(
           "DeformableModel is only supported by the SAP contact solver. "
           "Please use `kSap`, `kLagged`, or `kSimilar` as the discrete contact "
           "approximation for the MultibodyPlant containing deformable bodies.");
     }
-    if (!this->plant()->is_discrete()) {
+    if (!this->plant().is_discrete()) {
       throw std::runtime_error(
           "Deformable body simulation is only supported "
           "with discrete time MultibodyPlant.");
@@ -430,7 +430,7 @@ void DeformableModel<T>::DoDeclareSystemResources() {
   /* Add gravity to each body. */
   for (const auto& [deformable_id, fem_model] : fem_models_) {
     const T& density = body_id_to_density_prefinalize_.at(deformable_id);
-    const Vector3<T>& gravity = this->plant()->gravity_field().gravity_vector();
+    const Vector3<T>& gravity = this->plant().gravity_field().gravity_vector();
     auto gravity_force =
         std::make_unique<GravityForceField<T>>(gravity, density);
     DeformableBodyIndex index = body_id_to_index_.at(deformable_id);

multibody/plant/multibody_plant.cc

@@ -1690,7 +1690,7 @@ template <typename T>
 internal::DummyPhysicalModel<T>& MultibodyPlant<T>::AddDummyModel(
     std::unique_ptr<internal::DummyPhysicalModel<T>> model) {
   DRAKE_MBP_THROW_IF_FINALIZED();
-  DRAKE_THROW_UNLESS(model->plant() == this);
+  DRAKE_THROW_UNLESS(&model->plant() == this);
   return physical_models_->AddDummyModel(std::move(model));
 }
 

multibody/plant/physical_model.cc

@@ -50,9 +50,9 @@ bool PhysicalModel<T>::is_cloneable_to_symbolic() const {
 
 template <typename T>
 void PhysicalModel<T>::DeclareSystemResources() {
-  DRAKE_DEMAND(owning_plant_ != nullptr);
+  DRAKE_DEMAND(mutable_owning_plant_ != nullptr);
   DoDeclareSystemResources();
-  owning_plant_ = nullptr;
+  mutable_owning_plant_ = nullptr;
 }
 
 template <typename T>
@@ -66,7 +66,7 @@ void PhysicalModel<T>::DeclareSceneGraphPorts() {
 template <typename T>
 void PhysicalModel<T>::ThrowIfSystemResourcesDeclared(
     const char* function_name) const {
-  if (owning_plant_ == nullptr) {
+  if (mutable_owning_plant_ == nullptr) {
     throw std::logic_error(
         fmt::format("Calls to {}() after system resources have been declared "
                     "are not allowed.",
@@ -77,7 +77,7 @@ void PhysicalModel<T>::ThrowIfSystemResourcesDeclared(
 template <typename T>
 void PhysicalModel<T>::ThrowIfSystemResourcesNotDeclared(
     const char* function_name) const {
-  if (owning_plant_ != nullptr) {
+  if (mutable_owning_plant_ != nullptr) {
     throw std::logic_error(
         fmt::format("Calls to {}() before system resources have been declared "
                     "are not allowed.",
@@ -87,17 +87,17 @@ void PhysicalModel<T>::ThrowIfSystemResourcesNotDeclared(
 
 template <typename T>
 geometry::SceneGraph<T>& PhysicalModel<T>::mutable_scene_graph() {
-  DRAKE_THROW_UNLESS(owning_plant_ != nullptr);
+  DRAKE_THROW_UNLESS(mutable_owning_plant_ != nullptr);
   return internal::MultibodyPlantModelAttorney<T>::mutable_scene_graph(
-      owning_plant_);
+      mutable_owning_plant_);
 }
 
 template <typename T>
 systems::DiscreteStateIndex PhysicalModel<T>::DeclareDiscreteState(
     const VectorX<T>& model_value) {
-  DRAKE_THROW_UNLESS(owning_plant_ != nullptr);
+  DRAKE_THROW_UNLESS(mutable_owning_plant_ != nullptr);
   return internal::MultibodyPlantModelAttorney<T>::DeclareDiscreteState(
-      owning_plant_, model_value);
+      mutable_owning_plant_, model_value);
 }
 
 template <typename T>
@@ -106,9 +106,9 @@ systems::LeafOutputPort<T>& PhysicalModel<T>::DeclareAbstractOutputPort(
     typename systems::LeafOutputPort<T>::AllocCallback alloc_function,
     typename systems::LeafOutputPort<T>::CalcCallback calc_function,
     std::set<systems::DependencyTicket> prerequisites_of_calc) {
-  DRAKE_THROW_UNLESS(owning_plant_ != nullptr);
+  DRAKE_THROW_UNLESS(mutable_owning_plant_ != nullptr);
   return internal::MultibodyPlantModelAttorney<T>::DeclareAbstractOutputPort(
-      owning_plant_, std::move(name), std::move(alloc_function),
+      mutable_owning_plant_, std::move(name), std::move(alloc_function),
       std::move(calc_function), std::move(prerequisites_of_calc));
 }
 
@@ -118,9 +118,9 @@ systems::LeafOutputPort<T>& PhysicalModel<T>::DeclareVectorOutputPort(
     typename systems::LeafOutputPort<T>::CalcVectorCallback
         vector_calc_function,
     std::set<systems::DependencyTicket> prerequisites_of_calc) {
-  DRAKE_THROW_UNLESS(owning_plant_ != nullptr);
+  DRAKE_THROW_UNLESS(mutable_owning_plant_ != nullptr);
   return internal::MultibodyPlantModelAttorney<T>::DeclareVectorOutputPort(
-      owning_plant_, std::move(name), model_vector,
+      mutable_owning_plant_, std::move(name), model_vector,
       std::move(vector_calc_function), std::move(prerequisites_of_calc));
 }
 

multibody/plant/physical_model.h

@@ -67,7 +67,7 @@ class PhysicalModel : public internal::ScalarConvertibleComponent<T> {
    `DeclareSystemResources()` through the call to `MultibodyPlant::Finalize()`.
    @pre owning_plant != nullptr. */
   explicit PhysicalModel(MultibodyPlant<T>* owning_plant)
-      : owning_plant_(owning_plant) {
+      : owning_plant_(*owning_plant), mutable_owning_plant_(owning_plant) {
     DRAKE_DEMAND(owning_plant != nullptr);
   }
 
@@ -92,9 +92,8 @@ class PhysicalModel : public internal::ScalarConvertibleComponent<T> {
   std::unique_ptr<PhysicalModel<ScalarType>> CloneToScalar(
       MultibodyPlant<ScalarType>* plant) const {
     DRAKE_THROW_UNLESS(plant != nullptr);
-    /* The plant owning `this` PhysicalModel must be finalized and consequently
-     the plant back pointer is nulled out. */
-    if (this->plant() != nullptr) {
+    /* The plant owning `this` PhysicalModel must be finalized. */
+    if (!this->plant().is_finalized()) {
       throw std::logic_error(
           "The owning plant of the PhysicalModel to be cloned must be "
           "finalized.");
@@ -141,14 +140,13 @@ class PhysicalModel : public internal::ScalarConvertibleComponent<T> {
     return DoToPhysicalModelPointerVariant();
   }
 
-  /* Returns the back pointer to the MultibodyPlant owning `this`
-   PhysicalModel pre-finalize and nullptr post-finalize. */
-  const MultibodyPlant<T>* plant() const { return owning_plant_; }
+  /* Returns MultibodyPlant owning `this` PhysicalModel. */
+  const MultibodyPlant<T>& plant() const { return owning_plant_; }
 
  protected:
   /* Returns the mutable back pointer to the MultibodyPlant owning `this`
    PhysicalModel pre-finalize and nullptr post-finalize. */
-  MultibodyPlant<T>* mutable_plant() { return owning_plant_; }
+  MultibodyPlant<T>* mutable_plant() { return mutable_owning_plant_; }
 
   /* Derived classes must override this function to return their specific model
    variant. */
@@ -223,9 +221,11 @@ class PhysicalModel : public internal::ScalarConvertibleComponent<T> {
           systems::System<T>::all_sources_ticket()});
 
  private:
-  /* Back pointer to the MultibodyPlant owning `this` PhysicalModel. Only valid
-   pre-finalize and nulled out post-finalize. */
-  MultibodyPlant<T>* owning_plant_{nullptr};
+  /* Back pointer to the MultibodyPlant owning `this` PhysicalModel. */
+  const MultibodyPlant<T>& owning_plant_;
+  /* Mutable back pointer to the MultibodyPlant owning `this` PhysicalModel.
+   Nullified post-finalize. */
+  MultibodyPlant<T>* mutable_owning_plant_{nullptr};
 };
 
 }  // namespace multibody

multibody/plant/physical_model_collection.cc

@@ -78,7 +78,7 @@ template <typename T>
 void PhysicalModelCollection<T>::ThrowForIncompatibleModel(
     const PhysicalModel<T>& model) const {
   if (!owned_models_.empty() &&
-      model.plant() != owned_models_.back()->plant()) {
+      &model.plant() != &owned_models_.back()->plant()) {
     throw std::runtime_error(
         "The given model belongs to a different MultibodyPlant.");
   }

multibody/plant/physical_model_collection.h

@@ -39,15 +39,15 @@ class PhysicalModelCollection : public ScalarConvertibleComponent<T> {
    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(). */
+   @throws std::exception if model.plant() is different from plant(). */
   DeformableModel<T>& AddDeformableModel(
       std::unique_ptr<DeformableModel<T>> 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(). */
+   @throws std::exception if model.plant() is different from plant(). */
   DummyPhysicalModel<T>& AddDummyModel(
       std::unique_ptr<DummyPhysicalModel<T>> model);
 

multibody/plant/test/physical_model_collection_test.cc

@@ -66,6 +66,7 @@ GTEST_TEST(PhysicalModelCollectionTest, AddEmptyModels) {
 
   /* Mimic Finalizing the plant after all PhysicalModels have been added. */
   model_collection.DeclareSystemResources();
+  plant.Finalize();
 
   /* Neither the dummy model or the empty deformable model prevents scalar
    conversion. */
@@ -109,6 +110,7 @@ GTEST_TEST(PhysicalModelCollectionTest, NonEmptyDeformableModel) {
 
   /* Mimic Finalizing the plant after all PhysicalModels have been added. */
   model_collection.DeclareSystemResources();
+  plant.Finalize();
 
   EXPECT_TRUE(model_collection.is_cloneable_to_double());
   EXPECT_FALSE(model_collection.is_cloneable_to_autodiff());
@@ -127,14 +129,9 @@ GTEST_TEST(PhysicalModelCollectionTest, IncompatibleModel) {
   MultibodyPlant<double> another_plant(0.01);
   auto model_with_wrong_plant =
       std::make_unique<DummyPhysicalModel<double>>(&another_plant);
-  auto finalized_model = std::make_unique<DummyPhysicalModel<double>>(&plant);
-  finalized_model->DeclareSystemResources();
   auto ok_model = std::make_unique<DummyPhysicalModel<double>>(&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)),
+      model_collection.AddDummyModel(std::move(model_with_wrong_plant)),
       "The given model belongs to a different MultibodyPlant.");
   EXPECT_NO_THROW(model_collection.AddDummyModel(std::move(ok_model)));
 }