[plant] Add MbP back-pointer into ContactResults struct (RobotLocomot…

…ion#16478)

This enables consumers of the ContactResults to actually understand
what the results mean, even in the presence of scalar conversion.
Consumers cannot store a long-lived pointer to the MbP on their own,
because it will be incorrect any time the Diagram gets rebuilt.

bindings/pydrake/multibody/plant_py.cc

@@ -110,7 +110,8 @@ void DoScalarDependentDefinitions(py::module m, T) {
         .def("num_hydroelastic_contacts", &Class::num_hydroelastic_contacts,
             cls_doc.num_hydroelastic_contacts.doc)
         .def("hydroelastic_contact_info", &Class::hydroelastic_contact_info,
-            py::arg("i"), cls_doc.hydroelastic_contact_info.doc);
+            py::arg("i"), cls_doc.hydroelastic_contact_info.doc)
+        .def("plant", &Class::plant, py_rvp::reference, cls_doc.plant.doc);
     DefCopyAndDeepCopy(&cls);
     AddValueInstantiation<Class>(m);
   }

bindings/pydrake/multibody/test/plant_test.py

@@ -1958,6 +1958,7 @@ def test_contact(self, T):
         # ContactResults
         contact_results = ContactResults()
         self.assertTrue(contact_results.num_point_pair_contacts() == 0)
+        self.assertIsNone(contact_results.plant())
         copy.copy(contact_results)
 
     def test_contact_model(self):

multibody/plant/contact_results.cc

@@ -47,10 +47,22 @@ ContactResults<T>& ContactResults<T>::operator=(
   }
 
   point_pairs_info_ = contact_results.point_pairs_info_;
+  plant_ = contact_results.plant_;
 
   return *this;
 }
 
+template <typename T>
+const MultibodyPlant<T>* ContactResults<T>::plant() const {
+  return plant_;
+}
+
+template <typename T>
+void ContactResults<T>::set_plant(const MultibodyPlant<T>* plant) {
+  DRAKE_THROW_UNLESS(plant != nullptr);
+  plant_ = plant;
+}
+
 template <typename T>
 void ContactResults<T>::Clear() {
   point_pairs_info_.clear();
@@ -59,6 +71,7 @@ void ContactResults<T>::Clear() {
   } else {
     hydroelastic_contact_vector_of_unique_ptrs().clear();
   }
+  plant_ = nullptr;
 }
 
 template <typename T>

multibody/plant/contact_results.h

