Declare MultibodyPlant's SceneGraph ports in constructor.

[joemasterjohn]

Addresses #13530.

• Declares the SceneGraph ports for MultibodyPlant in the constructor so that they have consistent ordering with the ScalarConverting constructor.
• Removes the requirement that this MBP has been registered with a SceneGraph to access SG input/output ports
• Adds tests for port indexing

---

This change is 

[joemasterjohn]

+@jwnimmer-tri for feature review, please.

Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @jwnimmer-tri)

[jwnimmer-tri]

Reviewed 5 of 5 files at r1.
Reviewable status: 8 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @joemasterjohn)

---

multibody/plant/multibody_plant.h, line 319 at r1 (raw file):

///
/// If %MultibodyPlant registers geometry with a SceneGraph via calls to
/// RegisterCollisionGeometry(), an input port for geometric queries will be

This sentence is inaccurate now.

---

multibody/plant/multibody_plant.h, line 575 at r1 (raw file):

  /// connection with a SceneGraph.
  /// @throws std::exception if this system was not registered with a
  /// SceneGraph.

nit Missing unit test for a direct call to this method to prove it works correctly without a SceneGraph yet?

---

multibody/plant/multibody_plant.h, line 646 at r1 (raw file):

  /// SceneGraph.
  /// @throws std::exception if this system was not registered with a
  /// SceneGraph.

nit Missing unit test for a direct call to this method to prove it works correctly without a SceneGraph yet?

---

multibody/plant/multibody_plant.cc, line 1078 at r1 (raw file):

    const systems::Context<double>& context) const {
  if (num_collision_geometries() > 0) {
    if (!geometry_query_port_.is_valid()) {

This check will never trip anymore; this is dead code.

Probably we should be checking geometry_query_port_.HasValue instead of this.

Possibly relates #11448 in the sense that Eval<QueryObject> should be abstracted into a common helper so that it can provide a good error message no matter how its called.

---

multibody/plant/multibody_plant.cc, line 1802 at r1 (raw file):

  if (num_collision_geometries() > 0) {
    if (!geometry_query_port_.is_valid()) {

This check will never trip anymore; this is dead code. Ditto on the comments for the same discussion around L1078.

---

multibody/plant/test/multibody_plant_test.cc, line 2931 at r1 (raw file):

      kTolerance, MatrixCompareType::relative));
}

Maybe multibody_plant_scalar_conversion_test.cc is a more appropriate home for this test case?

---

multibody/plant/test/multibody_plant_test.cc, line 2935 at r1 (raw file):

void CompareMultibodyPlantPortIndices(const MultibodyPlant<T>& plant_t,
                                      const MultibodyPlant<U>& plant_u) {
  // Check input ports

nit Missing punctuation on a sentence. Ditto below.

---

multibody/plant/test/multibody_plant_test.cc, line 2987 at r1 (raw file):

  std::unique_ptr<Diagram<AutoDiffXd>> autodiff_diagram =
      systems::System<double>::ToAutoDiffXd<Diagram>(*diagram.get());

nit *diagram.get() is redundant, just *diagram works. Ditto below.

[joemasterjohn]

Reviewable status: 4 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @jwnimmer-tri)

---

multibody/plant/multibody_plant.h, line 319 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This sentence is inaccurate now.

Interesting. I believe it was inaccurate before as well, since the ports were created at the time of RegisterAsSourceForSceneGraph(), not Finalize().

Done.

---

multibody/plant/multibody_plant.h, line 575 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Missing unit test for a direct call to this method to prove it works correctly without a SceneGraph yet?

Done.

---

multibody/plant/multibody_plant.h, line 646 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Missing unit test for a direct call to this method to prove it works correctly without a SceneGraph yet?

Done.

---

multibody/plant/multibody_plant.cc, line 1078 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This check will never trip anymore; this is dead code.

Probably we should be checking geometry_query_port_.HasValue instead of this.

Possibly relates #11448 in the sense that Eval<QueryObject> should be abstracted into a common helper so that it can provide a good error message no matter how its called.

Done.

---

multibody/plant/multibody_plant.cc, line 1802 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This check will never trip anymore; this is dead code. Ditto on the comments for the same discussion around L1078.

Done.

---

multibody/plant/test/multibody_plant_test.cc, line 2931 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Maybe multibody_plant_scalar_conversion_test.cc is a more appropriate home for this test case?

Done.

[jwnimmer-tri]

Reviewed 4 of 4 files at r2.
Reviewable status: 4 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @joemasterjohn)

---

multibody/plant/multibody_plant.h, line 319 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Interesting. I believe it was inaccurate before as well, since the ports were created at the time of RegisterAsSourceForSceneGraph(), not Finalize().

Done.

"Upon construction, " is superflous -- the common case is that ports always exist. Maybe just %MultibodyPlant declares an input port for ...?

---

multibody/plant/multibody_plant.cc, line 1078 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Done.

Can you point me to the unit test that proves this new conditional is correct and triggers with the intended error message?

---

multibody/plant/multibody_plant.cc, line 1802 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Done.

Can you point me to the unit test that proves this new conditional is correct and triggers with the intended error message?

---

multibody/plant/test/multibody_plant_test.cc, line 2934 at r2 (raw file):

GTEST_TEST(MultibodyPlantTest, SceneGraphPorts) {
    MultibodyPlant<double> plant(0.0);
    plant.Finalize();

The documentation for the port getters does not say that they can only be used post-finalize. Why are we finalizing here? I could see testing both pre- and post-finalize, but I can't imagine a reason for testing only post-finalize.

[joemasterjohn]

Reviewable status: 4 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @jwnimmer-tri)

---

multibody/plant/multibody_plant.h, line 319 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

"Upon construction, " is superflous -- the common case is that ports always exist. Maybe just %MultibodyPlant declares an input port for ...?

Done.

---

multibody/plant/multibody_plant.cc, line 1078 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Can you point me to the unit test that proves this new conditional is correct and triggers with the intended error message?

I couldn't find them anywhere, so I just added two new tests CalcPointPairPenetrationsNoDiagramException in multibody_plant_test and HydroelasitcWithFallbackNoDiagramException in multibody_plant_hydroelastic_test. PTAL.

---

multibody/plant/multibody_plant.cc, line 1802 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Can you point me to the unit test that proves this new conditional is correct and triggers with the intended error message?

I couldn't find them anywhere, so I just added two new tests CalcPointPairPenetrationsNoDiagramException in multibody_plant_test and HydroelasitcWithFallbackNoDiagramException in multibody_plant_hydroelastic_test. PTAL.

---

multibody/plant/test/multibody_plant_test.cc, line 2934 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The documentation for the port getters does not say that they can only be used post-finalize. Why are we finalizing here? I could see testing both pre- and post-finalize, but I can't imagine a reason for testing only post-finalize.

Ok, putting in a check for both pre and post finalize. Done.

[jwnimmer-tri]

Reviewed 3 of 3 files at r3.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @joemasterjohn)

