From 795899e6f6e2cc4bc7d5dde940736fcf816b9b76 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Thu, 27 Aug 2026 17:36:57 -0700 Subject: [PATCH] [multibody] Assign joint coordinate starts in topology Store q_start and v_start for every LinkJointGraph joint during forest modeling, including unmodeled welds in fused assemblies. Simplify MultibodyPlant and MultibodyTree to use those assignments directly. Derive Joint coordinate counts from its existing limit vectors instead of virtual methods. Add an end-to-end MultibodyPlant regression test that checks all modeled and fused welds, including a weld fused into World. --- .../generated_docstrings/multibody_tree.h | 59 ++++------------- multibody/plant/multibody_plant.cc | 12 ++-- multibody/plant/test/fused_welds_test.cc | 32 ++++++++++ .../test/sap_driver_multidof_joints_test.cc | 7 -- multibody/topology/link_joint_graph.h | 4 ++ multibody/topology/link_joint_graph_inlines.h | 9 +++ multibody/topology/link_joint_graph_joint.h | 22 +++++++ multibody/topology/spanning_forest.cc | 21 ++++++ .../topology/test/spanning_forest_test.cc | 59 +++++++++++++++++ multibody/tree/ball_rpy_joint.h | 12 ---- multibody/tree/curvilinear_joint.h | 12 ---- multibody/tree/joint.h | 64 ++++++++----------- multibody/tree/multibody_tree.cc | 12 +++- multibody/tree/planar_joint.h | 12 ---- multibody/tree/prismatic_joint.h | 12 ---- multibody/tree/quaternion_floating_joint.h | 12 ---- multibody/tree/revolute_joint.h | 12 ---- multibody/tree/rpy_floating_joint.h | 12 ---- multibody/tree/screw_joint.h | 12 ---- multibody/tree/universal_joint.h | 12 ---- multibody/tree/weld_joint.h | 18 ------ 21 files changed, 203 insertions(+), 224 deletions(-) diff --git a/bindings/generated_docstrings/multibody_tree.h b/bindings/generated_docstrings/multibody_tree.h index 05be43d00aa1..4821125da263 100644 --- a/bindings/generated_docstrings/multibody_tree.h +++ b/bindings/generated_docstrings/multibody_tree.h @@ -3738,39 +3738,6 @@ the particular joint type). Note: The default generalized velocities v₀ are zero for every joint.)"""; } default_positions; - // Symbol: drake::multibody::Joint::do_get_num_positions - struct /* do_get_num_positions */ { - // Source: drake/multibody/tree/joint.h - const char* doc = -R"""(Implementation of the NVI num_positions(), see num_positions() for -details. - -Note: - Implementations must meet the styleguide requirements for - snake_case accessor methods.)"""; - } do_get_num_positions; - // Symbol: drake::multibody::Joint::do_get_num_velocities - struct /* do_get_num_velocities */ { - // Source: drake/multibody/tree/joint.h - const char* doc = -R"""(Implementation of the NVI num_velocities(), see num_velocities() for -details. - -Note: - Implementations must meet the styleguide requirements for - snake_case accessor methods.)"""; - } do_get_num_velocities; - // Symbol: drake::multibody::Joint::do_get_position_start - struct /* do_get_position_start */ { - // Source: drake/multibody/tree/joint.h - const char* doc = -R"""(Implementation of the NVI position_start(), see position_start() for -details. - -Note: - Implementations must meet the styleguide requirements for - snake_case accessor methods.)"""; - } do_get_position_start; // Symbol: drake::multibody::Joint::do_get_position_suffix struct /* do_get_position_suffix */ { // Source: drake/multibody/tree/joint.h @@ -3779,18 +3746,6 @@ R"""(Implementation of the NVI position_suffix(), see position_suffix() for details. The suffix should contain only alphanumeric characters (e.g. 'wx' not '_wx' or '.wx').)"""; } do_get_position_suffix; - // Symbol: drake::multibody::Joint::do_get_velocity_start - struct /* do_get_velocity_start */ { - // Source: drake/multibody/tree/joint.h - const char* doc = -R"""(Implementation of the NVI velocity_start(), see velocity_start() for -details. Note that this must be the offset within just the velocity -vector, *not* within the composite state vector. - -Note: - Implementations must meet the styleguide requirements for - snake_case accessor methods.)"""; - } do_get_velocity_start; // Symbol: drake::multibody::Joint::do_get_velocity_suffix struct /* do_get_velocity_suffix */ { // Source: drake/multibody/tree/joint.h @@ -3918,7 +3873,12 @@ R"""(Returns a const reference to the parent body P.)"""; const char* doc = R"""(Returns the index to the first generalized position for this joint within the vector q of generalized positions for the full multibody -system.)"""; +system. For a zero-dof joint, this is where its positions would have +started had it had any. An unmodeled weld within a fused Mobod +inherits that Mobod's position start. + +Precondition: + The MultibodyPlant has been finalized.)"""; } position_start; // Symbol: drake::multibody::Joint::position_suffix struct /* position_suffix */ { @@ -4048,7 +4008,12 @@ R"""(Returns a string identifying the type of ``this`` joint, such as const char* doc = R"""(Returns the index to the first generalized velocity for this joint within the vector v of generalized velocities for the full multibody -system.)"""; +system. For a zero-dof joint, this is where its velocities would have +started had it had any. An unmodeled weld within a fused Mobod +inherits that Mobod's velocity start. + +Precondition: + The MultibodyPlant has been finalized.)"""; } velocity_start; // Symbol: drake::multibody::Joint::velocity_suffix struct /* velocity_suffix */ { diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index 262d0732fd2f..5acac3fadc12 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -2019,9 +2019,11 @@ std::vector MultibodyPlant::GetPositionNames( std::vector names(num_positions(model_instance)); std::vector joint_indices = GetJointIndices(model_instance); // The offset into the position array is the position_start of the first - // mobilizer in the tree; here we just take the minimum. + // joint with positions; here we just take the minimum. Zero-dof joints can + // inherit a start from a fused Mobod in a different model instance. int position_offset = num_positions(); for (const auto& joint_index : joint_indices) { + if (get_joint(joint_index).num_positions() == 0) continue; position_offset = std::min(position_offset, get_joint(joint_index).position_start()); } @@ -2060,7 +2062,7 @@ std::vector MultibodyPlant::GetVelocityNames( for (JointIndex joint_index : GetJointIndices()) { const Joint& joint = get_joint(joint_index); - if (joint.num_positions() == 0) continue; // Skip welds. + if (joint.num_velocities() == 0) continue; // Skip welds. const std::string prefix = add_model_instance_prefix @@ -2086,16 +2088,18 @@ std::vector MultibodyPlant::GetVelocityNames( std::vector names(num_velocities(model_instance)); std::vector joint_indices = GetJointIndices(model_instance); // The offset into the velocity array is the velocity_start of the first - // mobilizer in the tree; here we just take the minimum. + // joint with velocities; here we just take the minimum. Zero-dof joints can + // inherit a start from a fused Mobod in a different model instance. int velocity_offset = num_velocities(); for (const auto& joint_index : joint_indices) { + if (get_joint(joint_index).num_velocities() == 0) continue; velocity_offset = std::min(velocity_offset, get_joint(joint_index).velocity_start()); } for (const auto& joint_index : joint_indices) { const Joint& joint = get_joint(joint_index); - if (joint.num_positions() == 0) continue; // Skip welds. + if (joint.num_velocities() == 0) continue; // Skip welds. // Sanity check: joint velocities are in range. DRAKE_DEMAND(joint.velocity_start() >= velocity_offset); diff --git a/multibody/plant/test/fused_welds_test.cc b/multibody/plant/test/fused_welds_test.cc index ff947a36d606..415f3292ea8f 100644 --- a/multibody/plant/test/fused_welds_test.cc +++ b/multibody/plant/test/fused_welds_test.cc @@ -157,6 +157,38 @@ TestModel MakeModel(bool fuse_welded_links) { return m; } +/* Verifies the user-facing Joint coordinate accessors for a WeldJoint that is +modeled by a zero-dof mobilizer without fusion, but is unmodeled when its links +are fused into a single Mobod. */ +GTEST_TEST(FusedTest, UnmodeledWeldJointCoordinateStarts) { + const TestModel unfused_model = MakeModel(false); + const TestModel fused_model = MakeModel(true); + + auto verify_weld_starts = [](const TestModel& model, bool is_fused, + int expected_start) { + const auto& graph = GetInternalTree(*model.plant).graph(); + for (const char* name : {"weld12", "weld23", "weldW4"}) { + SCOPED_TRACE(fmt::format("{} model, joint {}", + is_fused ? "fused" : "unfused", name)); + const auto& weld = model.plant->GetJointByName(name); + const auto& graph_joint = graph.joint_by_index(weld.index()); + + // Confirm that the fixtures exercise both the modeled and unmodeled + // paths. All three welds have the same q and v start within each model. + EXPECT_EQ(graph_joint.mobod_index().is_valid(), !is_fused); + EXPECT_EQ(weld.position_start(), expected_start); + EXPECT_EQ(weld.velocity_start(), expected_start); + } + }; + + // Without fusion, every modeled zero-dof weld follows the model's single + // revolute coordinate. With fusion, every unmodeled weld inherits the start + // of its fused Mobod; both fused Mobods start at zero. This includes weldW4, + // whose links are fused into the World Mobod. + verify_weld_starts(unfused_model, false, 1); + verify_weld_starts(fused_model, true, 0); +} + // Sets the revolute joint angle (q) and angular velocity (v) in the model. void SetState(const TestModel& m, double angle_rad, double angular_vel) { m.revolute->set_angle(m.context.get(), angle_rad); diff --git a/multibody/plant/test/sap_driver_multidof_joints_test.cc b/multibody/plant/test/sap_driver_multidof_joints_test.cc index 74cbbeb4923e..600e8ed8a1ea 100644 --- a/multibody/plant/test/sap_driver_multidof_joints_test.cc +++ b/multibody/plant/test/sap_driver_multidof_joints_test.cc @@ -88,13 +88,6 @@ class MultiDofJointWithLimits final : public Joint { template friend class MultiDofJointWithLimits; - int do_get_num_velocities() const override { return kNumDofs; } - int do_get_num_positions() const override { return kNumDofs; } - // Dummy implementation, knowing our unit tests below have a single joint of - // this type. - int do_get_velocity_start() const override { return 0; } - int do_get_position_start() const override { return 0; } - std::unique_ptr> MakeMobilizerForJoint( const SpanningForest::Mobod& mobod, MultibodyTree* tree) const override { diff --git a/multibody/topology/link_joint_graph.h b/multibody/topology/link_joint_graph.h index 1d12b650d18b..bfd25c22739e 100644 --- a/multibody/topology/link_joint_graph.h +++ b/multibody/topology/link_joint_graph.h @@ -781,6 +781,10 @@ class LinkJointGraph { // Tells this currently-unmodeled Joint that the given Mobod models it. void set_mobod_for_joint(JointOrdinal joint_ordinal, MobodIndex mobod_index); + // Records the coordinate starts assigned to this processed Joint. + void set_joint_coordinate_starts(JointOrdinal joint_ordinal, int q_start, + int v_start); + // The World Link must already be in the graph but there are no // WeldedLinksAssemblies yet. This creates the 0th WeldedLinksAssembly and // puts World in it. diff --git a/multibody/topology/link_joint_graph_inlines.h b/multibody/topology/link_joint_graph_inlines.h index b258f8c93d8f..da9a92b0d22a 100644 --- a/multibody/topology/link_joint_graph_inlines.h +++ b/multibody/topology/link_joint_graph_inlines.h @@ -96,6 +96,15 @@ inline void LinkJointGraph::set_mobod_for_joint(JointOrdinal joint_ordinal, joint.how_modeled_ = mobod_index; } +inline void LinkJointGraph::set_joint_coordinate_starts( + JointOrdinal joint_ordinal, int q_start, int v_start) { + Joint& joint = mutable_joint(joint_ordinal); + DRAKE_ASSERT(joint.has_been_processed()); + DRAKE_ASSERT(q_start >= 0 && v_start >= 0); + joint.q_start_ = q_start; + joint.v_start_ = v_start; +} + // LinkJointGraph definitions deferred until LoopConstraint defined. inline const LinkJointGraph::LoopConstraint& LinkJointGraph::loop_constraints( diff --git a/multibody/topology/link_joint_graph_joint.h b/multibody/topology/link_joint_graph_joint.h index c9bcb301686f..2936dbf4a55e 100644 --- a/multibody/topology/link_joint_graph_joint.h +++ b/multibody/topology/link_joint_graph_joint.h @@ -93,6 +93,20 @@ class LinkJointGraph::Joint { return std::get(how_modeled_); } + /* Returns the starting offset within the contiguous q vector assigned to + this %Joint during forest building. For a zero-dof %Joint, this is where its + coordinates would have started had it had any. For an unmodeled weld in a + fused WeldedLinksAssembly, this is the start of the fused Mobod that contains + the weld's Links. Returns -1 if there is no valid SpanningForest. */ + int q_start() const { return q_start_; } + + /* Returns the starting offset within the contiguous v vector assigned to + this %Joint during forest building. For a zero-dof %Joint, this is where its + coordinates would have started had it had any. For an unmodeled weld in a + fused WeldedLinksAssembly, this is the start of the fused Mobod that contains + the weld's Links. Returns -1 if there is no valid SpanningForest. */ + int v_start() const { return v_start_; } + /* (Internal use only) During construction of the forest, this is used to check whether this %Joint has already been modeled. */ bool has_been_processed() const { @@ -137,6 +151,8 @@ class LinkJointGraph::Joint { void ClearModel() { how_modeled_ = std::monostate{}; + q_start_ = -1; + v_start_ = -1; effective_parent_link_index_ = parent_link_index_; effective_child_link_index_ = child_link_index_; } @@ -190,6 +206,12 @@ class LinkJointGraph::Joint { std::variant how_modeled_; + // Coordinate assignments. For zero-dof joints these are still set to where + // coordinates would have started if there were any. Unmodeled welds inherit + // these values from their fused Mobod. + int q_start_{-1}; // within the full q vector + int v_start_{-1}; // within the full v vector + // These are set to the user's originals on construction and when the // forest is cleared or rebuilt. LinkIndex effective_parent_link_index_; diff --git a/multibody/topology/spanning_forest.cc b/multibody/topology/spanning_forest.cc index f73ee24af749..97e76d43d2ab 100644 --- a/multibody/topology/spanning_forest.cc +++ b/multibody/topology/spanning_forest.cc @@ -357,6 +357,27 @@ void SpanningForest::AssignCoordinates() { next_v += mobod.nv_; } + /* Assign a coordinate start to every Joint, including unmodeled welds in + fused WeldedLinksAssemblies. A modeled Joint gets the start of the Mobod + whose mobilizer models it. Both Links of an unmodeled weld necessarily + follow the same fused Mobod, whose start the Joint inherits. */ + for (JointOrdinal joint_ordinal{0}; joint_ordinal < ssize(joints()); + ++joint_ordinal) { + LinkJointGraph::Joint& joint = mutable_graph().mutable_joint(joint_ordinal); + MobodIndex mobod_index = joint.mobod_index(); + if (!mobod_index.is_valid()) { + const MobodIndex parent_mobod = + link_by_index(joint.effective_parent_link_index()).mobod_index(); + const MobodIndex child_mobod = + link_by_index(joint.effective_child_link_index()).mobod_index(); + DRAKE_DEMAND(parent_mobod.is_valid() && parent_mobod == child_mobod); + mobod_index = parent_mobod; + } + const Mobod& mobod = mobods(mobod_index); + mutable_graph().set_joint_coordinate_starts(joint_ordinal, mobod.q_start(), + mobod.v_start()); + } + /* O(n) inward pass counts outboard bodies and coordinates for each Mobod. (the rest of 3.3) */ for (auto m = data_.mobods.rbegin(); m != data_.mobods.rend(); ++m) { diff --git a/multibody/topology/test/spanning_forest_test.cc b/multibody/topology/test/spanning_forest_test.cc index 17804b94a5cc..681f1014221c 100644 --- a/multibody/topology/test/spanning_forest_test.cc +++ b/multibody/topology/test/spanning_forest_test.cc @@ -220,6 +220,65 @@ GTEST_TEST(SpanningForest, TreeAndLoopConstraintAPIs) { EXPECT_EQ(tree1.nv(), 1); } +/* Joint coordinate starts are assigned by the topology code for every Joint, +including welds that are unmodeled when a WeldedLinksAssembly is fused. */ +GTEST_TEST(SpanningForest, JointCoordinateStartsForFusedAndUnfusedWelds) { + LinkJointGraph graph; + graph.RegisterJointType("two_positions_one_velocity", 2, 1); + graph.RegisterJointType("revolute", 1, 1); + + const ModelInstanceIndex model_instance(1); + const LinkIndex link1 = graph.AddLink("link1", model_instance); + const LinkIndex link2 = graph.AddLink("link2", model_instance); + const LinkIndex link3 = graph.AddLink("link3", model_instance); + const LinkIndex link4 = graph.AddLink("link4", model_instance); + + const JointIndex moving_joint = + graph.AddJoint("moving", model_instance, "two_positions_one_velocity", + world_index(), link1); + const JointIndex weld12 = + graph.AddJoint("weld12", model_instance, "weld", link1, link2); + const JointIndex weld23 = + graph.AddJoint("weld23", model_instance, "weld", link2, link3); + const JointIndex tip_joint = + graph.AddJoint("tip", model_instance, "revolute", link3, link4); + + // Without fusion each weld has a zero-dof Mobod. Its start is where that + // Mobod's coordinates would have begun. + ASSERT_TRUE(graph.BuildForest()); + EXPECT_EQ(graph.joint_by_index(moving_joint).q_start(), 0); + EXPECT_EQ(graph.joint_by_index(moving_joint).v_start(), 0); + EXPECT_EQ(graph.joint_by_index(weld12).q_start(), 2); + EXPECT_EQ(graph.joint_by_index(weld12).v_start(), 1); + EXPECT_EQ(graph.joint_by_index(weld23).q_start(), 2); + EXPECT_EQ(graph.joint_by_index(weld23).v_start(), 1); + EXPECT_EQ(graph.joint_by_index(tip_joint).q_start(), 2); + EXPECT_EQ(graph.joint_by_index(tip_joint).v_start(), 1); + EXPECT_TRUE(graph.joint_by_index(weld12).mobod_index().is_valid()); + EXPECT_TRUE(graph.joint_by_index(weld23).mobod_index().is_valid()); + + // Rebuilding with fusion invalidates the old assignments until BuildForest + // assigns them again. + graph.SetGlobalForestBuildingOptions( + ForestBuildingOptions::kFuseWeldedLinksAssemblies); + EXPECT_EQ(graph.joint_by_index(moving_joint).q_start(), -1); + EXPECT_EQ(graph.joint_by_index(moving_joint).v_start(), -1); + EXPECT_EQ(graph.joint_by_index(weld12).q_start(), -1); + EXPECT_EQ(graph.joint_by_index(weld12).v_start(), -1); + + ASSERT_TRUE(graph.BuildForest()); + EXPECT_EQ(graph.joint_by_index(moving_joint).q_start(), 0); + EXPECT_EQ(graph.joint_by_index(moving_joint).v_start(), 0); + EXPECT_EQ(graph.joint_by_index(weld12).q_start(), 0); + EXPECT_EQ(graph.joint_by_index(weld12).v_start(), 0); + EXPECT_EQ(graph.joint_by_index(weld23).q_start(), 0); + EXPECT_EQ(graph.joint_by_index(weld23).v_start(), 0); + EXPECT_EQ(graph.joint_by_index(tip_joint).q_start(), 2); + EXPECT_EQ(graph.joint_by_index(tip_joint).v_start(), 1); + EXPECT_FALSE(graph.joint_by_index(weld12).mobod_index().is_valid()); + EXPECT_FALSE(graph.joint_by_index(weld23).mobod_index().is_valid()); +} + /* Creates a straightforward graph of two trees each with multiple branches, plus a lone unattached free link. There are no welds or reverse joints or loops. We intentionally jumble the link numbering to make sure we don't get the right diff --git a/multibody/tree/ball_rpy_joint.h b/multibody/tree/ball_rpy_joint.h index 685878d1d0c4..20245c0d5bae 100644 --- a/multibody/tree/ball_rpy_joint.h +++ b/multibody/tree/ball_rpy_joint.h @@ -212,18 +212,6 @@ class BallRpyJoint final : public Joint { } private: - int do_get_velocity_start() const final { - return get_mobilizer().velocity_start_in_v(); - } - - int do_get_num_velocities() const final { return 3; } - - int do_get_position_start() const final { - return get_mobilizer().position_start_in_q(); - } - - int do_get_num_positions() const final { return 3; } - std::string do_get_position_suffix(int index) const final { return get_mobilizer().position_suffix(index); } diff --git a/multibody/tree/curvilinear_joint.h b/multibody/tree/curvilinear_joint.h index 90bf6bb52bd4..083a2565cbd5 100644 --- a/multibody/tree/curvilinear_joint.h +++ b/multibody/tree/curvilinear_joint.h @@ -300,18 +300,6 @@ class CurvilinearJoint final : public Joint { } private: - int do_get_velocity_start() const final { - return get_mobilizer().velocity_start_in_v(); - } - - int do_get_num_velocities() const final { return 1; } - - int do_get_position_start() const final { - return get_mobilizer().position_start_in_q(); - } - - int do_get_num_positions() const final { return 1; } - std::string do_get_position_suffix(int index) const final { return get_mobilizer().position_suffix(index); } diff --git a/multibody/tree/joint.h b/multibody/tree/joint.h index 10c44302d956..e1707a31c0fa 100644 --- a/multibody/tree/joint.h +++ b/multibody/tree/joint.h @@ -160,11 +160,8 @@ class Joint : public MultibodyElement { DRAKE_DEMAND((vel_lower_limits.array() <= vel_upper_limits.array()).all()); DRAKE_DEMAND((acc_lower_limits.array() <= acc_upper_limits.array()).all()); - // N.B. We cannot use `num_positions()` here because it is virtual. - const int num_positions = pos_lower_limits.size(); - // initialize the default positions. - default_positions_ = VectorX::Zero(num_positions); + default_positions_ = VectorX::Zero(num_positions()); } /// Additional constructor overload for joints with zero damping. Refer to @@ -222,24 +219,42 @@ class Joint : public MultibodyElement { /// Returns the index to the first generalized velocity for this joint /// within the vector v of generalized velocities for the full multibody - /// system. - int velocity_start() const { return do_get_velocity_start(); } + /// system. For a zero-dof joint, this is where its velocities would have + /// started had it had any. An unmodeled weld within a fused Mobod inherits + /// that Mobod's velocity start. + /// @pre The MultibodyPlant has been finalized. + int velocity_start() const { + const int start = + this->get_parent_tree().graph().joint_by_index(index()).v_start(); + DRAKE_ASSERT(start >= 0); + return start; + } /// Returns the number of generalized velocities describing this joint. int num_velocities() const { - DRAKE_ASSERT(0 <= do_get_num_velocities() && do_get_num_velocities() <= 6); - return do_get_num_velocities(); + const int result = vel_lower_limits_.size(); + DRAKE_ASSERT(0 <= result && result <= 6); + return result; } /// Returns the index to the first generalized position for this joint /// within the vector q of generalized positions for the full multibody - /// system. - int position_start() const { return do_get_position_start(); } + /// system. For a zero-dof joint, this is where its positions would have + /// started had it had any. An unmodeled weld within a fused Mobod inherits + /// that Mobod's position start. + /// @pre The MultibodyPlant has been finalized. + int position_start() const { + const int start = + this->get_parent_tree().graph().joint_by_index(index()).q_start(); + DRAKE_ASSERT(start >= 0); + return start; + } /// Returns the number of generalized positions describing this joint. int num_positions() const { - DRAKE_ASSERT(0 <= do_get_num_positions() && do_get_num_positions() <= 7); - return do_get_num_positions(); + const int result = pos_lower_limits_.size(); + DRAKE_ASSERT(0 <= result && result <= 7); + return result; } /// Returns true if this joint's mobility allows relative rotation of the @@ -803,31 +818,6 @@ class Joint : public MultibodyElement { // End of hidden Doxygen section. protected: - /// Implementation of the NVI velocity_start(), see velocity_start() for - /// details. Note that this must be the offset within just the velocity - /// vector, _not_ within the composite state vector. - /// @note Implementations must meet the styleguide requirements for snake_case - /// accessor methods. - virtual int do_get_velocity_start() const = 0; - - /// Implementation of the NVI num_velocities(), see num_velocities() for - /// details. - /// @note Implementations must meet the styleguide requirements for snake_case - /// accessor methods. - virtual int do_get_num_velocities() const = 0; - - /// Implementation of the NVI position_start(), see position_start() for - /// details. - /// @note Implementations must meet the styleguide requirements for snake_case - /// accessor methods. - virtual int do_get_position_start() const = 0; - - /// Implementation of the NVI num_positions(), see num_positions() for - /// details. - /// @note Implementations must meet the styleguide requirements for - /// snake_case accessor methods. - virtual int do_get_num_positions() const = 0; - /// Implementation of the NVI position_suffix(), see position_suffix() for /// details. The suffix should contain only alphanumeric characters (e.g. /// 'wx' not '_wx' or '.wx'). diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index c4064aff87bb..d59b133b3ae7 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -86,9 +86,15 @@ void MultibodyTree::RegisterJointAndMaybeJointTypeInGraph( joint.num_velocities(), has_quaternion); } // Note changes in the graph. - link_joint_graph_.AddJoint(joint.name(), joint.model_instance(), type_name, - joint.parent_body().index(), - joint.child_body().index()); + const JointIndex graph_joint_index = link_joint_graph_.AddJoint( + joint.name(), joint.model_instance(), type_name, + joint.parent_body().index(), joint.child_body().index()); + const LinkJointGraph::Joint& graph_joint = + link_joint_graph_.joint_by_index(graph_joint_index); + const LinkJointGraph::JointTraits& traits = + link_joint_graph_.joint_traits(graph_joint.traits_index()); + DRAKE_DEMAND(traits.nq == joint.num_positions()); + DRAKE_DEMAND(traits.nv == joint.num_velocities()); } template diff --git a/multibody/tree/planar_joint.h b/multibody/tree/planar_joint.h index 8207309af79c..446d2896c073 100644 --- a/multibody/tree/planar_joint.h +++ b/multibody/tree/planar_joint.h @@ -293,18 +293,6 @@ class PlanarJoint final : public Joint { tau[2] -= damping_coeff[2] * v_angular; } - int do_get_velocity_start() const final { - return get_mobilizer().velocity_start_in_v(); - } - - int do_get_num_velocities() const final { return 3; } - - int do_get_position_start() const final { - return get_mobilizer().position_start_in_q(); - } - - int do_get_num_positions() const final { return 3; } - std::string do_get_position_suffix(int index) const final { return get_mobilizer().position_suffix(index); } diff --git a/multibody/tree/prismatic_joint.h b/multibody/tree/prismatic_joint.h index 97c9f0f4c704..f11035f95021 100644 --- a/multibody/tree/prismatic_joint.h +++ b/multibody/tree/prismatic_joint.h @@ -266,18 +266,6 @@ class PrismaticJoint final : public Joint { } private: - int do_get_velocity_start() const final { - return get_mobilizer().velocity_start_in_v(); - } - - int do_get_num_velocities() const final { return 1; } - - int do_get_position_start() const final { - return get_mobilizer().position_start_in_q(); - } - - int do_get_num_positions() const final { return 1; } - std::string do_get_position_suffix(int index) const final { return get_mobilizer().position_suffix(index); } diff --git a/multibody/tree/quaternion_floating_joint.h b/multibody/tree/quaternion_floating_joint.h index 9c4b97db19d6..2de1944d1e57 100644 --- a/multibody/tree/quaternion_floating_joint.h +++ b/multibody/tree/quaternion_floating_joint.h @@ -375,18 +375,6 @@ class QuaternionFloatingJoint final : public Joint { MultibodyForces* forces) const final; private: - int do_get_velocity_start() const final { - return get_mobilizer().velocity_start_in_v(); - } - - int do_get_num_velocities() const final { return 6; } - - int do_get_position_start() const final { - return get_mobilizer().position_start_in_q(); - } - - int do_get_num_positions() const final { return 7; } - std::string do_get_position_suffix(int index) const final { return get_mobilizer().position_suffix(index); } diff --git a/multibody/tree/revolute_joint.h b/multibody/tree/revolute_joint.h index c019ef26f601..527144c59f4f 100644 --- a/multibody/tree/revolute_joint.h +++ b/multibody/tree/revolute_joint.h @@ -297,18 +297,6 @@ class RevoluteJoint final : public Joint { } private: - int do_get_velocity_start() const final { - return get_mobilizer().velocity_start_in_v(); - } - - int do_get_num_velocities() const final { return 1; } - - int do_get_position_start() const final { - return get_mobilizer().position_start_in_q(); - } - - int do_get_num_positions() const final { return 1; } - std::string do_get_position_suffix(int index) const final { return get_mobilizer().position_suffix(index); } diff --git a/multibody/tree/rpy_floating_joint.h b/multibody/tree/rpy_floating_joint.h index 61f8d3539e3a..c64503869bd3 100644 --- a/multibody/tree/rpy_floating_joint.h +++ b/multibody/tree/rpy_floating_joint.h @@ -384,18 +384,6 @@ class RpyFloatingJoint final : public Joint { } private: - int do_get_velocity_start() const final { - return get_mobilizer().velocity_start_in_v(); - } - - int do_get_num_velocities() const final { return 6; } - - int do_get_position_start() const final { - return get_mobilizer().position_start_in_q(); - } - - int do_get_num_positions() const final { return 6; } - std::string do_get_position_suffix(int index) const final { return get_mobilizer().position_suffix(index); } diff --git a/multibody/tree/screw_joint.h b/multibody/tree/screw_joint.h index ee090acd1096..30b0e0918f5f 100644 --- a/multibody/tree/screw_joint.h +++ b/multibody/tree/screw_joint.h @@ -313,18 +313,6 @@ class ScrewJoint final : public Joint { tau[0] -= this->GetDamping(context) * v_angular; } - int do_get_velocity_start() const final { - return get_mobilizer().velocity_start_in_v(); - } - - int do_get_num_velocities() const final { return 1; } - - int do_get_position_start() const final { - return get_mobilizer().position_start_in_q(); - } - - int do_get_num_positions() const final { return 1; } - std::string do_get_position_suffix(int index) const final { return get_mobilizer().position_suffix(index); } diff --git a/multibody/tree/universal_joint.h b/multibody/tree/universal_joint.h index 1ee386489a79..5b306d1aec63 100644 --- a/multibody/tree/universal_joint.h +++ b/multibody/tree/universal_joint.h @@ -212,18 +212,6 @@ class UniversalJoint final : public Joint { } private: - int do_get_velocity_start() const final { - return get_mobilizer().velocity_start_in_v(); - } - - int do_get_num_velocities() const final { return 2; } - - int do_get_position_start() const final { - return get_mobilizer().position_start_in_q(); - } - - int do_get_num_positions() const final { return 2; } - std::string do_get_position_suffix(int index) const final { return get_mobilizer().position_suffix(index); } diff --git a/multibody/tree/weld_joint.h b/multibody/tree/weld_joint.h index 3c7cb196a70d..4291bff1e957 100644 --- a/multibody/tree/weld_joint.h +++ b/multibody/tree/weld_joint.h @@ -69,24 +69,6 @@ class WeldJoint final : public Joint { } private: - int do_get_velocity_start() const final { - // Since WeldJoint has no state, the start index has no meaning. However, - // we let its decide the return value for this case (this has to do with - // allowing zero sized Eigen blocks). - return get_mobilizer().velocity_start_in_v(); - } - - int do_get_num_velocities() const final { return 0; } - - int do_get_position_start() const final { - // Since WeldJoint has no state, the start index has no meaning. However, - // we let it decide the return value for this case (this has to do with - // allowing zero sized Eigen blocks). - return get_mobilizer().position_start_in_q(); - } - - int do_get_num_positions() const final { return 0; } - std::string do_get_position_suffix(int index) const final { return get_mobilizer().position_suffix(index); }