Adds careful handling of massless links to avoid singular forests

[sherm1]

This is the 7th installment in the PR train for #20225, following #21695. This is a small one for a change!

What's here:

• Careful handling of massless links to avoid cutting loops in a way that would unnecessarily leave a terminal massless link.
• Modified existing massless-body unit tests to test for the improved behavior.

What's not here:

• Combining LinkComposites onto a single Mobod
• Some standalone graph-walking algorithms needed by MbP
• RemoveLink()
• Modifications to MbP to use this stuff

There are no user-visible changes here.

---

This change is 

[sherm1]

+@joemasterjohn for feature review please. This is a small PR but adds a tricky algorithm that needs your wise eyeballs on it. The unit tests here are just modified versions of existing tests (with better behavior now) -- I'm soliciting ideas for more/better tests if you find yourself unconvinced by these.

Reviewable status: LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers

[joemasterjohn]

feature with a few nits. +@ggould-tri for platform review, please.

Reviewed 3 of 3 files at r1, all commit messages.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee ggould-tri(platform)

---

multibody/topology/test/spanning_forest_test.cc line 1054 at r1 (raw file):

  EXPECT_EQ(ssize(graph.joints()), 7);
  EXPECT_TRUE(graph.link_by_index(BodyIndex(7)).is_shadow());
  EXPECT_EQ(graph.link_by_index(BodyIndex(7)).primary_link(), BodyIndex(4));

nit: duplicated 4 lines of code here.

Code quote:

  EXPECT_EQ(ssize(graph.links()), 8);  // Added a shadow.
  EXPECT_EQ(ssize(graph.joints()), 7);
  EXPECT_TRUE(graph.link_by_index(BodyIndex(7)).is_shadow());
  EXPECT_EQ(graph.link_by_index(BodyIndex(7)).primary_link(), BodyIndex(4));

  EXPECT_EQ(ssize(graph.links()), 8);  // Added a shadow.
  EXPECT_EQ(ssize(graph.joints()), 7);
  EXPECT_TRUE(graph.link_by_index(BodyIndex(7)).is_shadow());
  EXPECT_EQ(graph.link_by_index(BodyIndex(7)).primary_link(), BodyIndex(4));

---

multibody/topology/test/spanning_forest_test.cc line 1074 at r1 (raw file):

  EXPECT_EQ(ssize(graph.joints()), 7);
  EXPECT_TRUE(graph.link_by_index(BodyIndex(7)).is_shadow());
  EXPECT_EQ(graph.link_by_index(BodyIndex(7)).primary_link(), BodyIndex(4));

nit: duplicated 4 lines of code here.

Code quote:

  EXPECT_EQ(ssize(graph.links()), 8);
  EXPECT_EQ(ssize(graph.joints()), 7);
  EXPECT_TRUE(graph.link_by_index(BodyIndex(7)).is_shadow());
  EXPECT_EQ(graph.link_by_index(BodyIndex(7)).primary_link(), BodyIndex(4));

  EXPECT_EQ(ssize(graph.links()), 8);
  EXPECT_EQ(ssize(graph.joints()), 7);
  EXPECT_TRUE(graph.link_by_index(BodyIndex(7)).is_shadow());
  EXPECT_EQ(graph.link_by_index(BodyIndex(7)).primary_link(), BodyIndex(4));

---

multibody/topology/test/spanning_forest_test.cc line 1082 at r1 (raw file):

            BodyIndex(7));

  // Changing both 3 and 4 to massless breaks the loop at 6 instead of 4.

nit: Just double checking for code coverage:

This tests case 3 of your newly added code (3 (massless) has a single (massless) successor link 4, recurse).
The prior graph with only link 3 massless tests case 1 (single massful successor link 4, keep moving).
And case 2 (terminating massless link) is tested above here in SimpleTrees.

Does that seem correct?

[sherm1]

Reviewable status: LGTM missing from assignee ggould-tri(platform)

---

multibody/topology/test/spanning_forest_test.cc line 1054 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

nit: duplicated 4 lines of code here.

Done, thanks. I think I mangled this in a manual merge :(

---

multibody/topology/test/spanning_forest_test.cc line 1074 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

nit: duplicated 4 lines of code here.

Done

---

multibody/topology/test/spanning_forest_test.cc line 1082 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

nit: Just double checking for code coverage:

This tests case 3 of your newly added code (3 (massless) has a single (massless) successor link 4, recurse).
The prior graph with only link 3 massless tests case 1 (single massful successor link 4, keep moving).
And case 2 (terminating massless link) is tested above here in SimpleTrees.

Does that seem correct?

Yes. I verified this in the debugger and annotated the spots that hit cases 1, 2, and 3.

[ggould-tri]

; nits. I love the direction this is going.

Reviewed 3 of 3 files at r1.
Reviewable status: 2 unresolved discussions

---

multibody/topology/spanning_forest.cc line 456 at r1 (raw file):

/* Grows each Tree by one level with two exceptions:
   - TODO(sherm1) if we're optimizing composites, keep growing LinkComposites
      as far as possible since they use only a single Mobod, and

nit: Not clear from this what the remaining action or defect of this TODO is.

Code quote:

   - TODO(sherm1) if we're optimizing composites, keep growing LinkComposites
      as far as possible since they use only a single Mobod, and

---

multibody/topology/spanning_forest.cc line 589 at r1 (raw file):

        data_.dynamics_ok = false;
        data_.why_no_dynamics = fmt::format(
            "Link {} on {} joint {} is a terminal, articulated, massless "

minor: Is this message still accurate? It seems to me that with this change it's a massless, terminal weld-cluster of links; it is neither necessary nor sufficient for any individual link to be both articulated and terminal?

So possibly this message should talk about the joint, not the specific link?

Code quote:

terminal

[sherm1]

Thanks, Joe and Grant! Grant, PTAL and LMK if I cleared up the confusion I caused.

Reviewable status: complete! all discussions resolved, LGTM from assignees ggould-tri(platform),joemasterjohn

---

multibody/topology/spanning_forest.cc line 456 at r1 (raw file):

Previously, ggould-tri wrote…

nit: Not clear from this what the remaining action or defect of this TODO is.

Done, thanks -- poorly worded. I took another crack at it, PTAL. There are ultimately two exceptions, but the first one (packing a bunch of links onto the same body) isn't in this PR.

---

multibody/topology/spanning_forest.cc line 589 at r1 (raw file):

Previously, ggould-tri wrote…

minor: Is this message still accurate? It seems to me that with this change it's a massless, terminal weld-cluster of links; it is neither necessary nor sufficient for any individual link to be both articulated and terminal?

So possibly this message should talk about the joint, not the specific link?

OK, this is still correct now. (A consequence of the poorly-worded TODO) Currently all the links are treated separately even if they are connected by welds. But in the next PR the basic unit grows from a single link to a link composite, and the whole composite could be massless as you are anticipating.

[joemasterjohn]

Reviewed 1 of 1 files at r2, 1 of 1 files at r3, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees ggould-tri(platform),joemasterjohn

---

multibody/topology/test/spanning_forest_test.cc line 1082 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Yes. I verified this in the debugger and annotated the spots that hit cases 1, 2, and 3.

Awesome, thanks!