---

multibody/plant/test/multibody_plant_hydroelastic_test.cc, line 307 at r3 (raw file):

  }

  void ConfigureWithoutDiagram(ContactModel model) {

This is a lot of duplicated code with the above.

It seems to me that the important point is not the lack of diagram, but that the ports are not connected. My initial reaction is that the method above could take a bool connect_scene_graph = true, to allow opting out of the connection.

When set we use AddMultibodyPlantSceneGraph as now. When unset, we construct and add the systems, but do not connect them. WDYT?

Alternatively, consolidating access to Eval<QueryObject> into a helper (which we'll have to do anyway to improve the error message in #11448) would reduce (perhaps even remove) the need for this extra unit test -- the non-hydro test would be sufficient on its own, through glass box reasoning.

[joemasterjohn]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @jwnimmer-tri)

---

multibody/plant/test/multibody_plant_hydroelastic_test.cc, line 307 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This is a lot of duplicated code with the above.

It seems to me that the important point is not the lack of diagram, but that the ports are not connected. My initial reaction is that the method above could take a bool connect_scene_graph = true, to allow opting out of the connection.

When set we use AddMultibodyPlantSceneGraph as now. When unset, we construct and add the systems, but do not connect them. WDYT?

Alternatively, consolidating access to Eval<QueryObject> into a helper (which we'll have to do anyway to improve the error message in #11448) would reduce (perhaps even remove) the need for this extra unit test -- the non-hydro test would be sufficient on its own, through glass box reasoning.

Easy enough to change to what you suggested. Done.

I'm having trouble seeing how a helper for Eval<QueryObject> would eliminate this test. Even if it had a safe fallback for when the port is not connected, MBP is still left without a query object and have to fail in the same place, no?

[jwnimmer-tri]

feature.

Reviewed 1 of 2 files at r4.
Reviewable status: needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @jwnimmer-tri)

---

multibody/plant/test/multibody_plant_hydroelastic_test.cc, line 307 at r3 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Easy enough to change to what you suggested. Done.

I'm having trouble seeing how a helper for Eval<QueryObject> would eliminate this test. Even if it had a safe fallback for when the port is not connected, MBP is still left without a query object and have to fail in the same place, no?

My mental model of the helper is that throws when the port is not connected -- that it encapsulates the user-friendly error reporting added in two places in the this PR. Then if everything within MbP calls that helper instead of Eval directly, we can know for sure that we only need to check the error message fidelity once, and that everything else would behave the same way, because they reuse the implementation.

[jwnimmer-tri]

Reviewed 1 of 2 files at r4, 2 of 2 files at r5.
Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @joemasterjohn)

