diff --git a/multibody/topology/link_joint_graph.cc b/multibody/topology/link_joint_graph.cc index 7d80b2b53707..1a20848e862d 100644 --- a/multibody/topology/link_joint_graph.cc +++ b/multibody/topology/link_joint_graph.cc @@ -704,6 +704,169 @@ bool LinkJointGraph::link_is_static(const Link& link) const { ForestBuildingOptions::kStatic); } +/* Runs through the Mobods in the model but records the (active) Link +indexes rather than the Mobod indexes. */ +std::vector LinkJointGraph::FindPathFromWorld( + LinkIndex link_index) const { + ThrowIfForestNotBuiltYet(__func__); + const SpanningForest::Mobod* mobod = + &forest().mobods()[link_to_mobod(link_index)]; + std::vector path(mobod->level() + 1); + while (mobod->inboard().is_valid()) { + const Link& link = links(mobod->link_ordinal()); + path[mobod->level()] = link.index(); // Active Link if composite. + mobod = &forest().mobods(mobod->inboard()); + } + DRAKE_DEMAND(mobod->is_world()); + path[0] = LinkIndex(0); + return path; +} + +LinkIndex LinkJointGraph::FindFirstCommonAncestor(LinkIndex link1_index, + LinkIndex link2_index) const { + ThrowIfForestNotBuiltYet(__func__); + const MobodIndex mobod_ancestor = forest().FindFirstCommonAncestor( + link_to_mobod(link1_index), link_to_mobod(link2_index)); + const Link& ancestor_link = + links(forest().mobod_to_link_ordinal(mobod_ancestor)); + return ancestor_link.index(); +} + +std::vector LinkJointGraph::FindSubtreeLinks( + LinkIndex link_index) const { + ThrowIfForestNotBuiltYet(__func__); + const MobodIndex root_mobod_index = link_to_mobod(link_index); + return forest().FindSubtreeLinks(root_mobod_index); +} + +// Our link_composites 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> LinkJointGraph::GetSubgraphsOfWeldedLinks() + const { + ThrowIfForestNotBuiltYet(__func__); + + std::vector> subgraphs; + + // First, collect all the precomputed Link Composites. World is always + // the first one, even if nothing is welded to it. + for (const LinkComposite& composite : link_composites()) { + subgraphs.emplace_back( + std::set(composite.links.cbegin(), composite.links.cend())); + } + + // Finally, make one-Link subgraphs for Links that aren't in any composite. + for (const Link& link : links()) { + if (link.composite().has_value()) continue; + subgraphs.emplace_back(std::set{link.index()}); + } + + return subgraphs; +} + +// Strategy here is to make repeated use of CalcLinksWeldedTo(), separating +// the singleton sets from the actually-welded sets, and then move the +// singletons to the end to match what GetSubgraphsOfWeldedLinks() does. +std::vector> LinkJointGraph::CalcSubgraphsOfWeldedLinks() + const { + // Work with ordinals rather than indexes. + std::vector visited(num_user_links(), false); + + // World always comes first, even if it is alone. + std::vector> subgraphs{CalcLinksWeldedTo(LinkIndex(0))}; + for (LinkIndex index : subgraphs[0]) visited[index_to_ordinal(index)] = true; + + std::vector> singletons; + // If a Forest was already built, there may be shadow links added to + // the graph -- don't process those here. + for (LinkOrdinal link_ordinal(1); link_ordinal < num_user_links(); + ++link_ordinal) { + const Link& link = links(link_ordinal); + if (link.is_shadow() || visited[link_ordinal]) continue; + std::set welded_links = CalcLinksWeldedTo(link.index()); + for (LinkIndex index : welded_links) + visited[index_to_ordinal(index)] = true; + if (ssize(welded_links) == 1) { + singletons.emplace_back(std::move(welded_links)); + } else { + subgraphs.emplace_back(std::move(welded_links)); + } + } + + // Now move all the singletons onto the end of the subgraphs list. + for (auto& singleton : singletons) + subgraphs.emplace_back(std::move(singleton)); + + return subgraphs; +} + +// If the Link isn't part of a LinkComposite just return the Link. Otherwise, +// return all the Links in its LinkComposite. +std::set LinkJointGraph::GetLinksWeldedTo( + LinkIndex link_index) const { + ThrowIfForestNotBuiltYet(__func__); + DRAKE_DEMAND(link_index.is_valid()); + DRAKE_THROW_UNLESS(has_link(link_index)); + const Link& link = link_by_index(link_index); + const std::optional composite_index = link.composite(); + if (!composite_index.has_value()) return std::set{link_index}; + const std::vector& welded_links = + link_composites(*composite_index).links; + return std::set(welded_links.cbegin(), welded_links.cend()); +} + +// Without a Forest we don't have LinkComposites available so recursively +// chase Weld joints instead. +std::set LinkJointGraph::CalcLinksWeldedTo( + LinkIndex link_index) const { + std::set result; + AppendLinksWeldedTo(link_index, &result); + return result; +} + +void LinkJointGraph::AppendLinksWeldedTo(LinkIndex link_index, + std::set* result) const { + DRAKE_DEMAND(result != nullptr); + DRAKE_DEMAND(link_index.is_valid()); + DRAKE_THROW_UNLESS(has_link(link_index)); + DRAKE_DEMAND(!result->contains(link_index)); + + const Link& link = link_by_index(link_index); + + // A Link is always considered welded to itself. + result->insert(link_index); + + // For World we have to look for static links and pretend they are welded to + // World. (Links might have been explicitly flagged as static or part of a + // static model instance.) + if (link.is_world()) { + for (const Link& maybe_static : links()) { + if (result->contains(maybe_static.index())) continue; + if (link_is_static(maybe_static)) + AppendLinksWeldedTo(maybe_static.index(), &*result); + } + } + + // Now run through all the actual joints, looking for welds. + for (auto joint_index : link.joints()) { + const Joint& joint = joint_by_index(joint_index); + if (joint.traits_index() != weld_joint_traits_index()) continue; + const LinkIndex welded_link_index = joint.other_link_index(link_index); + // Don't reprocess if we already did the other end. + if (!result->contains(welded_link_index)) + AppendLinksWeldedTo(welded_link_index, &*result); + } +} + +void LinkJointGraph::ThrowIfForestNotBuiltYet(const char* func) const { + if (!forest_is_valid()) { + throw std::logic_error( + fmt::format("{}(): no SpanningForest available. Call BuildForest() " + "before calling this function.", + func)); + } +} + void LinkJointGraph::ThrowLinkWasRemoved(const char* func, LinkIndex link_index) const { throw std::logic_error(fmt::format( diff --git a/multibody/topology/link_joint_graph.h b/multibody/topology/link_joint_graph.h index 53ce6d916940..a4edf96e7236 100644 --- a/multibody/topology/link_joint_graph.h +++ b/multibody/topology/link_joint_graph.h @@ -417,11 +417,11 @@ class LinkJointGraph { [[nodiscard]] inline const LoopConstraint& loop_constraints( LoopConstraintIndex constraint_index) const; - /** Links with this index or higher are "ephemeral" (added during + /** Links with this ordinal or higher are "ephemeral" (added during forest-building). See the class comment for more information. */ [[nodiscard]] int num_user_links() const { return data_.num_user_links; } - /** Joints with this index or higher are "ephemeral" (added during + /** Joints with this ordinal or higher are "ephemeral" (added during forest-building). See the class comment for more information. */ [[nodiscard]] int num_user_joints() const { return data_.num_user_joints; } @@ -509,6 +509,86 @@ class LinkJointGraph { void ChangeJointType(JointIndex existing_joint_index, const std::string& name_of_new_type); + /** After the Forest is built, returns the sequence of Links from World to the + given Link L in the Forest. In case the Forest was built with a single Mobod + for each Link Composite (Links connected by Weld joints), we only report the + "active Link" for each Link Composite so that there is only one Link returned + for each level in Link L's tree. World is always the active Link for its + composite so will always be the first entry in the result. However, if L is + part of a Link Composite the final entry will be L's composite's active link, + which might not be L. Cost is O(ℓ) where ℓ is Link L's level in the + SpanningForest. + @@throws std::exception if the SpanningForest hasn't been built yet. + @see link_composites(), SpanningForest::FindPathFromWorld() */ + std::vector FindPathFromWorld(LinkIndex link_index) const; + + /** After the Forest is built, finds the first Link common to the paths + towards World from each of two Links in the SpanningForest. Returns World + immediately if the Links are on different trees of the forest. Otherwise the + cost is O(ℓ) where ℓ is the length of the longer path from one of the Links + to the ancestor. + @throws std::exception if the SpanningForest hasn't been built yet. + @see SpanningForest::FindFirstCommonAncestor() + @see SpanningForest::FindPathsToFirstCommonAncestor() */ + LinkIndex FindFirstCommonAncestor(LinkIndex link1_index, + LinkIndex link2_index) const; + + /** After the Forest is built, finds all the Links following the Forest + subtree whose root mobilized body is the one followed by the given Link. That + is, we look up the given Link's Mobod B and return all the Links that follow + B or any other Mobod in the subtree rooted at B. The Links following B + come first, and the rest follow the depth-first ordering of the Mobods. + In particular, the result is _not_ sorted by LinkIndex. Computational cost + is O(ℓ) where ℓ is the number of Links following the subtree. + @throws std::exception if the SpanningForest hasn't been built yet. + @see SpanningForest::FindSubtreeLinks() */ + std::vector FindSubtreeLinks(LinkIndex link_index) const; + + /** After the Forest is built, this method can be called to return a + partitioning of the LinkJointGraph into subgraphs such that (a) every Link is + in one and only one subgraph, and (b) two Links are in the same subgraph iff + there is a path between them which consists only of Weld joints. + Each subgraph of welded Links is represented as a set of + Link indexes (using LinkIndex). By definition, these subgraphs will be + disconnected by any non-Weld joint between two Links. A few notes: + - The maximum number of returned subgraphs equals the number of Links in + the graph. This corresponds to a graph with no Weld joints. + - The World Link is included in a set of welded Links, and this set is + element zero in the returned vector. The other subgraphs are in no + particular order. + - The minimum number of subgraphs is one. This corresponds to a graph with + all Links welded to World. + + @throws std::exception if the SpanningForest hasn't been built yet. + @see CalcSubgraphsOfWeldedLinks() if you haven't built a Forest yet */ + std::vector> GetSubgraphsOfWeldedLinks() 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. + @see GetSubgraphsOfWeldedLinks() if you already have a Forest built */ + std::vector> CalcSubgraphsOfWeldedLinks() const; + + /** After the Forest is built, 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. + 2. Two unique Links are considered welded together exclusively by the + presence of weld joints, not by other constructs such as constraints. + + Therefore, if `link_index` is a valid index to a Link in this graph, then the + return vector will always contain at least one entry storing `link_index`. + This is fast because we just need to sort the already-calculated + LinkComposite the given `link_index` is part of (if any). + + @throws std::exception if the SpanningForest hasn't been built yet or + `link_index` is out of range + @see CalcLinksWeldedTo() if you haven't built a Forest yet */ + std::set GetLinksWeldedTo(LinkIndex link_index) const; + + /** This slower method does not depend on the SpanningForest having + already been built. It is a fallback for when there is no Forest. + @see GetLinksWeldedTo() if you already have a Forest built */ + std::set CalcLinksWeldedTo(LinkIndex link_index) const; + // FYI Debugging APIs (including Graphviz-related) are defined in // link_joint_graph_debug.cc. @@ -518,6 +598,7 @@ class LinkJointGraph { The result is in the "dot" language, see https://graphviz.org. If you write it to some file foo.dot, you can generate a viewable png (for example) using the command `dot -Tpng foo.dot >foo.png`. + @see SpanningForest::GenerateGraphvizString() @see MakeGraphvizFiles() for an easier way to get pngs. */ std::string GenerateGraphvizString( std::string_view label, bool include_ephemeral_elements = true) const; @@ -700,6 +781,12 @@ class LinkJointGraph { void AddUnmodeledJointToComposite(JointOrdinal unmodeled_joint_ordinal, LinkCompositeIndex which); + // This is the implementation for CalcLinksWeldedTo(). + void AppendLinksWeldedTo(LinkIndex link_index, + std::set* result) const; + + void ThrowIfForestNotBuiltYet(const char* func) const; + [[noreturn]] void ThrowLinkWasRemoved(const char* func, LinkIndex link_index) const; diff --git a/multibody/topology/spanning_forest.cc b/multibody/topology/spanning_forest.cc index e33d89c870a6..372b926b1116 100644 --- a/multibody/topology/spanning_forest.cc +++ b/multibody/topology/spanning_forest.cc @@ -1008,6 +1008,99 @@ void SpanningForest::GrowCompositeMobod( } } +std::vector SpanningForest::FindPathFromWorld( + MobodIndex index) const { + const Mobod* mobod = &mobods(index); + std::vector path(mobod->level() + 1); + while (mobod->inboard().is_valid()) { + path[mobod->level()] = mobod->index(); + mobod = &mobods(mobod->inboard()); + } + DRAKE_DEMAND(mobod->is_world()); + path[0] = MobodIndex(0); + return path; +} + +MobodIndex SpanningForest::FindFirstCommonAncestor( + MobodIndex mobod1_index, MobodIndex mobod2_index) const { + // A body is its own first common ancestor. + if (mobod1_index == mobod2_index) return mobod1_index; + + // If either body is World, that's the first common ancestor. + if (mobod1_index == world_mobod_index() || + mobod2_index == world_mobod_index()) { + return world_mobod_index(); + } + + const Mobod* branch1 = &mobods(mobod1_index); + const Mobod* branch2 = &mobods(mobod2_index); + + // If they are in different trees, World is the ancestor. + if (branch1->tree() != branch2->tree()) return world_mobod().index(); + + // Get down to a common level, then go down both branches. + while (branch1->level() > branch2->level()) + branch1 = &mobods(branch1->inboard()); + while (branch2->level() > branch1->level()) + branch2 = &mobods(branch2->inboard()); + + // Both branches are at the same level now. + while (branch1->index() != branch2->index()) { + branch1 = &mobods(branch1->inboard()); + branch2 = &mobods(branch2->inboard()); + } + + return branch1->index(); // Same as branch2->index(). +} + +MobodIndex SpanningForest::FindPathsToFirstCommonAncestor( + MobodIndex mobod1_index, MobodIndex mobod2_index, + std::vector* path1, std::vector* path2) const { + DRAKE_DEMAND(path1 != nullptr && path2 != nullptr); + path1->clear(); + path2->clear(); + // A body is its own first common ancestor. + if (mobod1_index == mobod2_index) return mobod1_index; + + const Mobod* branch1 = &mobods(mobod1_index); + const Mobod* branch2 = &mobods(mobod2_index); + + // Get down to a common level, then go down both branches. + while (branch1->level() > branch2->level()) { + path1->push_back(branch1->index()); + branch1 = &mobods(branch1->inboard()); + } + while (branch2->level() > branch1->level()) { + path2->push_back(branch2->index()); + branch2 = &mobods(branch2->inboard()); + } + + // Both branches are at the same level now. + while (branch1->index() != branch2->index()) { + path1->push_back(branch1->index()); + path2->push_back(branch2->index()); + branch1 = &mobods(branch1->inboard()); + branch2 = &mobods(branch2->inboard()); + } + + return branch1->index(); // Same as branch2->index(). +} + +std::vector SpanningForest::FindSubtreeLinks( + MobodIndex root_mobod_index) const { + const int num_subtree_mobods = mobods(root_mobod_index).num_subtree_mobods(); + std::vector result; + result.reserve(num_subtree_mobods); // Will be at least this big. + for (int i = 0; i < num_subtree_mobods; ++i) { + const Mobod& subtree_mobod = mobods(MobodIndex(root_mobod_index + i)); + for (LinkOrdinal ordinal : subtree_mobod.follower_link_ordinals()) { + const Link& link = links(ordinal); + result.push_back(link.index()); + } + } + return result; +} + SpanningForest::Data::Data() = default; SpanningForest::Data::Data(const Data&) = default; SpanningForest::Data::Data(Data&&) = default; diff --git a/multibody/topology/spanning_forest.h b/multibody/topology/spanning_forest.h index a95140aa1171..e38518d3107f 100644 --- a/multibody/topology/spanning_forest.h +++ b/multibody/topology/spanning_forest.h @@ -152,6 +152,55 @@ class SpanningForest { using Link = LinkJointGraph::Link; using Joint = LinkJointGraph::Joint; + /** Returns the sequence of mobilized bodies from World to the given mobod B, + inclusive of both. The 0th element is always present in the result and is + the World (at level 0) with each entry being the Mobod at the next-higher + level along the path to B. Cost is O(ℓ) where ℓ is B's level in its tree. */ + std::vector FindPathFromWorld(MobodIndex index) const; + + /** Finds the highest-numbered mobilized body that is common to the paths + from each of the given ones to World. Returns World immediately if the bodies + are on different trees; otherwise the cost is O(ℓ) where ℓ is the length + of the longer path from one of the bodies to the ancestor. + @note Because the forest uses depth-first numbering for the Mobods, the + highest-numbered Mobod is also the ancestor with the highest level (i.e., + the one farthest from World). + @see FindPathsToFirstCommonAncestor() */ + MobodIndex FindFirstCommonAncestor(MobodIndex mobod1_index, + MobodIndex mobod2_index) const; + + /** Finds the highest numbered common ancestor to two mobilized bodies and + returns the paths to the ancestor from each of them. The mobilizers along + the returned paths are the only ones that can affect the _relative_ pose + between the given mobilized bodies. The returned paths do not include the + ancestor but end with the Mobod whose inboard body is the ancestor. The + ancestor Mobod is returned separately as the function return value. + Complexity is O(ℓ) where ℓ is the length of the longer path from one of the + bodies to the ancestor. Each of the given Mobods will be included in its + returned path (as the first entry) except when it is the ancestor, in which + case its path will be empty. + @note Because the forest uses depth-first numbering for the Mobods, the + highest-numbered Mobod is also the ancestor with the highest level (i.e., + the one farthest from World). + @param mobod1_index The index of Mobod 1 + @param mobod2_index The index of Mobod 2 + @param path1 path to ancestor from Mobod 1, not including the ancestor + @param path2 path to ancestor from Mobod 2, not including the ancestor + @retval ancestor_index the ancestor mobilized body's index + @see FindFirstCommonAncestor() if you don't need the paths + @pre indices are valid, path pointers are non-null */ + MobodIndex FindPathsToFirstCommonAncestor( + MobodIndex mobod1_index, MobodIndex mobod2_index, + std::vector* path1, std::vector* path2) const; + + /** Finds all the Links following the Forest subtree whose root mobilized body + B is given. That is, we return all the Links that follow B or any other Mobod + in the subtree rooted at B. The Links following B come first, and the rest + follow the depth-first ordering of the Mobods. In particular, the result is + _not_ sorted by LinkIndex. Computational cost is O(ℓ) where ℓ is the number of + Links following the subtree. */ + std::vector FindSubtreeLinks(MobodIndex root_mobod_index) const; + /** Returns a reference to the graph that owns this forest (as set during construction). */ const LinkJointGraph& graph() const { @@ -348,6 +397,12 @@ class SpanningForest { // FYI Debugging APIs (including Graphviz-related) are defined in // spanning_forest_debug.cc. + /** Generate a graphviz representation of this %SpanningForest, with the + given label at the top. The result is in the "dot" language, see + https://graphviz.org. If you write it to some file foo.dot, you can + generate a viewable png (for example) using the command + `dot -Tpng foo.dot >foo.png`. + @see LinkJointGraph::GenerateGraphvizString() */ std::string GenerateGraphvizString(std::string_view label) const; private: diff --git a/multibody/topology/test/link_joint_graph_test.cc b/multibody/topology/test/link_joint_graph_test.cc index 49818b7633a5..b37793c6276c 100644 --- a/multibody/topology/test/link_joint_graph_test.cc +++ b/multibody/topology/test/link_joint_graph_test.cc @@ -274,6 +274,24 @@ GTEST_TEST(LinkJointGraph, WorldOnlyTest) { // With no forest built, there are no composites. EXPECT_TRUE(graph.link_composites().empty()); + // These "Calc" functions don't require a forest. + EXPECT_EQ(graph.CalcSubgraphsOfWeldedLinks(), + std::vector>{{world_link_index}}); + EXPECT_EQ(graph.CalcLinksWeldedTo(world_link_index), + std::set{world_link_index}); + + // The "Find" and "Get" methods should complain if there's no Forest. + DRAKE_EXPECT_THROWS_MESSAGE(graph.FindPathFromWorld(world_link_index), + "FindPathFromWorld.*no SpanningForest.*"); + DRAKE_EXPECT_THROWS_MESSAGE( + graph.FindFirstCommonAncestor(world_link_index, world_link_index), + "FindFirstCommonAncestor.*no SpanningForest.*"); + DRAKE_EXPECT_THROWS_MESSAGE(graph.FindSubtreeLinks(world_link_index), + "FindSubtreeLinks.*no SpanningForest.*"); + DRAKE_EXPECT_THROWS_MESSAGE(graph.GetSubgraphsOfWeldedLinks(), + "GetSubgraphsOfWeldedLinks.*no SpanningForest.*"); + DRAKE_EXPECT_THROWS_MESSAGE(graph.GetLinksWeldedTo(world_link_index), + "GetLinksWeldedTo.*no SpanningForest.*"); EXPECT_TRUE(graph.BuildForest()); const SpanningForest& forest = graph.forest(); @@ -287,6 +305,18 @@ GTEST_TEST(LinkJointGraph, WorldOnlyTest) { EXPECT_EQ(ssize(graph.link_composites(LinkCompositeIndex(0)).links), 1); EXPECT_EQ(graph.link_composites(LinkCompositeIndex(0)).links[0], world_link_index); + EXPECT_FALSE(graph.link_composites(LinkCompositeIndex(0)).is_massless); + + EXPECT_EQ(graph.FindPathFromWorld(world_link_index), + std::vector{world_link_index}); // Just World. + EXPECT_EQ(graph.FindFirstCommonAncestor(world_link_index, world_link_index), + world_link_index); + EXPECT_EQ(graph.FindSubtreeLinks(world_link_index), + std::vector{world_link_index}); + EXPECT_EQ(graph.GetSubgraphsOfWeldedLinks(), + std::vector>{{world_link_index}}); + EXPECT_EQ(graph.GetLinksWeldedTo(world_link_index), + std::set{world_link_index}); // Check that Clear() puts the graph back to default-constructed condition. // First add some junk to the graph. @@ -315,6 +345,90 @@ GTEST_TEST(LinkJointGraph, WorldOnlyTest) { EXPECT_TRUE(graph.forest_is_valid()); } +/* Check that various algorithms work on a non-trivial graph (they were +tested on the world-only graph above). Most of these +are actually wrappers around SpanningForest algorithms which are tested +more thoroughly in spanning_forest_test.cc, so this is mostly testing +that the wrapping worked correctly. + +Here is the graph: + + {9} + ^ + | + {1} -> {2} => {3} => {8} + World + {0} {4} => {5} -> {6} + + {7} (static) +*/ +GTEST_TEST(LinkJointGraph, CheckMiscAlgorithms) { + LinkJointGraph graph; + graph.RegisterJointType("revolute", 1, 1); + + for (int i = 1; i <= 9; ++i) { + graph.AddLink(fmt::format("link{}", i), default_model_instance(), + i == 7 ? LinkFlags::kStatic : LinkFlags::kDefault); + } + + graph.AddJoint("joint0", default_model_instance(), "revolute", LinkIndex(1), + LinkIndex(2)); + graph.AddJoint("joint1", default_model_instance(), "weld", LinkIndex(2), + LinkIndex(3)); + graph.AddJoint("joint2", default_model_instance(), "weld", LinkIndex(3), + LinkIndex(8)); + graph.AddJoint("joint3", default_model_instance(), "revolute", LinkIndex(2), + LinkIndex(9)); + graph.AddJoint("joint4", default_model_instance(), "weld", LinkIndex(4), + LinkIndex(5)); + graph.AddJoint("joint5", default_model_instance(), "revolute", LinkIndex(5), + LinkIndex(6)); + + const std::vector> expected_subgraphs{ + {LinkIndex(0), LinkIndex(7)}, + {LinkIndex(2), LinkIndex(3), LinkIndex(8)}, + {LinkIndex(4), LinkIndex(5)}, + {LinkIndex(1)}, + {LinkIndex(6)}, + {LinkIndex(9)}}; + + // The two "Calc" functions should work even though we haven't built the + // forest. + EXPECT_EQ(graph.CalcSubgraphsOfWeldedLinks(), expected_subgraphs); + + EXPECT_EQ(graph.CalcLinksWeldedTo(LinkIndex(3)), + (std::set{{LinkIndex(2), LinkIndex(3), LinkIndex(8)}})); + + // Building the forest gives us access to the faster functions. + graph.BuildForest(); + + EXPECT_EQ(graph.GetSubgraphsOfWeldedLinks(), expected_subgraphs); + EXPECT_EQ(graph.GetLinksWeldedTo(LinkIndex(3)), + (std::set{{LinkIndex(2), LinkIndex(3), LinkIndex(8)}})); + + EXPECT_EQ( + graph.FindPathFromWorld(LinkIndex(3)), + (std::vector{LinkIndex(0), LinkIndex(1), LinkIndex(2), LinkIndex(3)})); + + EXPECT_EQ(graph.FindFirstCommonAncestor(LinkIndex(8), LinkIndex(9)), + LinkIndex(2)); + EXPECT_EQ(graph.FindFirstCommonAncestor(LinkIndex(9), LinkIndex(8)), + LinkIndex(2)); + + EXPECT_EQ( + graph.FindSubtreeLinks(LinkIndex(2)), + (std::vector{LinkIndex(2), LinkIndex(3), LinkIndex(8), LinkIndex(9)})); + + // Welds for static links get added first, then floating joints. + EXPECT_EQ(graph.FindSubtreeLinks(LinkIndex(0)), + (std::vector{ + LinkIndex(0), LinkIndex(7), // static (tree0) + LinkIndex(1), LinkIndex(2), LinkIndex(3), LinkIndex(8), + LinkIndex(9), // tree1 + LinkIndex(4), LinkIndex(5), LinkIndex(6) // tree2 + })); +} + // Make sure AddLink() rejects obvious errors. GTEST_TEST(LinkJointGraph, AddLinkErrors) { LinkJointGraph graph; diff --git a/multibody/topology/test/spanning_forest_test.cc b/multibody/topology/test/spanning_forest_test.cc index 9ee4083d03f8..db18ec902609 100644 --- a/multibody/topology/test/spanning_forest_test.cc +++ b/multibody/topology/test/spanning_forest_test.cc @@ -64,9 +64,9 @@ GTEST_TEST(SpanningForest, WorldOnlyTest) { EXPECT_EQ(world_mobod_index, MobodIndex(0)); EXPECT_EQ(graph.link_to_mobod(world_link_index), MobodIndex(0)); EXPECT_EQ(ssize(graph.link_composites()), 1); - EXPECT_EQ(ssize(graph.link_composites(LinkCompositeIndex(0)).links), 1); - EXPECT_EQ(graph.link_composites(LinkCompositeIndex(0)).links[0], - world_link_index); + EXPECT_EQ(graph.link_composites(LinkCompositeIndex(0)).links, + std::vector{world_link_index}); + EXPECT_FALSE(graph.link_composites(LinkCompositeIndex(0)).is_massless); // Check that the World-only forest makes sense. EXPECT_EQ(ssize(forest.mobods()), 1); @@ -78,12 +78,18 @@ GTEST_TEST(SpanningForest, WorldOnlyTest) { EXPECT_EQ(forest.welded_mobods()[0][0], world_mobod_index); EXPECT_EQ(forest.mobod_to_link_ordinal(world_mobod_index), world_link_ordinal); - EXPECT_EQ(ssize(forest.mobod_to_link_ordinals(world_mobod_index)), 1); - EXPECT_EQ(forest.mobod_to_link_ordinals(world_mobod_index)[0], - world_link_ordinal); + EXPECT_EQ(forest.mobod_to_link_ordinals(world_mobod_index), + std::vector{world_link_ordinal}); EXPECT_EQ(forest.num_positions(), 0); EXPECT_EQ(forest.num_velocities(), 0); EXPECT_TRUE(forest.quaternion_starts().empty()); + EXPECT_EQ(forest.FindPathFromWorld(world_mobod_index), + std::vector{world_mobod_index}); // Just World. + EXPECT_EQ(forest.FindSubtreeLinks(world_mobod_index), + std::vector{world_link_index}); + EXPECT_EQ( + forest.FindFirstCommonAncestor(world_mobod_index, world_mobod_index), + world_mobod_index); // Exercise the Mobod API to check the World Mobod for reasonableness. EXPECT_TRUE(world.is_world()); @@ -95,8 +101,7 @@ GTEST_TEST(SpanningForest, WorldOnlyTest) { EXPECT_FALSE(world.inboard().is_valid()); EXPECT_TRUE(world.outboards().empty()); EXPECT_EQ(world.link_ordinal(), world_link_ordinal); - EXPECT_EQ(ssize(world.follower_link_ordinals()), 1); - EXPECT_EQ(world.follower_link_ordinals()[0], world_link_ordinal); + EXPECT_EQ(world.follower_link_ordinals(), std::vector{world_link_ordinal}); EXPECT_FALSE(world.joint_ordinal().is_valid()); EXPECT_FALSE(world.tree().is_valid()); EXPECT_EQ(world.welded_mobods_group(), WeldedMobodsIndex(0)); @@ -331,10 +336,8 @@ GTEST_TEST(SpanningForest, MultipleBranchesDefaultOptions) { EXPECT_EQ(forest.mobod_to_link_ordinal(MobodIndex(mobod_link.first)), LinkOrdinal(mobod_link.second)); // Each Mobod has only a single Link that follows it. - EXPECT_EQ( - ssize(forest.mobod_to_link_ordinals(MobodIndex(mobod_link.first))), 1); - EXPECT_EQ(forest.mobod_to_link_ordinals(MobodIndex(mobod_link.first))[0], - LinkOrdinal(mobod_link.second)); + EXPECT_EQ(forest.mobod_to_link_ordinals(MobodIndex(mobod_link.first)), + std::vector{LinkOrdinal(mobod_link.second)}); } EXPECT_EQ(forest.height(), 7); @@ -410,6 +413,27 @@ GTEST_TEST(SpanningForest, MultipleBranchesDefaultOptions) { EXPECT_EQ(find_outv(8), pair(18, 6)); // tree1 nonterminal EXPECT_EQ(find_outv(10), pair(20, 3)); EXPECT_EQ(find_outv(14), pair(24, 0)); // tree1 terminal + + const std::vector expected_path_from_14{ + {MobodIndex(0)}, {MobodIndex{7}}, {MobodIndex(8)}, {MobodIndex(14)}}; + EXPECT_EQ(forest.FindPathFromWorld(MobodIndex(14)), expected_path_from_14); + + // Mobods on different trees have only World as a common ancestor. + EXPECT_EQ(forest.FindFirstCommonAncestor(MobodIndex(5), MobodIndex(14)), + MobodIndex(0)); + + // Check that long/short and short/long branch ordering both work since + // they are handled by different code. + EXPECT_EQ(forest.FindFirstCommonAncestor(MobodIndex(13), MobodIndex(14)), + MobodIndex(8)); // See right hand drawing above. + EXPECT_EQ(forest.FindFirstCommonAncestor(MobodIndex(14), MobodIndex(13)), + MobodIndex(8)); + + // Check special case: if either body is World the answer is World. + EXPECT_EQ(forest.FindFirstCommonAncestor(MobodIndex(0), MobodIndex(13)), + MobodIndex(0)); + EXPECT_EQ(forest.FindFirstCommonAncestor(MobodIndex(14), MobodIndex(0)), + MobodIndex(0)); } /* Starting with the same graph as the previous test, change the tree1 @@ -466,19 +490,16 @@ GTEST_TEST(SpanningForest, MultipleBranchesBaseJointOptions) { // There is only the World composite, but now tree1's base link is included. EXPECT_EQ(ssize(graph.link_composites()), 1); // just World - EXPECT_EQ(ssize(graph.link_composites(LinkCompositeIndex(0)).links), 2); - EXPECT_EQ(graph.link_composites(LinkCompositeIndex(0)).links[0], - graph.world_link().index()); - EXPECT_EQ(graph.link_composites(LinkCompositeIndex(0)).links[1], - graph.links(tree1.front().link_ordinal()).index()); + EXPECT_EQ(graph.link_composites(LinkCompositeIndex(0)).links, + (std::vector{graph.world_link().index(), + graph.links(tree1.front().link_ordinal()).index()})); + EXPECT_FALSE(graph.link_composites(LinkCompositeIndex(0)).is_massless); // Similarly, there is only one WeldedMobods group, containing just World - // and tree1's base - EXPECT_EQ(ssize(forest.welded_mobods()), 1); - EXPECT_EQ(ssize(forest.welded_mobods(WeldedMobodsIndex(0))), 2); - EXPECT_EQ(forest.welded_mobods(WeldedMobodsIndex(0))[0], - forest.world_mobod().index()); - EXPECT_EQ(forest.welded_mobods(WeldedMobodsIndex(0))[1], tree1.base_mobod()); + // and tree1's base. + EXPECT_EQ(forest.welded_mobods(), + (std::vector>{ + {forest.world_mobod().index(), tree1.base_mobod()}})); } /* Verify that our base body choice policy works as intended. The policy @@ -718,6 +739,12 @@ GTEST_TEST(SpanningForest, SerialChainAndMore) { EXPECT_EQ(forest.world_mobod().nv_outboard(), forest.num_velocities()); EXPECT_EQ(forest.world_mobod().num_subtree_mobods(), ssize(forest.mobods())); + EXPECT_EQ( + graph.FindSubtreeLinks(graph.world_link().index()), + (std::vector{LinkIndex(0), LinkIndex(1), LinkIndex(2), LinkIndex(3), + LinkIndex(4), LinkIndex(5), LinkIndex(7), LinkIndex(6), + LinkIndex(8), LinkIndex(11), LinkIndex(10), LinkIndex(9)})); + // Counts for generic middle Mobod. const SpanningForest::Mobod& mobod_for_link3 = forest.mobods(graph.link_by_index(LinkIndex(3)).mobod_index()); @@ -731,6 +758,8 @@ GTEST_TEST(SpanningForest, SerialChainAndMore) { EXPECT_EQ(mobod_for_link3.nq_outboard(), 2); EXPECT_EQ(mobod_for_link3.nv_outboard(), 2); EXPECT_EQ(mobod_for_link3.num_subtree_mobods(), 3); + EXPECT_EQ(graph.FindSubtreeLinks(LinkIndex(3)), + (std::vector{LinkIndex(3), LinkIndex(4), LinkIndex(5)})); // Counts for a Mobod with nq != nv. const SpanningForest::Mobod& mobod_for_base11 = @@ -744,6 +773,8 @@ GTEST_TEST(SpanningForest, SerialChainAndMore) { EXPECT_EQ(mobod_for_base11.nq_outboard(), 0); EXPECT_EQ(mobod_for_base11.nv_outboard(), 0); EXPECT_EQ(mobod_for_base11.num_subtree_mobods(), 2); + EXPECT_EQ(graph.FindSubtreeLinks(LinkIndex(11)), + (std::vector{LinkIndex(11), LinkIndex(10)})); const std::vector welded_mobods0{MobodIndex(0), MobodIndex(6), MobodIndex(7), MobodIndex(8)}; @@ -798,6 +829,16 @@ GTEST_TEST(SpanningForest, SerialChainAndMore) { EXPECT_EQ(ssize(forest.trees()), 3); EXPECT_EQ(ssize(forest.welded_mobods()), 1); // Just World. + // Starting with a Link somewhere in a composite, we should get all the + // Links on that composite followed by anything outboard. + EXPECT_EQ(graph.FindSubtreeLinks(LinkIndex(10)), + (std::vector{LinkIndex(11), LinkIndex(10)})); + EXPECT_EQ( + graph.FindSubtreeLinks(LinkIndex(6)), + (std::vector{LinkIndex(0), LinkIndex(7), LinkIndex(6), LinkIndex(8), + LinkIndex(1), LinkIndex(2), LinkIndex(3), LinkIndex(4), + LinkIndex(5), LinkIndex(11), LinkIndex(10), LinkIndex(9)})); + /* SerialChain 3 (merge composites except for 10 & 11) ------------------------------------------------------ We can optionally insist that a weld joint within a composite that would @@ -996,6 +1037,23 @@ GTEST_TEST(SpanningForest, WeldedSubgraphs) { EXPECT_EQ(ssize(graph.links()), 14); // Includes World. EXPECT_EQ(ssize(graph.joints()), 14); + // We can calculate the subgraphs without first building a Forest. This + // function should only consider user Links, not shadow links even if they + // have already been created. + // {0 5 7 12} {1 4 13} {6 8 10} {2} {3} {9} {11} (see first drawing above) + const std::vector> expected_before_subgraphs{ + {LinkIndex(0), LinkIndex(5), LinkIndex(7), LinkIndex(12)}, + {LinkIndex(1), LinkIndex(4), LinkIndex(13)}, + {LinkIndex(6), LinkIndex(8), LinkIndex(10)}, + {LinkIndex(2)}, + {LinkIndex(3)}, + {LinkIndex(9)}, + {LinkIndex(11)}}; + const std::vector> before_subgraphs = + graph.CalcSubgraphsOfWeldedLinks(); + EXPECT_EQ(before_subgraphs.size(), 7); + EXPECT_EQ(before_subgraphs, expected_before_subgraphs); + EXPECT_TRUE(graph.BuildForest()); const SpanningForest& forest = graph.forest(); @@ -1026,11 +1084,108 @@ GTEST_TEST(SpanningForest, WeldedSubgraphs) { EXPECT_EQ(graph.link_composites(c).links[link], expected_links[c][link]); } + // After building the Forest and adding shadow bodies, the non-Forest-using + // subgraph method should not change its result. The Forest-using fast one + // will include Shadow Links as well as the user's. + EXPECT_EQ(graph.CalcSubgraphsOfWeldedLinks(), + expected_before_subgraphs); // no change + + const std::vector> welded_subgraphs = + graph.GetSubgraphsOfWeldedLinks(); + + // Verify number of expected subgraphs. + EXPECT_EQ(welded_subgraphs.size(), 8); + + // The first subgraph must contain the world. + const std::set world_subgraph = welded_subgraphs[0]; + EXPECT_EQ(world_subgraph.count(world_index()), 1); + + // Build the expected set of subgraphs (see above). + std::set> expected_subgraphs; + // {0, 5, 7, 12}, {1, 4, 13, 15}, {6, 8, 10, 16}, {3}, {9}, {2}, {11}, {14} + const std::set& expected_world_subgraph = + *expected_subgraphs + .insert({LinkIndex(0), LinkIndex(5), LinkIndex(7), LinkIndex(12)}) + .first; + const std::set& expected_subgraphA = + *expected_subgraphs + .insert({LinkIndex(1), LinkIndex(4), LinkIndex(13), LinkIndex(15)}) + .first; + const std::set& expected_subgraphB = + *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. + EXPECT_EQ(world_subgraph, expected_world_subgraph); + + // In order to compare the computed list of welded bodies against the expected + // list, irrespective of the ordering in the computed list, we first convert + // the computed subgraphs to a set. + const std::set> welded_subgraphs_set( + welded_subgraphs.begin(), welded_subgraphs.end()); + EXPECT_EQ(welded_subgraphs_set, expected_subgraphs); + + // Verify we can query the list of bodies welded to a particular Link. + EXPECT_EQ(graph.GetLinksWeldedTo(LinkIndex(9)).size(), 1); + EXPECT_EQ(graph.GetLinksWeldedTo(LinkIndex(11)).size(), 1); + EXPECT_EQ(graph.GetLinksWeldedTo(LinkIndex(4)), expected_subgraphA); + EXPECT_EQ(graph.GetLinksWeldedTo(LinkIndex(13)), expected_subgraphA); + EXPECT_EQ(graph.GetLinksWeldedTo(LinkIndex(10)), expected_subgraphB); + EXPECT_EQ(graph.GetLinksWeldedTo(LinkIndex(6)), expected_subgraphB); + // Now let's verify that we got the expected SpanningForest. To understand, // refer to the 6-level forest diagram above. EXPECT_EQ(ssize(forest.mobods()), 17); EXPECT_EQ(ssize(forest.loop_constraints()), 3); + // 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)); + + // Repeat but this time collect the paths to the ancestor. + std::vector path1, path2; + + // Check with path1 longer. + EXPECT_EQ( + forest.FindPathsToFirstCommonAncestor( + graph.link_by_index(LinkIndex(11)).mobod_index(), + graph.link_by_index(LinkIndex(12)).mobod_index(), &path1, &path2), + graph.link_by_index(LinkIndex(5)).mobod_index()); + EXPECT_EQ(path1, (std::vector{MobodIndex(5), MobodIndex(2)})); + EXPECT_EQ(path2, (std::vector{MobodIndex(6)})); + + // Check with path2 longer. + EXPECT_EQ( + forest.FindPathsToFirstCommonAncestor( + graph.link_by_index(LinkIndex(15)).mobod_index(), + graph.link_by_index(LinkIndex(16)).mobod_index(), &path1, &path2), + graph.link_by_index(LinkIndex(13)).mobod_index()); + EXPECT_EQ(path1, (std::vector{MobodIndex(15), MobodIndex(14)})); + EXPECT_EQ(path2, (std::vector{MobodIndex(12), MobodIndex(11), + MobodIndex(10), MobodIndex(9)})); + + EXPECT_EQ( + forest.FindPathsToFirstCommonAncestor( + graph.link_by_index(LinkIndex(10)).mobod_index(), + graph.link_by_index(LinkIndex(2)).mobod_index(), &path1, &path2), + graph.link_by_index(LinkIndex(0)).mobod_index()); + EXPECT_EQ(path1, (std::vector{MobodIndex(10), MobodIndex(9), + MobodIndex(8), MobodIndex(7)})); + EXPECT_EQ(path2, (std::vector{MobodIndex(3), MobodIndex(2), + MobodIndex(1)})); + // Expected level for each mobod in forest (index by MobodIndex). std::array expected_level{0, 1, 2, 3, 4, 3, 2, 1, 2, 3, 4, 5, 6, 5, 3, 4, 1}; @@ -1999,8 +2154,7 @@ GTEST_TEST(SpanningForest, MasslessMergedComposites) { EXPECT_EQ(shadow_link.index(), LinkIndex(9)); EXPECT_EQ(shadow_link.primary_link(), LinkIndex(3)); EXPECT_EQ(shadow_link.mobod_index(), MobodIndex(6)); - ASSERT_EQ(ssize(shadow_link.joints()), 1); - EXPECT_EQ(shadow_link.joints()[0], JointIndex(5)); + EXPECT_EQ(shadow_link.joints(), std::vector{JointIndex(5)}); ASSERT_EQ(ssize(graph.link_composites()), 1); // just the world composite EXPECT_EQ(