From f9f893a56763935cc7dc42245fcc968b99a6a079 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Wed, 20 Aug 2025 18:51:46 -0700 Subject: [PATCH 1/7] Got rid of topology use in RigidBody. Killed unused AddMobilizer() overload. Still need to remove RigidBodyTopology, MobilizerTopology, and probably FrameTopology. Watch out for cloning of new fields. --- .../rational/rational_forward_kinematics.cc | 28 +++++--- .../rational_forward_kinematics_internal.cc | 35 +++++---- ...ional_forward_kinematics_test_utilities.cc | 10 ++- multibody/topology/link_joint_graph_link.h | 6 +- .../topology/test/spanning_forest_test.cc | 11 ++- multibody/tree/mobilizer.h | 12 +++- multibody/tree/multibody_tree-inl.h | 14 +--- multibody/tree/multibody_tree.cc | 51 +++++++------ multibody/tree/multibody_tree.h | 44 ------------ multibody/tree/multibody_tree_topology.cc | 13 ++-- multibody/tree/multibody_tree_topology.h | 5 -- multibody/tree/rigid_body.h | 72 ++++++++++--------- .../tree/test/multibody_tree_creation_test.cc | 15 +--- 13 files changed, 135 insertions(+), 181 deletions(-) diff --git a/multibody/rational/rational_forward_kinematics.cc b/multibody/rational/rational_forward_kinematics.cc index 672a0f6e5b09..dfcefd1f8175 100644 --- a/multibody/rational/rational_forward_kinematics.cc +++ b/multibody/rational/rational_forward_kinematics.cc @@ -232,20 +232,28 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial( // X_F'M' = X_FM.inverse() // X_M'C' = X_PF.inverse() const internal::MultibodyTree& tree = GetInternalTree(plant_); - const internal::RigidBodyTopology& parent_topology = - tree.get_topology().get_rigid_body_topology(parent); - const internal::RigidBodyTopology& child_topology = - tree.get_topology().get_rigid_body_topology(child); + const internal::SpanningForest& forest = tree.forest(); + + const internal::MobodIndex parent_mobod_index = + forest.link_by_index(parent).mobod_index(); + const internal::SpanningForest::Mobod& parent_mobod = + forest.mobods(parent_mobod_index); + + const internal::MobodIndex child_mobod_index = + forest.link_by_index(child).mobod_index(); + const internal::SpanningForest::Mobod& child_mobod = + forest.mobods(child_mobod_index); + internal::MobodIndex mobilizer_index; bool is_order_reversed{}; - if (parent_topology.parent_body.is_valid() && - parent_topology.parent_body == child) { + if (parent_mobod.inboard().is_valid() && + parent_mobod.inboard() == child_mobod_index) { is_order_reversed = true; - mobilizer_index = parent_topology.inboard_mobilizer; - } else if (child_topology.parent_body.is_valid() && - child_topology.parent_body == parent) { + mobilizer_index = parent_mobod_index; + } else if (child_mobod.inboard().is_valid() && + child_mobod.inboard() == parent_mobod_index) { is_order_reversed = false; - mobilizer_index = child_topology.inboard_mobilizer; + mobilizer_index = child_mobod_index; } else { throw std::invalid_argument(fmt::format( "CalcChildBodyPoseAsMultilinearPolynomial: the pair of body indices " diff --git a/multibody/rational/rational_forward_kinematics_internal.cc b/multibody/rational/rational_forward_kinematics_internal.cc index 0c0fe6bcec14..72e97f7e765a 100644 --- a/multibody/rational/rational_forward_kinematics_internal.cc +++ b/multibody/rational/rational_forward_kinematics_internal.cc @@ -16,7 +16,8 @@ std::vector FindPath(const MultibodyPlant& plant, BodyIndex start, BodyIndex end) { DRAKE_DEMAND(start.is_valid()); DRAKE_DEMAND(end.is_valid()); - const MultibodyTreeTopology& topology = GetInternalTree(plant).get_topology(); + const MultibodyTree& tree = GetInternalTree(plant); + const SpanningForest& forest = tree.forest(); // Do a breadth first search in the tree. The `worklist` stores the nodes // ready for exploration; In the `ancestor` map, the values are the immediate @@ -40,13 +41,21 @@ std::vector FindPath(const MultibodyPlant& plant, if (current == end) { break; } - const RigidBodyTopology& current_node_topology = - topology.get_rigid_body_topology(current); + const LinkJointGraph::Link& current_link = forest.link_by_index(current); + const SpanningForest::Mobod& current_mobod = + forest.mobods(current_link.mobod_index()); if (current != world_index()) { - const BodyIndex parent = current_node_topology.parent_body; + const SpanningForest::Mobod& parent_mobod = + forest.mobods(current_mobod.inboard()); + const BodyIndex parent = + tree.forest().links(parent_mobod.link_ordinal()).index(); visit_edge(current, parent); } - for (BodyIndex child : current_node_topology.child_bodies) { + for (const MobodIndex& child_mobod_index : current_mobod.outboards()) { + const SpanningForest::Mobod& child_mobod = + forest.mobods(child_mobod_index); + const BodyIndex child = + tree.forest().links(child_mobod.link_ordinal()).index(); visit_edge(current, child); } } @@ -71,20 +80,20 @@ std::vector FindMobilizersOnPath( std::vector mobilizers_on_path; mobilizers_on_path.reserve(path.size() - 1); const MultibodyTree& tree = GetInternalTree(plant); + const SpanningForest& forest = tree.forest(); for (int i = 0; i < static_cast(path.size()) - 1; ++i) { - const RigidBodyTopology& rigid_body_topology = - tree.get_topology().get_rigid_body_topology(path[i]); - if (path[i] != world_index() && - rigid_body_topology.parent_body == path[i + 1]) { + // Figure out which mobilizer connects path[i] with path[i+1]. + const LinkJointGraph::Link& link_i = forest.link_by_index(path[i]); + const LinkJointGraph::Link& link_ip1 = forest.link_by_index(path[i + 1]); + const SpanningForest::Mobod& mobod_i = forest.mobods(link_i.mobod_index()); + if (!mobod_i.is_world() && mobod_i.inboard() == link_ip1.mobod_index()) { // path[i] is the child of path[i+1] in MultibodyTreeTopology, they are // connected by path[i]'s inboard mobilizer. - mobilizers_on_path.push_back(rigid_body_topology.inboard_mobilizer); + mobilizers_on_path.push_back(link_i.mobod_index()); } else { // path[i] is the parent of path[i+1] in MultibodyTreeTopology, they are // connected by path[i+1]'s inboard mobilizer. - mobilizers_on_path.push_back(tree.get_topology() - .get_rigid_body_topology(path[i + 1]) - .inboard_mobilizer); + mobilizers_on_path.push_back(link_ip1.mobod_index()); } } return mobilizers_on_path; diff --git a/multibody/rational/test/rational_forward_kinematics_test_utilities.cc b/multibody/rational/test/rational_forward_kinematics_test_utilities.cc index 250aa9e72162..0d96aa4a8b53 100644 --- a/multibody/rational/test/rational_forward_kinematics_test_utilities.cc +++ b/multibody/rational/test/rational_forward_kinematics_test_utilities.cc @@ -80,9 +80,8 @@ IiwaTest::IiwaTest() for (int i = 0; i < 8; ++i) { iiwa_link_[i] = iiwa_->GetBodyByName("iiwa_link_" + std::to_string(i)).index(); - iiwa_joint_[i] = iiwa_tree_.get_topology() - .get_rigid_body_topology(iiwa_link_[i]) - .inboard_mobilizer; + iiwa_joint_[i] = + iiwa_tree_.forest().link_by_index(iiwa_link_[i]).mobod_index(); } } @@ -93,9 +92,8 @@ FinalizedIiwaTest::FinalizedIiwaTest() for (int i = 0; i < 8; ++i) { iiwa_link_[i] = iiwa_->GetBodyByName("iiwa_link_" + std::to_string(i)).index(); - iiwa_joint_[i] = iiwa_tree_.get_topology() - .get_rigid_body_topology(iiwa_link_[i]) - .inboard_mobilizer; + iiwa_joint_[i] = + iiwa_tree_.forest().link_by_index(iiwa_link_[i]).mobod_index(); } } diff --git a/multibody/topology/link_joint_graph_link.h b/multibody/topology/link_joint_graph_link.h index 0155b210240e..b2aeed0cf028 100644 --- a/multibody/topology/link_joint_graph_link.h +++ b/multibody/topology/link_joint_graph_link.h @@ -113,9 +113,9 @@ class LinkJointGraph::Link { MobodIndex mobod_index() const { return mobod_; } /* Returns the Joint that was used to associate this %Link with its - mobilized body. If this %Link is part of a LinkComposite, returns the Joint - that connects this %Link to the Composite, not necessarily the Joint that is - modeled by the Mobod returned by mobod_index(). */ + mobilized body. If this %Link is part of a LinkComposite, returns the weld + Joint that connects this %Link to the Composite, not necessarily the Joint + that is modeled by the Mobod returned by mobod_index(). */ JointIndex inboard_joint_index() const { return joint_; } /* Returns the index of the LinkComposite this %Link is part of, if any. diff --git a/multibody/topology/test/spanning_forest_test.cc b/multibody/topology/test/spanning_forest_test.cc index aa0112770e3e..88337407a6cf 100644 --- a/multibody/topology/test/spanning_forest_test.cc +++ b/multibody/topology/test/spanning_forest_test.cc @@ -843,7 +843,7 @@ GTEST_TEST(SpanningForest, SerialChainAndMore) { EXPECT_NO_THROW(forest.SanityCheckForest()); // The graph shouldn't change from SpanningForest 1, but the forest will. - EXPECT_EQ(ssize(graph.joints()) - graph.num_user_joints(), 4); + EXPECT_EQ(graph.num_joints() - graph.num_user_joints(), 4); EXPECT_EQ(graph.link_composites(LinkCompositeIndex(0)).links, link_composites0); EXPECT_EQ(graph.link_composites(LinkCompositeIndex(1)).links, @@ -984,7 +984,7 @@ non-composite links are {3}, {9}, {2}, {11}, and {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 mobilizer (rather than making composites from +giving every Link its own mobilizer (rather than merging composites from welded-together ones) we expect this forest of 3 trees and 17 Mobods: level 6 12{16*} .... ... loop constraint @@ -1011,7 +1011,7 @@ though each Link has its own Mobod. Those are: The corresponding Mobods are in WeldedMobod groups: [0 1 2 6] [8 9 14 15] [10 11 13 12] -Remodeling with composite link merging turned on should immediately create +Remodeling with link composite merging 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 @@ -1470,8 +1470,7 @@ sufficient to prevent both massless Links from being terminal. {1} {2} massless 1{1} 3{2} - 🡑 0 🡑 1 🡑 T0 🡑 T1 T -= tree + 🡑 0 🡑 1 🡑 T0 🡑 T1 T = tree ----> ........{0}........ ........ 0 ........ World World @@ -1570,7 +1569,7 @@ The expected as-modeled graph and spanning forest model are: Notes: - Joint numbering determines branch ordering in the tree so the middle branch gets modeled last. - - Model Joint 8 is the added floating joint to World. + - Ephemeral Joint 8 is the added floating joint to World. - Link {8} is {6s} (shadow 1 of Link {6}); {9} is {6ss} (shadow 2). - Loop constraints (shown as . .) are always ordered so that the constraint's "parent" is the primary link and "child" is the shadow link. Thus the order diff --git a/multibody/tree/mobilizer.h b/multibody/tree/mobilizer.h index c502a2f0c729..351ea3c54157 100644 --- a/multibody/tree/mobilizer.h +++ b/multibody/tree/mobilizer.h @@ -3,7 +3,6 @@ #include #include #include -#include #include "drake/common/autodiff.h" #include "drake/common/drake_assert.h" @@ -357,6 +356,10 @@ class Mobilizer : public MultibodyElement { // and any spatial velocity to machine precision. bool has_six_dofs() const { return num_velocities() == 6; } + bool is_floating_base_mobilizer() const { + return is_floating_base_mobilizer_; + } + // Returns `true` if `this` uses a quaternion parameterization of rotations. virtual bool has_quaternion_dofs() const { return false; } @@ -798,6 +801,10 @@ class Mobilizer : public MultibodyElement { is_locked_parameter_index_); } + void set_is_floating_base_mobilizer(bool is_floating) { + is_floating_base_mobilizer_ = is_floating; + } + protected: // NVI to CalcNMatrix(). Implementations can safely assume that N is not the // nullptr and that N has the proper size. @@ -907,6 +914,9 @@ class Mobilizer : public MultibodyElement { // System parameter index for `this` mobilizer's lock state stored in a // context. systems::AbstractParameterIndex is_locked_parameter_index_; + + // Set according to some policy that defines a "floating base body". + bool is_floating_base_mobilizer_{false}; }; } // namespace internal diff --git a/multibody/tree/multibody_tree-inl.h b/multibody/tree/multibody_tree-inl.h index b7b67919ea2a..6a8025f04217 100644 --- a/multibody/tree/multibody_tree-inl.h +++ b/multibody/tree/multibody_tree-inl.h @@ -1,13 +1,10 @@ #pragma once #include -#include #include #include #include -#include #include -#include #include #include @@ -130,7 +127,7 @@ const MobilizerType& MultibodyTree::AddMobilizer( const BodyIndex outboard_body_index = mobilizer->outboard_body().index(); topology_.get_mutable_rigid_body_topology(outboard_body_index) - .is_floating_base = is_floating_base_body; + .is_floating_base = mobilizer->is_floating_base_mobilizer(); topology_.get_mutable_rigid_body_topology(outboard_body_index) .has_quaternion_dofs = mobilizer->has_quaternion_dofs(); @@ -139,15 +136,6 @@ const MobilizerType& MultibodyTree::AddMobilizer( return *raw_mobilizer_ptr; } -template -template