---

multibody/plant/multibody_plant.h, line 3700 at r5 (raw file):

  // geometry_query_input_port is not connected, but geometry has been
  // registered.
  geometry::QueryObject<T> GeometryQueryEvalHelper(

This should return by const reference, to avoid extra (and potentially expensive) copies.

---

multibody/plant/multibody_plant.h, line 3700 at r5 (raw file):

  // geometry_query_input_port is not connected, but geometry has been
  // registered.
  geometry::QueryObject<T> GeometryQueryEvalHelper(

nit I'm not sure which one of these words is supposed to be the verb?

Consider maybe EvalGeometryQueryInput?

[joemasterjohn]

Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @joemasterjohn)

---

multibody/plant/test/multibody_plant_hydroelastic_test.cc, line 307 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

My mental model of the helper is that throws when the port is not connected -- that it encapsulates the user-friendly error reporting added in two places in the this PR. Then if everything within MbP calls that helper instead of Eval directly, we can know for sure that we only need to check the error message fidelity once, and that everything else would behave the same way, because they reuse the implementation.

Oh I see, just to consolidate the error message. Ok, that sounds good to me. I've added the helper function as you've described. PTAL.

Side Note:
I had to use this pattern of specializing symbolic::Expression versions MBP methods interacting with scene graph so it doesn't try to create templates for types unsupported by SceneGraph at this point.

@amcastro-tri has suggested avoiding the somewhat ugly pattern by using something like:

if constexpr (std::is_same<T, double>::value)
...
else if constexpr (std::is_same<T, symbolic::Expression>::value)
...

But in the situation with this helper method, even the method signature poses a problem because the linker won't be able to find a reference to QueryObject<symbolic::Expression>::QueryObject(QueryObject<symbolic::Expression> const&). Thoughts?

[joemasterjohn]

Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @jwnimmer-tri)

---

multibody/plant/multibody_plant.h, line 3700 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This should return by const reference, to avoid extra (and potentially expensive) copies.

Done.

---

multibody/plant/multibody_plant.h, line 3700 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit I'm not sure which one of these words is supposed to be the verb?

Consider maybe EvalGeometryQueryInput?

Good point. Changed it.

[joemasterjohn]

+@sherm1 for platform review, please. /cc @rpoyner-tri

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @jwnimmer-tri and @sherm1)

[jwnimmer-tri]

Reviewed 2 of 2 files at r6.
Reviewable status: LGTM missing from assignee sherm1(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @sherm1)

---

multibody/plant/test/multibody_plant_hydroelastic_test.cc, line 307 at r3 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Oh I see, just to consolidate the error message. Ok, that sounds good to me. I've added the helper function as you've described. PTAL.

Side Note:
I had to use this pattern of specializing symbolic::Expression versions MBP methods interacting with scene graph so it doesn't try to create templates for types unsupported by SceneGraph at this point.

@amcastro-tri has suggested avoiding the somewhat ugly pattern by using something like:

if constexpr (std::is_same<T, double>::value)
...
else if constexpr (std::is_same<T, symbolic::Expression>::value)
...

But in the situation with this helper method, even the method signature poses a problem because the linker won't be able to find a reference to QueryObject<symbolic::Expression>::QueryObject(QueryObject<symbolic::Expression> const&). Thoughts?

Check using MemberSceneGraph = ... for one approach -- declare the methods to use a typedef instead of QueryObject directly, and then swap out the typedef for dummy when using Expression.

[sherm1]

+@rpoyner-tri for platform review pass 1

Reviewable status: LGTM missing from assignees sherm1(platform),rpoyner-tri, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @rpoyner-tri and @sherm1)

[rpoyner-tri]

pending squash

Reviewed 1 of 2 files at r6.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @joemasterjohn and @sherm1)

---

multibody/plant/multibody_plant.h, line 352 at r6 (raw file):

 Description | | :--------: | :--------------: | :------: | :----------------: |
 :------------------- | |  material  | coulomb_friction |   yes¹   |
 CoulombFriction<T> | Static and Dynamic friction. |

The doxygen rendering of this is poor. What's the plan for fixing it?

[joemasterjohn]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform) (waiting on @jwnimmer-tri, @rpoyner-tri, and @sherm1)

---

multibody/plant/multibody_plant.h, line 352 at r6 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

The doxygen rendering of this is poor. What's the plan for fixing it?

Fixed. Not sure how it got changed, and why Reviewable didn't mark the whitespace changes... Maybe because it's in a block comment? weird.

[joemasterjohn]

Dismissed @rpoyner-tri from a discussion.
Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @jwnimmer-tri and @sherm1)

[sherm1]