@@ -15,6 +15,10 @@
 namespace drake {
 namespace multibody {
 
+#ifndef DRAKE_DOXYGEN_CXX
+template <typename T> class MultibodyPlant;
+#endif
+
 /**
  A container class storing the contact results information for each contact
  pair for a given state of the simulation. Note that copying this data structure
@@ -50,11 +54,18 @@ class ContactResults {
    method aborts. */
   const HydroelasticContactInfo<T>& hydroelastic_contact_info(int i) const;
 
+  /** Returns the plant that produced these contact results. In most cases the
+  result will be non-null, but default-constructed results might have nulls. */
+  const MultibodyPlant<T>* plant() const;
+
   // The following methods should only be called by MultibodyPlant and testing
   // fixtures and are undocumented rather than being made private with friends.
   #ifndef DRAKE_DOXYGEN_CXX
+  /* Sets the plant that produced these contact results. */
+  void set_plant(const MultibodyPlant<T>* plant);
+
   /* Clears the set of contact information for when the old data becomes
-   invalid. */
+   invalid. This includes clearing the `plant` back to nullptr. */
   void Clear();
 
   /* Add a new contact pair result to `this`. */
@@ -123,6 +134,8 @@ class ContactResults {
   std::variant<std::vector<const HydroelasticContactInfo<T>*>,
                std::vector<std::unique_ptr<HydroelasticContactInfo<T>>>>
       hydroelastic_contact_info_;
+
+  const MultibodyPlant<T>* plant_{nullptr};
 };
 
 }  // namespace multibody

multibody/plant/multibody_plant.cc

@@ -1327,11 +1327,12 @@ void MultibodyPlant<T>::CalcContactResultsContinuous(
   this->ValidateContext(context);
   DRAKE_DEMAND(contact_results != nullptr);
   contact_results->Clear();
+  contact_results->set_plant(this);
   if (num_collision_geometries() == 0) return;
 
   switch (contact_model_) {
     case ContactModel::kPoint:
-      CalcContactResultsContinuousPointPair(context, contact_results);
+      AppendContactResultsContinuousPointPair(context, contact_results);
       break;
 
     case ContactModel::kHydroelastic:
@@ -1340,7 +1341,7 @@ void MultibodyPlant<T>::CalcContactResultsContinuous(
 
     case ContactModel::kHydroelasticWithFallback:
       // Simply merge the contributions of each contact representation.
-      CalcContactResultsContinuousPointPair(context, contact_results);
+      AppendContactResultsContinuousPointPair(context, contact_results);
       AppendContactResultsContinuousHydroelastic(context, contact_results);
       break;
   }
@@ -1360,6 +1361,8 @@ void MultibodyPlant<T>::AppendContactResultsContinuousHydroelastic(
     const systems::Context<T>& context,
     ContactResults<T>* contact_results) const {
   this->ValidateContext(context);
+  DRAKE_DEMAND(contact_results != nullptr);
+  DRAKE_DEMAND(contact_results->plant() == this);
   const internal::HydroelasticContactInfoAndBodySpatialForces<T>&
       contact_info_and_spatial_body_forces =
           EvalHydroelasticContactForces(context);
@@ -1372,10 +1375,12 @@ void MultibodyPlant<T>::AppendContactResultsContinuousHydroelastic(
 }
 
 template <typename T>
-void MultibodyPlant<T>::CalcContactResultsContinuousPointPair(
+void MultibodyPlant<T>::AppendContactResultsContinuousPointPair(
     const systems::Context<T>& context,
     ContactResults<T>* contact_results) const {
   this->ValidateContext(context);
+  DRAKE_DEMAND(contact_results != nullptr);
+  DRAKE_DEMAND(contact_results->plant() == this);
 
   const std::vector<PenetrationAsPointPair<T>>& point_pairs =
       EvalPointPairPenetrations(context);
@@ -1389,7 +1394,6 @@ void MultibodyPlant<T>::CalcContactResultsContinuousPointPair(
       EvalGeometryQueryInput(context);
   const geometry::SceneGraphInspector<T>& inspector = query_object.inspector();
 
-  contact_results->Clear();
   for (size_t icontact = 0; icontact < point_pairs.size(); ++icontact) {
     const auto& pair = point_pairs[icontact];
     const GeometryId geometryA_id = pair.id_A;
@@ -1491,11 +1495,12 @@ void MultibodyPlant<T>::CalcContactResultsDiscrete(
     ContactResults<T>* contact_results) const {
   DRAKE_DEMAND(contact_results != nullptr);
   contact_results->Clear();
+  contact_results->set_plant(this);
   if (num_collision_geometries() == 0) return;
 
   switch (contact_model_) {
     case ContactModel::kPoint:
-      CalcContactResultsDiscretePointPair(context, contact_results);
+      AppendContactResultsDiscretePointPair(context, contact_results);
       break;
 
     case ContactModel::kHydroelastic:
@@ -1506,7 +1511,7 @@ void MultibodyPlant<T>::CalcContactResultsDiscrete(
 
     case ContactModel::kHydroelasticWithFallback:
       // Simply merge the contributions of each contact representation.
-      CalcContactResultsDiscretePointPair(context, contact_results);
+      AppendContactResultsDiscretePointPair(context, contact_results);
 
       // N.B. We are simply computing the hydro force as function of the state,
       // not the actual discrete approximation used by the contact solver.
@@ -1516,11 +1521,12 @@ void MultibodyPlant<T>::CalcContactResultsDiscrete(
 }
 
 template <typename T>
-void MultibodyPlant<T>::CalcContactResultsDiscretePointPair(
+void MultibodyPlant<T>::AppendContactResultsDiscretePointPair(
     const systems::Context<T>& context,
     ContactResults<T>* contact_results) const {
   this->ValidateContext(context);
   DRAKE_DEMAND(contact_results != nullptr);
+  DRAKE_DEMAND(contact_results->plant() == this);
   if (num_collision_geometries() == 0) return;
 
   const std::vector<PenetrationAsPointPair<T>>& point_pairs =
@@ -1543,7 +1549,6 @@ void MultibodyPlant<T>::CalcContactResultsDiscretePointPair(
   DRAKE_DEMAND(vn.size() >= num_contacts);
   DRAKE_DEMAND(vt.size() >= 2 * num_contacts);
 
-  contact_results->Clear();
   for (size_t icontact = 0; icontact < point_pairs.size(); ++icontact) {
     const auto& pair = point_pairs[icontact];
     const GeometryId geometryA_id = pair.id_A;

multibody/plant/multibody_plant.h

@@ -4425,30 +4425,35 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
 
   // Helper method to fill in the ContactResults given the current context when
   // the model is continuous.
+  // @param[out] contact_results is fully overwritten
   void CalcContactResultsContinuous(const systems::Context<T>& context,
                                     ContactResults<T>* contact_results) const;
 
   // Helper method for the continuous mode plant, to fill in the ContactResults
   // for the point pair model, given the current context. Called by
   // CalcContactResultsContinuous.
-  void CalcContactResultsContinuousPointPair(
+  // @param[in,out] contact_results is appended to
+  void AppendContactResultsContinuousPointPair(
       const systems::Context<T>& context,
       ContactResults<T>* contact_results) const;
 
   // Helper method to fill in `contact_results` with hydroelastic forces as a
   // function of the state stored in `context`.
+  // @param[in,out] contact_results is appended to
   void AppendContactResultsContinuousHydroelastic(
       const systems::Context<T>& context,
       ContactResults<T>* contact_results) const;
 
   // This method accumulates both point and hydroelastic contact results into
   // contact_results when the model is discrete.
+  // @param[out] contact_results is fully overwritten
   void CalcContactResultsDiscrete(const systems::Context<T>& context,
                                   ContactResults<T>* contact_results) const;
 
   // Helper method to fill in contact_results with point contact information
   // for the given state stored in context, when the model is discrete.
-  void CalcContactResultsDiscretePointPair(
+  // @param[in,out] contact_results is appended to
+  void AppendContactResultsDiscretePointPair(
       const systems::Context<T>& context,
       ContactResults<T>* contact_results) const;
 

multibody/plant/test/box_test.cc

@@ -77,6 +77,7 @@ class SlidingBoxTest : public ::testing::Test {
       const ContactResults<double>& contact_results =
           the_plant.get_contact_results_output_port()
               .Eval<ContactResults<double>>(the_context);
+      ASSERT_EQ(contact_results.plant(), &the_plant);
 
       // Only one contact pair.
       ASSERT_EQ(contact_results.num_point_pair_contacts(), 1);

multibody/plant/test/multibody_plant_hydroelastic_contact_results_output_test.cc

@@ -86,6 +86,7 @@ class HydroelasticContactResultsOutputTester : public ::testing::Test {
     const ContactResults<double>& contact_results =
         plant_->get_contact_results_output_port().Eval<ContactResults<double>>(
             *plant_context_);
+    DRAKE_DEMAND(contact_results.plant() == plant_);
     DRAKE_DEMAND(contact_results.num_hydroelastic_contacts() == 1);
     return contact_results.hydroelastic_contact_info(0);
   }