diff --git a/multibody/topology/link_joint_graph.cc b/multibody/topology/link_joint_graph.cc index 373fed7b183a..b2b6b0953c43 100644 --- a/multibody/topology/link_joint_graph.cc +++ b/multibody/topology/link_joint_graph.cc @@ -613,14 +613,14 @@ LinkCompositeIndex LinkJointGraph::AddToLinkComposite( LinkCompositeIndex(ssize(data_.link_composites)); data_.link_composites.emplace_back(LinkComposite{ .links = std::vector{maybe_composite_link.index()}, - .is_massless = maybe_composite_link.treat_as_massless()}); + .is_massless = maybe_composite_link.is_massless()}); } LinkComposite& existing_composite = data_.link_composites[*existing_composite_index]; existing_composite.links.push_back(new_link.index()); - // If any Link in the composite has mass, then the whole thing is massful. - if (!new_link.treat_as_massless()) existing_composite.is_massless = false; + // For the composite to be massless, _all_ its links must be massless. + if (!new_link.is_massless()) existing_composite.is_massless = false; new_link.link_composite_index_ = existing_composite_index; return *existing_composite_index; diff --git a/multibody/topology/link_joint_graph.h b/multibody/topology/link_joint_graph.h index fa9e1d0bcfcd..8e26cd220681 100644 --- a/multibody/topology/link_joint_graph.h +++ b/multibody/topology/link_joint_graph.h @@ -472,8 +472,8 @@ class LinkJointGraph { BodyIndex link1_index, BodyIndex link2_index) const; /** Returns true if the given Link should be treated as massless. That - requires that the Link was marked TreatAsMassless and is not connected by - a Weld Joint to a massful Link or Composite. */ + requires that the Link was flagged kMassless and is not connected by + a Weld Joint to a massful Composite. */ [[nodiscard]] bool must_treat_as_massless(LinkOrdinal link_ordinal) const; /** (Internal use only) For testing -- invalidates the Forest. */ diff --git a/multibody/topology/link_joint_graph_defs.h b/multibody/topology/link_joint_graph_defs.h index c39c43342e3d..c2c40ab91a0c 100644 --- a/multibody/topology/link_joint_graph_defs.h +++ b/multibody/topology/link_joint_graph_defs.h @@ -26,10 +26,10 @@ using LoopConstraintIndex = TypeSafeIndex; these also produces a LinkFlags object. */ enum class LinkFlags : uint32_t { kDefault = 0, - kStatic = 1 << 0, ///< Implicitly welded to World. - kMustBeBaseBody = 1 << 1, ///< Ensure connection to World if none. - kTreatAsMassless = 1 << 2, ///< Can't be a terminal body in a tree. - kShadow = 1 << 3 ///< Link is a shadow (internal use only). + kStatic = 1 << 0, ///< Implicitly welded to World. + kMustBeBaseBody = 1 << 1, ///< Ensure connection to World if none. + kMassless = 1 << 2, ///< Can't be a terminal body in a tree. + kShadow = 1 << 3 ///< Link is a shadow (internal use only). }; /** Joint properties that can affect how the SpanningForest gets built. Or-ing diff --git a/multibody/topology/link_joint_graph_inlines.h b/multibody/topology/link_joint_graph_inlines.h index 78ce4d1a13a6..64156298cee9 100644 --- a/multibody/topology/link_joint_graph_inlines.h +++ b/multibody/topology/link_joint_graph_inlines.h @@ -52,7 +52,7 @@ inline bool LinkJointGraph::must_treat_as_massless( const Link& link = links(link_ordinal); // TODO(sherm1) If part of a Composite then this is only massless if the // entire Composite is composed of massless Links. - return link.treat_as_massless(); + return link.is_massless(); } // LinkJointGraph definitions deferred until Joint defined. diff --git a/multibody/topology/link_joint_graph_link.h b/multibody/topology/link_joint_graph_link.h index b37326f5bccb..a97b3f99c367 100644 --- a/multibody/topology/link_joint_graph_link.h +++ b/multibody/topology/link_joint_graph_link.h @@ -82,10 +82,12 @@ class LinkJointGraph::Link { return static_cast(flags_ & LinkFlags::kMustBeBaseBody); } - /** Returns `true` if this %Link was added with - LinkFlags::kTreatAsMassless. */ - bool treat_as_massless() const { - return static_cast(flags_ & LinkFlags::kTreatAsMassless); + /** Returns `true` if this %Link was added with LinkFlags::kMassless. + However, this %Link may still be _effectively_ massful if it is welded + into a massful composite. See LinkJointGraph::must_treat_as_massless() + for the full story. */ + bool is_massless() const { + return static_cast(flags_ & LinkFlags::kMassless); } /** Returns `true` if this is a shadow Link added by BuildForest(). */ diff --git a/multibody/topology/spanning_forest.cc b/multibody/topology/spanning_forest.cc index d448b8cf00a5..04cc79c94e67 100644 --- a/multibody/topology/spanning_forest.cc +++ b/multibody/topology/spanning_forest.cc @@ -407,7 +407,7 @@ void SpanningForest::ChooseBaseBodiesAndAddTrees(int* num_unprocessed_links) { /* Should be nothing left now except unjointed (single) Links. We'll attach with either a floating joint or a weld joint depending on modeling options. - If a weld and we're optimizing composites the Link will just join the World + If a weld and we're optimizing composites, the Link will just join the World Mobod. Otherwise it gets a new Mobod that serves as the base body of a new Tree. Although static links and links belonging to a static model instance get welded to World at the start of forest building, it is still possible @@ -425,7 +425,7 @@ void SpanningForest::ChooseBaseBodiesAndAddTrees(int* num_unprocessed_links) { unjointed_link); const JointOrdinal next_joint_ordinal = graph().index_to_ordinal(next_joint_index); - if (should_be_unmodeled_weld_in_composite(joints(next_joint_ordinal))) { + if (should_merge_parent_and_child(joints(next_joint_ordinal))) { JoinExistingMobod(&data_.mobods[0], unjointed_link, next_joint_ordinal); } else { AddNewMobod(unjointed_link, next_joint_ordinal, world_mobod().index(), @@ -452,19 +452,19 @@ void SpanningForest::ExtendTrees(const std::vector& joints_to_model, 1 If we're optimizing composites (by merging welded-together Links onto a single Mobod), keep growing a LinkComposite as far as possible to build the complete composite and model it with a single Mobod. - 2 If we encounter a "treat as massless" Link (or massless merged - LinkComposite) its Mobod can't be a terminal body of a branch. In that - case we keep growing that branch until we can end with something massful. - Note that a merged LinkComposite can be terminal if _any_ one of its Links - is massful. + 2 If we encounter a massless Link (or massless merged LinkComposite), we + don't want its Mobod to be a terminal body of a branch. In that case we + keep growing that branch until we can end with something massful. If we + fail, we have to mark this forest as unsuited for dynamics since its mass + matrix will be singular. Exception 1 preserves the goal of minimizing maximum branch length since we still only advance by one Mobod, regardless of how many merged Links that entails. Exception 2 sacrifices the even growth of branches in order to achieve the more -critical goal of ending every branch with a massful body so that the system -mass matrix is non-singular. +critical goal of ending every branch with a massful body to avoid making the +system mass matrix singular. Definitions used below ---------------------- @@ -477,7 +477,7 @@ Definitions used below - A "merged joint" is a weld joint that connects two links that should be part of the same merged LinkComposite. Merged joints do not have a corresponding mobilizer in the forest; they are unmodeled. -- Define Jₒₚₑₙ(L+) as the set of all as-yet-unmodeled joints connected to any +- Define Jₒₚₑₙ(L+) as the set of all not-yet-processed joints connected to any link in L+. These are "open" in the sense that only inboard link I of the joint is in L+; the other link O still needs to be dealt with. Open joints define the next level to be added to the forest outboard of L+. @@ -551,7 +551,7 @@ void SpanningForest::ExtendTreesOneLevel(const std::vector& J_in, const JointOrdinal j_level_ordinal = graph().index_to_ordinal(j_level_index); const Joint& j_level = joints(j_level_ordinal); - DRAKE_DEMAND(!should_be_unmodeled_weld_in_composite(j_level)); + DRAKE_DEMAND(!should_merge_parent_and_child(j_level)); /* j_level might connect to any link of an inboard LinkComposite that follows inboard_mobod. That's the link we want as inboard link; not @@ -678,7 +678,7 @@ void SpanningForest::FindNextLevelJoints(MobodIndex inboard_mobod_index, const Joint& j_in = joint_by_index(j_in_index); if (j_in.has_been_processed()) continue; - if (!should_be_unmodeled_weld_in_composite(j_in)) { + if (!should_merge_parent_and_child(j_in)) { J_level->push_back(j_in.index()); continue; } @@ -718,7 +718,7 @@ bool SpanningForest::HasMassfulOutboardLink( const Joint& joint = joint_by_index(joint_index); const BodyIndex outboard_link_index = FindOutboardLink(inboard_mobod_index, joint); - if (!link_by_index(outboard_link_index).treat_as_massless()) return true; + if (!link_by_index(outboard_link_index).is_massless()) return true; } return false; } @@ -738,8 +738,7 @@ const SpanningForest::Mobod& SpanningForest::AddNewMobod( data_.mobods.emplace_back(new_mobod_index, outboard_link_ordinal, joint_ordinal, inboard_level + 1, is_reversed); const Link& outboard_link = links(outboard_link_ordinal); - if (!outboard_link.treat_as_massless()) - new_mobod.has_massful_follower_link_ = true; + if (!outboard_link.is_massless()) new_mobod.has_massful_follower_link_ = true; /* If the inboard Mobod is World, start a new Tree. */ Mobod& inboard_mobod = data_.mobods[inboard_mobod_index]; @@ -945,7 +944,7 @@ const SpanningForest::Mobod& SpanningForest::JoinExistingMobod( follower_link_ordinal, inboard_mobod->index(), weld_joint.index()); inboard_mobod->follower_link_ordinals_.push_back(follower_link_ordinal); const Link& follower_link = links(follower_link_ordinal); - if (!follower_link.treat_as_massless()) + if (!follower_link.is_massless()) inboard_mobod->has_massful_follower_link_ = true; /* We're not going to model this weld Joint since it is interior to @@ -965,9 +964,9 @@ void SpanningForest::GrowCompositeMobod( to the composite and move on. */ const LinkOrdinal follower_link_ordinal = graph().index_to_ordinal(follower_link_index); + const Link& follower_link = links(follower_link_ordinal); if (link_is_already_in_forest(follower_link_ordinal)) { const Link& inboard_link = links(mobod->link_ordinal()); - const Link& follower_link = links(follower_link_ordinal); DRAKE_DEMAND(follower_link.composite() == inboard_link.composite()); mutable_graph().AddUnmodeledJointToComposite(weld_joint_ordinal, *follower_link.composite()); @@ -977,11 +976,10 @@ void SpanningForest::GrowCompositeMobod( JoinExistingMobod(&*mobod, follower_link_ordinal, weld_joint_ordinal); --(*num_unprocessed_links); - const Link& follower_link = links(follower_link_ordinal); for (JointIndex joint_index : follower_link.joints()) { const JointOrdinal joint_ordinal = graph().index_to_ordinal(joint_index); const Joint& joint = joints(joint_ordinal); - if (!should_be_unmodeled_weld_in_composite(joint)) { + if (!should_merge_parent_and_child(joint)) { outboard_joint_indexes->push_back(joint_index); continue; } diff --git a/multibody/topology/spanning_forest.h b/multibody/topology/spanning_forest.h index c42df70f2f72..17a4abf6d0d9 100644 --- a/multibody/topology/spanning_forest.h +++ b/multibody/topology/spanning_forest.h @@ -419,7 +419,7 @@ class SpanningForest { // Grows the trees containing each of the given Joints by one level. The // output parameter `J_out` (cleared on entry) on return // contains the set of Joints that should be modeled at the next level. - // @pre the pointers are non-null and J_in is not empty. On return, + // @pre the pointers are non-null and `J_in` is not empty. On return, // num_unprocessed_links will have been decremented by the number of Links // that were modeled. void ExtendTreesOneLevel(const std::vector& J_in, @@ -551,16 +551,13 @@ class SpanningForest { // connecting parent and child Links and need to decide whether the parent // and child will follow a single Mobod or two different Mobods. If we // decide to merge them, the Joint won't be modeled at all since it will be - // interior to the composite. Here is the policy: - // - If the joint is not a weld, we're not building a composite; - // return false (don't merge). - // - If the weld joint has been marked "must be modeled" then both its - // parent and child Links must have their own Mobods; return false. - // - Look at the joint's model instance's forest building options. If - // they include "merge link composites" then we will merge parent and - // child; return true. - // - Otherwise we're not combining; return false; - bool should_be_unmodeled_weld_in_composite(const Joint& joint) { + // interior to the composite. + // + // To return true (merge), the following must all be true: + // - The joint must be a weld, and + // - the joint's model instance must request merging composites, and + // - the joint has _not_ demanded that it be modeled. + bool should_merge_parent_and_child(const Joint& joint) { return joint.is_weld() && !joint.must_be_modeled() && should_merge_link_composites(joint.model_instance()); } diff --git a/multibody/topology/test/link_joint_graph_test.cc b/multibody/topology/test/link_joint_graph_test.cc index 1a2bac8ff9c7..dea399ae35e3 100644 --- a/multibody/topology/test/link_joint_graph_test.cc +++ b/multibody/topology/test/link_joint_graph_test.cc @@ -81,7 +81,7 @@ namespace { // Verify that the enums in link_joint_graph_defs.h work properly as bitmaps. GTEST_TEST(LinkJointGraph, FlagsAndOptions) { const auto is_static = LinkFlags::kStatic; - const auto massless = LinkFlags::kTreatAsMassless; + const auto massless = LinkFlags::kMassless; // Or-ed flags still have LinkFlags type. auto link_flags = is_static | massless; @@ -89,7 +89,7 @@ GTEST_TEST(LinkJointGraph, FlagsAndOptions) { // And is bitwise but still returns LinkFlags, convertible to bool. EXPECT_EQ(link_flags & is_static, LinkFlags::kStatic); - EXPECT_EQ(link_flags & massless, LinkFlags::kTreatAsMassless); + EXPECT_EQ(link_flags & massless, LinkFlags::kMassless); EXPECT_FALSE(static_cast(link_flags & LinkFlags::kMustBeBaseBody)); EXPECT_EQ(link_flags & LinkFlags::kMustBeBaseBody, LinkFlags::kDefault); @@ -333,9 +333,9 @@ GTEST_TEST(LinkJointGraph, AddLinkErrors) { // Addlink accepts flags, but not Shadow which is set internally only. const BodyIndex link2_index = graph.AddLink("link2", ModelInstanceIndex(3), - LinkFlags::kTreatAsMassless | LinkFlags::kMustBeBaseBody); + LinkFlags::kMassless | LinkFlags::kMustBeBaseBody); const LinkJointGraph::Link& link2 = graph.link_by_index(link2_index); - EXPECT_TRUE(link2.treat_as_massless() && link2.must_be_base_body()); + EXPECT_TRUE(link2.is_massless() && link2.must_be_base_body()); EXPECT_FALSE(link2.is_static_flag_set() || link2.is_shadow()); DRAKE_EXPECT_THROWS_MESSAGE( @@ -378,14 +378,14 @@ GTEST_TEST(LinkJoinGraph, LinkAPITest) { EXPECT_FALSE(link5.is_world()); EXPECT_FALSE(link5.is_anchored()); EXPECT_FALSE(link5.is_static_flag_set()); - EXPECT_FALSE(link5.treat_as_massless()); + EXPECT_FALSE(link5.is_massless()); EXPECT_FALSE(link5.is_shadow()); EXPECT_TRUE(link5.must_be_base_body()); LinkJointGraphTester::set_link_flags(LinkFlags::kStatic, &link5); EXPECT_TRUE(link5.is_static_flag_set()); EXPECT_TRUE(link5.is_anchored()); // Static links are anchored to World. - EXPECT_TRUE(link5.must_be_base_body()); // Unchanged. - EXPECT_FALSE(link5.treat_as_massless()); // Unchanged. + EXPECT_TRUE(link5.must_be_base_body()); // Unchanged. + EXPECT_FALSE(link5.is_massless()); // Unchanged. // Only LinkJointGraph sets these; no public interface. EXPECT_TRUE(link5.joints().empty()); diff --git a/multibody/topology/test/spanning_forest_test.cc b/multibody/topology/test/spanning_forest_test.cc index d9b3f81a77f1..080b7200a4c2 100644 --- a/multibody/topology/test/spanning_forest_test.cc +++ b/multibody/topology/test/spanning_forest_test.cc @@ -1102,9 +1102,9 @@ GTEST_TEST(SpanningForest, SimpleTrees) { // Define the graph. const std::set massless{2, 4, 8}; for (int i = 1; i <= 10; ++i) { - graph.AddLink("link" + std::to_string(i), model_instance, - massless.contains(i) ? LinkFlags::kTreatAsMassless - : LinkFlags::kDefault); + graph.AddLink( + "link" + std::to_string(i), model_instance, + massless.contains(i) ? LinkFlags::kMassless : LinkFlags::kDefault); } const std::vector> joints{ {3, 1}, {3, 2}, {8, 3}, {10, 8}, {10, 9}, {9, 4}, {9, 7}}; @@ -1216,7 +1216,7 @@ GTEST_TEST(SpanningForest, MasslessLinksChangeLoopBreaking) { // Changing just 3 to massless results in the same forest. // (Tests Case 2 in ExtendTreesOneLevel()) - graph.ChangeLinkFlags(BodyIndex(3), LinkFlags::kTreatAsMassless); + graph.ChangeLinkFlags(BodyIndex(3), LinkFlags::kMassless); EXPECT_TRUE(graph.BuildForest()); EXPECT_EQ(ssize(graph.links()), 8); @@ -1232,7 +1232,7 @@ GTEST_TEST(SpanningForest, MasslessLinksChangeLoopBreaking) { // Changing both 3 and 4 to massless breaks the loop at 6 instead of 4. // (Tests Case 3 in ExtendTreesOneLevel()) - graph.ChangeLinkFlags(BodyIndex(4), LinkFlags::kTreatAsMassless); + graph.ChangeLinkFlags(BodyIndex(4), LinkFlags::kMassless); EXPECT_TRUE(graph.BuildForest()); EXPECT_EQ(ssize(graph.links()), 8); @@ -1283,8 +1283,8 @@ GTEST_TEST(SpanningForest, MasslessBodiesShareSplitLink) { graph.RegisterJointType("prismatic", 1, 1); const ModelInstanceIndex model_instance(19); - graph.AddLink("massless_1", model_instance, LinkFlags::kTreatAsMassless); - graph.AddLink("massless_2", model_instance, LinkFlags::kTreatAsMassless); + graph.AddLink("massless_1", model_instance, LinkFlags::kMassless); + graph.AddLink("massless_2", model_instance, LinkFlags::kMassless); graph.AddLink("link_3", model_instance); graph.AddJoint("prismatic_0", model_instance, "prismatic", world_index(), @@ -1469,7 +1469,7 @@ GTEST_TEST(SpanningForest, WorldCompositeComesFirst) { graph.RegisterJointType("revolute", 1, 1); const ModelInstanceIndex model_instance(5); // arbitrary - graph.AddLink("massless_link_1", model_instance, LinkFlags::kTreatAsMassless); + graph.AddLink("massless_link_1", model_instance, LinkFlags::kMassless); graph.AddLink("link2", model_instance); graph.AddLink("link3", model_instance); graph.AddLink("link4", model_instance); @@ -1641,7 +1641,7 @@ GTEST_TEST(SpanningForest, ShadowLinkPreservesJointOrder) { } // Now make link3 massless, rebuild, and check a few things. - graph.ChangeLinkFlags(BodyIndex(3), LinkFlags::kTreatAsMassless); + graph.ChangeLinkFlags(BodyIndex(3), LinkFlags::kMassless); graph.BuildForest(); const LinkJointGraph::Link& new_primary_link = graph.link_by_index(BodyIndex(2)); @@ -1699,9 +1699,9 @@ GTEST_TEST(SpanningForest, LoopWithComposites) { const std::set massless{3, 4}; for (int i = 1; i <= 10; ++i) { - graph.AddLink("link" + std::to_string(i), model_instance, - massless.contains(i) ? LinkFlags::kTreatAsMassless - : LinkFlags::kDefault); + graph.AddLink( + "link" + std::to_string(i), model_instance, + massless.contains(i) ? LinkFlags::kMassless : LinkFlags::kDefault); } const std::vector> weld_joints{{1, 2}, {3, 4}, {5, 6}}; @@ -1846,9 +1846,9 @@ GTEST_TEST(SpanningForest, MasslessMergedComposites) { const std::set massless{4, 5, 6}; for (int i = 1; i <= 8; ++i) { - graph.AddLink("link" + std::to_string(i), model_instance, - massless.contains(i) ? LinkFlags::kTreatAsMassless - : LinkFlags::kDefault); + graph.AddLink( + "link" + std::to_string(i), model_instance, + massless.contains(i) ? LinkFlags::kMassless : LinkFlags::kDefault); } const std::vector> revolute_joints{ @@ -1926,8 +1926,8 @@ GTEST_TEST(SpanningForest, MasslessMergedComposites) { massful with which to end the branch in Tree 1. This should affect when we see the loop so Tree 0 will have height 3 and Tree 1 height 4, with link 3 split. */ - graph.ChangeLinkFlags(BodyIndex(7), LinkFlags::kTreatAsMassless); - graph.ChangeLinkFlags(BodyIndex(8), LinkFlags::kTreatAsMassless); + graph.ChangeLinkFlags(BodyIndex(7), LinkFlags::kMassless); + graph.ChangeLinkFlags(BodyIndex(8), LinkFlags::kMassless); EXPECT_TRUE(graph.BuildForest()); const auto& newer_shadow_link = graph.link_by_index(BodyIndex(9)); EXPECT_TRUE(newer_shadow_link.is_shadow());