Platform with a few minor comments. Needs a squash at least but would be better to rebase so there is only one commit here, with the commit message you want in the history for posterity.

Reviewed 2 of 5 files at r1, 1 of 4 files at r2, 2 of 2 files at r4, 1 of 2 files at r6, 1 of 1 files at r7.
Reviewable status: 8 unresolved discussions (waiting on @joemasterjohn)

---

multibody/plant/multibody_plant.h, line 3705 at r7 (raw file):

          "This MultibodyPlant registered geometry for contact handling. "
          "However its query input port (get_geometry_query_input_port()) "
          "is not connected.");

minor: I see that we know here that the port is not connected, but how do we know that geometry was registered? Could this be called in a circumstance where the port exists but no geometry has been registered? If there is some guarantee that prevents that from happening, please explain in the method comment. Better if you can enforce that with a DRAKE_DEMAND().

---

multibody/plant/multibody_plant.cc, line 1092 at r7 (raw file):

    const systems::Context<AutoDiffXd>& context) const {
  if (num_collision_geometries() > 0) {
    const auto &query_object = EvalGeometryQueryInput(context);;

nit should be auto& query_object

---

multibody/plant/multibody_plant.cc, line 1788 at r7 (raw file):

  if (num_collision_geometries() > 0) {
    const auto &query_object = EvalGeometryQueryInput(context);

nit should be auto& query_object

---

multibody/plant/test/multibody_plant_hydroelastic_test.cc, line 481 at r7 (raw file):

}

TEST_F(ContactModelTest, HydroelasitcWithFallbackDisconnectedPorts) {

nit: Hydroelasitc -> Hydroelastic

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc, line 108 at r7 (raw file):

}

GTEST_TEST(MultibodyPlantTest, RegisterSceneGraphPostFinalize) {

minor: needs a comment explaining what's being tested here. Also it's not obvious to me why it is significant to have a pre and post finalize test -- can you explain that for the naive reader?

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc, line 122 at r7 (raw file):

  const MultibodyPlant<AutoDiffXd>* autodiff_plant =
      dynamic_cast<const MultibodyPlant<AutoDiffXd>*>(
          autodiff_diagram->GetSystems()[0]);

minor: should note that this is assuming plant is index 0 -- there is no guarantee of that. Also should test that the cast didn't return a nullptr.

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc, line 127 at r7 (raw file):

}

GTEST_TEST(MultibodyPlantTest, RegisterSceneGraphPreFinalize) {

minor: needs a comment explaining what's being tested here

---

multibody/plant/test/multibody_plant_test.cc, line 1569 at r7 (raw file):

}

GTEST_TEST(MultibodyPlantTest, CalcPointPairPenetrationsDisconnectedPorts) {

minor: needs a comment explaining what's being tested here. I guess all this is just to generate an error message so we can test it?

[joemasterjohn]

Reviewable status: 3 unresolved discussions (waiting on @jwnimmer-tri, @rpoyner-tri, and @sherm1)

---

multibody/plant/multibody_plant.h, line 3705 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: I see that we know here that the port is not connected, but how do we know that geometry was registered? Could this be called in a circumstance where the port exists but no geometry has been registered? If there is some guarantee that prevents that from happening, please explain in the method comment. Better if you can enforce that with a DRAKE_DEMAND().

Actually that was a lazy copy/paste when I was consolidating the error messages. The Eval was usually called after a geometry_source_is_registered() (which I'm now realizing only verifies that a source id exists, not even that geometry has been registered). It seems like having the port connected with no registered collision geometry doesn't cause a problem for this method, I'll just change the error message to mention the port not being connected.

---

multibody/plant/test/multibody_plant_scalar_conversion_test.cc, line 108 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: needs a comment explaining what's being tested here. Also it's not obvious to me why it is significant to have a pre and post finalize test -- can you explain that for the naive reader?

It was previously significant because pre/post finalize produced inconsistent port indices, but that's the bug this PR is fixing.
These tests were actually just my original minimal code proofs that the bug was there. I guess it isn't very significant any more, except to show that the bug doesn't exist / it doesn't matter how you manipulate a plant vis-a-vis transmogrification.

I'll just leave in the case that actually produced inconsistent ordering (finalize then register with scene graph) as a verification of consistent port indices after scalar conversion.

---

multibody/plant/test/multibody_plant_test.cc, line 1569 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: needs a comment explaining what's being tested here. I guess all this is just to generate an error message so we can test it?

Yes it's all just to get to that error message. Collision geometry has to be registered for the program flow starting at EvalPointPairPenetrations()to reach the error produced from EvalGeometryQueryInput().

Now that I'm considering your comment, I see the test fixture friend class MultibodyPlantTesterthat already exists to help with calls to private methods. I'm just adding a wrapper in there so I can call EvalGeometryQueryInput()without all the setup.