Skip to content

Commit

Permalink
Fast subgraph calculator and new tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@sherm1
sherm1 committed Oct 13, 2023
1 parent 8f8e43f commit 82846ef
Show file tree
Hide file tree
Showing 3 changed files with 161 additions and 63 deletions.
40 changes: 28 additions & 12 deletions multibody/topology/link_joint_graph.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -442,12 +442,37 @@ LinkIndex LinkJointGraph::FindFirstCommonAncestor(LinkIndex link1_index,
return forest().mobod_to_link(mobod_ancestor);
}

// Our composite_links collection doesn't include lone Links that aren't welded
// to anything. The return from this function must include every Link, with
// the World link in the first set (even if nothing is welded to it).
std::vector<std::set<LinkIndex>> LinkJointGraph::FindSubgraphsOfWeldedLinks()
const {
if (!forest_is_valid()) return FindSubgraphsOfWeldedLinksNoModel();

std::vector<std::set<LinkIndex>> subgraphs;

// First, collect all the precomputed Composite Links. World is always
// the first one, even if nothing is welded to it.
for (const std::vector<LinkIndex>& composite : composite_links()) {
subgraphs.emplace_back(
std::set<LinkIndex>(composite.cbegin(), composite.cend()));
}

// Finally, make one-Link subgraphs for Links that aren't in any composite.
for (const Link& link : links()) {
if (link.composite().is_valid()) continue;
subgraphs.emplace_back(std::set<LinkIndex>{link.index()});
}

return subgraphs;
}

