[multibody] Assign coordinate starts to all joints in topology

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 */ {

multibody/plant/multibody_plant.cc

@@ -2019,9 +2019,11 @@ std::vector<std::string> MultibodyPlant<T>::GetPositionNames(
   std::vector<std::string> names(num_positions(model_instance));
   std::vector<JointIndex> 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<std::string> MultibodyPlant<T>::GetVelocityNames(
 
   for (JointIndex joint_index : GetJointIndices()) {
     const Joint<T>& 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<std::string> MultibodyPlant<T>::GetVelocityNames(
   std::vector<std::string> names(num_velocities(model_instance));
   std::vector<JointIndex> 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<T>& 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);

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<WeldJoint>(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);

multibody/plant/test/sap_driver_multidof_joints_test.cc

@@ -88,13 +88,6 @@ class MultiDofJointWithLimits final : public Joint<T> {
   template <typename>
   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<Mobilizer<T>> MakeMobilizerForJoint(
       const SpanningForest::Mobod& mobod,
       MultibodyTree<T>* tree) const override {

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.

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(

multibody/topology/link_joint_graph_joint.h

@@ -93,6 +93,20 @@ class LinkJointGraph::Joint {
     return std::get<MobodIndex>(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<std::monostate, MobodIndex, WeldedLinksAssemblyIndex>
       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_;

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) {

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

multibody/tree/ball_rpy_joint.h

@@ -212,18 +212,6 @@ class BallRpyJoint final : public Joint<T> {
   }
 
  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);
   }

multibody/tree/curvilinear_joint.h

@@ -300,18 +300,6 @@ class CurvilinearJoint final : public Joint<T> {
   }
 
  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);
   }