// Note that this algorithm works with LinkJointGraph as an undirected graph.
// We don't care which Link is the parent or child; just that there is a
// weld Joint connecting two Links.
// TODO(sherm1) Reimplement using already-built Composites.
std::vector<std::set<LinkIndex>> LinkJointGraph::FindSubgraphsOfWeldedLinks()
const {
// This slow version doesn't require that a SpanningForest has been built.
std::vector<std::set<LinkIndex>>
LinkJointGraph::FindSubgraphsOfWeldedLinksNoModel() const {
std::vector<bool> visited(ssize(links()), false);
std::vector<std::set<LinkIndex>> subgraphs;

Expand Down Expand Up @@ -479,15 +504,6 @@ std::vector<std::set<LinkIndex>> LinkJointGraph::FindSubgraphsOfWeldedLinks()
return subgraphs;
}

#ifdef NOTDEF
// Our composite_links collection doesn't include lone Links that aren't welded
// to anything. The return from this function must include every Link, with
// the World link in the first set (even if nothing is welded to it).
std::vector<std::set<LinkIndex>> LinkJointGraph::FindSubgraphsOfWeldedLinks()
const {
}
#endif

// As mentioned above, the use of "parent" here does not imply anything about
// the Parent/Child Joint connection. It is really just the root of a subgraph.
// TODO(sherm1) Reimplement using already-built Composites.
Expand Down
5 changes: 4 additions & 1 deletion multibody/topology/link_joint_graph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -374,9 +374,12 @@ class LinkJointGraph {
element zero in the returned vector.
- The minimum number of subgraphs is one. This corresponds to a graph with
all Links welded to the world. */
std::vector<std::set<LinkIndex>> XFindSubgraphsOfWeldedLinks() const;
std::vector<std::set<LinkIndex>> FindSubgraphsOfWeldedLinks() const;

/** This much-slower method does not depend on the SpanningForest having
already been built. It is a fallback for when there is no Forest. */
std::vector<std::set<LinkIndex>> FindSubgraphsOfWeldedLinksNoModel() const;

/** Returns all Links that are transitively welded, or rigidly affixed, to
`link_index_`, per these two definitions:
1. A Link is always considered welded to itself.
Expand Down
179 changes: 129 additions & 50 deletions multibody/topology/test/link_joint_graph_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -478,57 +478,82 @@ Additionally we have the following non-weld joints:
- RevoluteJoint(3, 13): connects Link 3 to subgraph A.
- PrimaticJoint(1, 10): connects subgraph A and B.
In the picture double bars are welds, single moving joints, braces are links:
{9}
A {4} /15* {11} * = added joint
/1/ \2\ ' /10 \8
{1}==0=={13}-3-{3} -14*- {0}==12=={5}==11=={7}--9--{2}
\7 World \13\
{10} C {12}
/4/ \5\ B
{6}=6={8}
The input is given as three unconnected graphs. Joints are shown with
parent->child direction. Double bars are welds, single bars are moving joints.
Links {0-13} are shown in braces, joint numbers 0-13 are plain. Subgraphs
A, B, C formed by welds are labeled to match the description above.
C
12 11 9
{0}<==={5}===>{7}--->{2}
World ^ |10 |
13‖ v |8
{12} {11}<---+ Link/Joint graph as input
3 0 7 4
{3}--->{13}<==={1}--->{10}===>{6}
2‖ A ^ 5‖ B ‖
v ‖1 v ‖6
{4}=====+ {8}<====+
{9}
When we build the forest, we have to provide every link with a path to World.
We'll first process the upper graph which already starts at World. Then we
have to pick a base body for the next graph. Link {3} should be chosen since
it appears only as a parent; it gets floating joint 14. Link {9} will also be
a base body; it gets floating joint 15.
There are three loops in this graph: {7-2-11}, {13-1-4}, and {10-6-8}. The last
two are formed entirely of welds. When modeling in the mode where every Joint
gets a Mobod all of these must be broken by adding shadow links. Because of
the processing order, Link {11} will be split with shadow link {14*} on Joint 8,
Link {1} gets shadow {15*} on weld Joint 1, and Link {8} gets shadow {16*} on
weld Joint 6. (Link {1} gets split rather than {4} to preserve parent->child
order of Joint 1.)
Therefore we expect the following Composites, with the "World" Composite first:
{0, 5, 7, 12}, {13, 1, 4}, {6, 8, 10}, {3}, {9}, {2}, {11}.
Note that the all-weld loops 13-1-4 and 10-6-8 are harmless since the closing
weld can just be ignored. The loop 7-2-11 however requires breaking; we're
currently just ignoring the loop-closing joint 8.
{0, 5, 7, 12}, {13, 1, 4, 15*}, {10, 6, 8, 16*}
and the remaining non-composite links are {3}, {9}, {2}, {11}, {14*}.
Forest building should start with Link {5} since that is the only direct
connection to World in the input ({3} and {9} get connected later). If we're
giving every Link its own mobilizers (rather than making composites from
welded-together ones) we expect this forest of 3 trees and 14 Mobods:
level 5 10{6} 11{8}
level 4 4{11} 9{10}
level 3 3{2} 8{1} 12{4}
level 2 2{7} 5{12} 7{13}
base mobods 1{5} 6{3} 13{9}
welded-together ones) we expect this forest of 3 trees and 17 Mobods:
level 6 12{16}
level 5 11{6} 13{8}
level 4 4{14} 10{10} 15{15}
level 3 3{2} 5{11} 9{1} 14{4}
level 2 2{7} 6{12} 8{13}
base mobods 1{5} 7{3} 16{9}
\ | /
World ...........0{0}.............
Some of the Links are welded together. We call those Composite Links even
though each has its own Mobod. Those are: {0 5 7 12} {13 1 4} {10 6 8}
The corresponding Mobods are Composite Mobods: [0 1 2 5] [7 8 12] [9 10 11]
That should immediately create composite {0 5 7 12} on
mobod 0, then see outboard links {2} and {11} as new base bodies and grow
those two trees, discovering a loop at joint 8. Then it should choose link [3]
as a base link and add floating joint 14, and grow that tree. Finally it makes
free link {9} a base body. The forest should then look like this:
level 3 5{10 6 8}
level 2 4{13 1 4}
base mobods 1{2} 2{11} 3{3} 6{9} (four trees)
though each has its own Mobod. Those are:
{0 5 7 12} {13 1 4 15} {10 6 8 16}
The corresponding Mobods are Composite Mobods:
[0 1 2 6] [8 9 14 15] [10 11 13 12]
Remodeling with composite link combining turned on should immediately create
composite {0 5 7 12} on mobod 0, then see outboard links {2} and {11} as new
base bodies and grow those two trees, discovering a loop at joint 8. As before,
Link {11} gets split with a shadow link {14} for joint 8. Then it
should choose link {3} as a base link and add floating joint 14, and grow that
tree. Finally it makes free link {9} a base body. The forest should then look
like this:
level 3 6{10 6 8}
level 2 2{14} 5{13 1 4}
base mobods 1{2} 3{11} 4{3} 7{9} (four trees)
\ \ | /
World ...........0{0 5 7 12}......
In this case the Composite Links are the same, but there are no Composite
Mobods (except World alone).
*/
In this case we don't need to split the all-Weld loops since they are now
just composite links {0 5 7 12} {13 1 4} {10 6 8}. There are no Composite
Mobods (except World alone). */
GTEST_TEST(LinkJointGraph, Weldedsubgraphs) {
LinkJointGraph graph;
graph.RegisterJointType(kRevoluteType, 1, 1);
Expand Down Expand Up @@ -561,7 +586,7 @@ GTEST_TEST(LinkJointGraph, Weldedsubgraphs) {
graph.AddJoint("joint" + std::to_string(j++), model_instance, kRevoluteType,
LinkIndex(3), LinkIndex(13));

// subgraph B: formed by bodies 8, 6, 10.
// subgraph B: formed by bodies 6, 8, 10.
graph.AddJoint("joint" + std::to_string(j++), model_instance,
graph.weld_type_name(), LinkIndex(10), LinkIndex(6));
graph.AddJoint("joint" + std::to_string(j++), model_instance,
Expand Down Expand Up @@ -592,33 +617,42 @@ GTEST_TEST(LinkJointGraph, Weldedsubgraphs) {

EXPECT_EQ(ssize(graph.links()), 14); // this includes the world Link.

graph.BuildForest();
graph.DumpGraph("WeldedSubgraphs (not combined)");
const SpanningForest& forest = graph.forest();
forest.SanityCheckForest();
forest.DumpForest("WeldedSubgraphs (not combined)");

const std::vector<std::set<LinkIndex>> welded_subgraphs =
graph.FindSubgraphsOfWeldedLinks();

// Verify number of expected subgraphs.
EXPECT_EQ(welded_subgraphs.size(), 7);
EXPECT_EQ(welded_subgraphs.size(), 8);

// The first subgraph must contain the world.
const std::set<LinkIndex> world_subgraph = welded_subgraphs[0];
EXPECT_EQ(world_subgraph.count(world_index()), 1);

// Build the expected set of subgraphs.
// Build the expected set of subgraphs (see above).
std::set<std::set<LinkIndex>> expected_subgraphs;
// {0, 5, 7, 12}, {1, 4, 13}, {6, 8, 10}, {3}, {9}, {2}, {11}.
// {0, 5, 7, 12}, {1, 4, 13, 15}, {6, 8, 10, 16}, {3}, {9}, {2}, {11}, {14}
const std::set<LinkIndex>& expected_world_subgraph =
*expected_subgraphs
.insert({LinkIndex(0), LinkIndex(5), LinkIndex(7), LinkIndex(12)})
.first;
const std::set<LinkIndex>& expected_subgraphA =
*expected_subgraphs.insert({LinkIndex(1), LinkIndex(4), LinkIndex(13)})
*expected_subgraphs.insert({LinkIndex(1), LinkIndex(4), LinkIndex(13),
LinkIndex(15)})
.first;
const std::set<LinkIndex>& expected_subgraphB =
*expected_subgraphs.insert({LinkIndex(6), LinkIndex(8), LinkIndex(10)})
*expected_subgraphs.insert({LinkIndex(6), LinkIndex(8), LinkIndex(10),
LinkIndex(16)})
.first;
expected_subgraphs.insert({LinkIndex(3)});
expected_subgraphs.insert({LinkIndex(9)});
expected_subgraphs.insert({LinkIndex(2)});
expected_subgraphs.insert({LinkIndex(11)});
expected_subgraphs.insert({LinkIndex(14)});

// We do expect the first subgraph to correspond to the set of bodies welded
// to the world.
Expand All @@ -639,23 +673,68 @@ GTEST_TEST(LinkJointGraph, Weldedsubgraphs) {
EXPECT_EQ(graph.FindLinksWeldedTo(LinkIndex(10)), expected_subgraphB);
EXPECT_EQ(graph.FindLinksWeldedTo(LinkIndex(6)), expected_subgraphB);

// TODO(sherm1) Move to its own test suite.
graph.BuildForest();
graph.DumpGraph("WeldedSubgraphs (not combined)");
const SpanningForest& forest = graph.forest();
forest.SanityCheckForest();
forest.DumpForest("WeldedSubgraphs (not combined)");

// Now let's verify that we got the expected SpanningForest. To understand,
// refer to the taller (max level 6) diagram above.
EXPECT_EQ(ssize(forest.mobods()), 17);

// Note that this is a question about how these Links got modeled, not
// about the original graph.
EXPECT_EQ(graph.FindFirstCommonAncestor(LinkIndex(11), LinkIndex(12)),
LinkIndex(5));
EXPECT_EQ(graph.FindFirstCommonAncestor(LinkIndex(16), LinkIndex(15)),
LinkIndex(13));
EXPECT_EQ(graph.FindFirstCommonAncestor(LinkIndex(10), LinkIndex(2)),
LinkIndex(0));

// Expected level for each mobod in forest (index by MobodIndex).
std::array<int, 17> expected_level{0, 1, 2, 3, 4, 3, 2, 1, 2,
3, 4, 5, 6, 5, 3, 4, 1};
for (auto& mobod : forest.mobods()) {
EXPECT_EQ(mobod.level(), expected_level[mobod.index()]);
}

EXPECT_EQ(ssize(forest.composite_mobods()), 3);
EXPECT_EQ(forest.composite_mobods(CompositeMobodIndex(0)),
(std::vector<MobodIndex>{MobodIndex(0), MobodIndex(1),
MobodIndex(2), MobodIndex(6)}));
EXPECT_EQ(forest.composite_mobods(CompositeMobodIndex(1)),
(std::vector<MobodIndex>{MobodIndex(8), MobodIndex(9),
MobodIndex(14), MobodIndex(15)}));
EXPECT_EQ(forest.composite_mobods(CompositeMobodIndex(2)),
(std::vector<MobodIndex>{MobodIndex(10), MobodIndex(11),
MobodIndex(13), MobodIndex(12)}));

// Now combine composites so they get a single Mobod.
graph.BuildForest(ModelingOptions::CombineCompositeLinks);
graph.DumpGraph("WeldedSubgraphs (combined)");
forest.SanityCheckForest();
forest.DumpForest("WeldedSubgraphs (combined)");

EXPECT_EQ(ssize(graph.links()), 15); // Only one added shadow.
EXPECT_EQ(ssize(graph.composite_links()), 3);
EXPECT_EQ(graph.composite_links(CompositeLinkIndex(0)),
(std::vector<LinkIndex>{LinkIndex(0), LinkIndex(5), LinkIndex(7),
LinkIndex(12)}));
EXPECT_EQ(
graph.composite_links(CompositeLinkIndex(1)),
(std::vector<LinkIndex>{LinkIndex(13), LinkIndex(1), LinkIndex(4)}));
EXPECT_EQ(
graph.composite_links(CompositeLinkIndex(2)),
(std::vector<LinkIndex>{LinkIndex(10), LinkIndex(6), LinkIndex(8)}));

// Now let's verify that we got the expected SpanningForest. To understand,
// refer to the shorter (max level 3) diagram above.
EXPECT_EQ(ssize(forest.mobods()), 8);
std::array<int, 8> expected_level_combined{0, 1, 2, 1, 1, 2, 3, 1};
for (auto& mobod : forest.mobods()) {
EXPECT_EQ(mobod.level(), expected_level_combined[mobod.index()]);
}

EXPECT_EQ(ssize(forest.composite_mobods()), 1); // just World
EXPECT_EQ(ssize(forest.composite_mobods(CompositeMobodIndex(0))), 1);
EXPECT_EQ(forest.composite_mobods(CompositeMobodIndex(0))[0],
MobodIndex(0));
}

// Ten links, 8 in a tree and 2 free ones.
Expand Down

0 comments on commit 82846ef

Please sign in to comment.