[multibody] Add shadow links and distribute mass properties (closed-loop topology)

bindings/generated_docstrings/multibody_plant.h

@@ -5305,6 +5305,19 @@ See also:
 Raises:
     RuntimeError if called pre-finalize.)""";
         } GetEffortUpperLimits;
+        // Symbol: drake::multibody::MultibodyPlant::GetEnableLoopTopology
+        struct /* GetEnableLoopTopology */ {
+          // Source: drake/multibody/plant/multibody_plant.h
+          const char* doc =
+R"""((Internal use only for now) Returns the current setting for whether
+Finalize() automatically deals with closed-topology (looped) systems.
+
+Note:
+    This function can be called pre-Finalize() or post-Finalize().
+
+See also:
+    SetEnableLoopTopology(), Finalize())""";
+        } GetEnableLoopTopology;
         // Symbol: drake::multibody::MultibodyPlant::GetFloatingBaseBodies
         struct /* GetFloatingBaseBodies */ {
           // Source: drake/multibody/plant/multibody_plant.h
@@ -6817,6 +6830,38 @@ Parameter ``params``:
     if params.bodyA() or params.bodyB() do not correspond to rigid
     bodies in ``this`` MultibodyPlant.)""";
         } SetDistanceConstraintParams;
+        // Symbol: drake::multibody::MultibodyPlant::SetEnableLoopTopology
+        struct /* SetEnableLoopTopology */ {
+          // Source: drake/multibody/plant/multibody_plant.h
+          const char* doc =
+R"""((Internal use only for now) For systems whose links and joints form
+one or more kinematic loops (a "closed topology"), controls whether
+Finalize() should deal with those automatically. The default setting
+is *not* to deal with kinematic loops (if one is encountered, an
+exception is thrown).
+
+Note:
+    This feature is in development and is not yet functional.
+
+To deal with loops automatically requires modifying the system's
+topology so that it is structured as a tree of links and joints, plus
+constraints needed to enforce loop closure. Breaking a loop is done by
+splitting a link within that loop. Mass properties are divided between
+the original ("primary") link and the new ("shadow") link. Then a weld
+constraint is added between the primary and shadow links to enforce
+loop closure. When the weld constraint is satisfied, the original
+physics is restored.
+
+Parameter ``enable``:
+    Whether Finalize() should automatically model closed kinematic
+    loops rather than throwing.
+
+Raises:
+    RuntimeError if called after Finalize().
+
+See also:
+    GetEnableLoopTopology(), Finalize())""";
+        } SetEnableLoopTopology;
         // Symbol: drake::multibody::MultibodyPlant::SetFloatingBaseBodyPoseInAnchoredFrame
         struct /* SetFloatingBaseBodyPoseInAnchoredFrame */ {
           // Source: drake/multibody/plant/multibody_plant.h

bindings/generated_docstrings/multibody_tree.h

@@ -7164,6 +7164,11 @@ Parameter ``context``:
 Precondition:
     the context makes sense for use by this RigidBody.)""";
         } CalcSpatialInertiaInBodyFrame;
+        // Symbol: drake::multibody::RigidBody::CalcSpatialInertiaInBodyFrameFromParameters
+        struct /* CalcSpatialInertiaInBodyFrameFromParameters */ {
+          // Source: drake/multibody/tree/rigid_body.h
+          const char* doc = R"""()""";
+        } CalcSpatialInertiaInBodyFrameFromParameters;
         // Symbol: drake::multibody::RigidBody::CloneToScalar
         struct /* CloneToScalar */ {
           // Source: drake/multibody/tree/rigid_body.h
@@ -7299,6 +7304,10 @@ Parameter ``com``:
 Raises:
     RuntimeError if context is null.
 
+Raises:
+    RuntimeError if this is an ephemeral shadow link (its mass
+    properties are not independently settable).
+
 Warning:
     Do not use this function unless it is needed (think twice).)""";
         } SetCenterOfMassInBodyFrame;
@@ -7331,7 +7340,11 @@ Parameter ``center_of_mass_position``:
     the context makes sense for use by this RigidBody.
 
 Raises:
-    RuntimeError if context is null.)""";
+    RuntimeError if context is null.
+
+Raises:
+    RuntimeError if this is an ephemeral shadow link (its mass
+    properties are not independently settable).)""";
         } SetCenterOfMassInBodyFrameAndPreserveCentralInertia;
         // Symbol: drake::multibody::RigidBody::SetMass
         struct /* SetMass */ {
@@ -7354,7 +7367,11 @@ Parameter ``mass``:
     the context makes sense for use by this RigidBody.
 
 Raises:
-    RuntimeError if context is null.)""";
+    RuntimeError if context is null.
+
+Raises:
+    RuntimeError if this is an ephemeral shadow link (its mass
+    properties are not independently settable).)""";
         } SetMass;
         // Symbol: drake::multibody::RigidBody::SetSpatialInertiaInBodyFrame
         struct /* SetSpatialInertiaInBodyFrame */ {
@@ -7376,7 +7393,11 @@ Parameter ``M_Bo_B``:
     the context makes sense for use by this RigidBody.
 
 Raises:
-    RuntimeError if context is null.)""";
+    RuntimeError if context is null.
+
+Raises:
+    RuntimeError if this is an ephemeral shadow link (its mass
+    properties are not independently settable).)""";
         } SetSpatialInertiaInBodyFrame;
         // Symbol: drake::multibody::RigidBody::Unlock
         struct /* Unlock */ {

multibody/plant/BUILD.bazel

@@ -459,6 +459,17 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "auto_closed_topology_test",
+    deps = [
+        ":plant",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:expect_throws_message",
+        "//geometry:scene_graph",
+        "//multibody/parsing",
+    ],
+)
+
 drake_cc_googletest(
     name = "fused_welds_test",
     deps = [

multibody/plant/multibody_plant.cc

@@ -1168,6 +1168,10 @@ geometry::GeometryId MultibodyPlant<T>::RegisterVisualGeometry(
 template <typename T>
 const std::vector<geometry::GeometryId>&
 MultibodyPlant<T>::GetVisualGeometriesForBody(const RigidBody<T>& body) const {
+  // Check that visual_geometries_ has been sized correctly and that the
+  // body index is valid.
+  DRAKE_ASSERT(ssize(visual_geometries_) == num_bodies());
+  DRAKE_ASSERT(body.index() < num_bodies());
   return visual_geometries_[body.index()];
 }
 
@@ -1209,6 +1213,9 @@ template <typename T>
 const std::vector<geometry::GeometryId>&
 MultibodyPlant<T>::GetCollisionGeometriesForBody(
     const RigidBody<T>& body) const {
+  // Check that collision_geometries_ has been sized correctly and that the
+  // body index is valid.
+  DRAKE_ASSERT(ssize(collision_geometries_) == num_bodies());
   DRAKE_ASSERT(body.index() < num_bodies());
   return collision_geometries_[body.index()];
 }
@@ -1468,6 +1475,11 @@ void MultibodyPlant<T>::SetFuseWeldedLinks(
   mutable_tree().SetFuseWeldedLinks(fuse, model_instance);
 }
 
+template <typename T>
+void MultibodyPlant<T>::SetEnableLoopTopology(bool enable) {
+  mutable_tree().SetEnableLoopTopology(enable);
+}
+
 template <typename T>
 BaseBodyJointType MultibodyPlant<T>::GetBaseBodyJointType(
     std::optional<ModelInstanceIndex> model_instance) const {
@@ -1480,6 +1492,11 @@ bool MultibodyPlant<T>::GetFuseWeldedLinks(
   return internal_tree().GetFuseWeldedLinks(model_instance);
 }
 
+template <typename T>
+bool MultibodyPlant<T>::GetEnableLoopTopology() const {
+  return internal_tree().GetEnableLoopTopology();
+}
+
 template <typename T>
 void MultibodyPlant<T>::DeclareMiscContinuousStates() {
   DRAKE_DEMAND(!is_discrete());
@@ -1501,6 +1518,18 @@ void MultibodyPlant<T>::Finalize() {
   // After finalizing the base class, the tree is read-only.
   internal::MultibodyTreeSystem<T>::Finalize();
 
+  // At Finalize(), multibody tree may create shadow links (when loop
+  // topology is allowed), which don't come through AddRigidBody() and so have
+  // no entries in the per-body geometry arrays yet. A shadow never carries any
+  // geometry of its own -- it's an internal modeling artifact that coincides
+  // with its primary link -- but these arrays are indexed by BodyIndex and so
+  // must stay dense over num_bodies(); see GetVisualGeometriesForBody(). Note
+  // that shadows deliberately get no SceneGraph frame: body_index_to_frame_id_
+  // is map-keyed and is documented to tolerate bodies with no frame.
+  // TODO(sherm1) Give shadows a SceneGraph frame for visualization purposes.
+  visual_geometries_.resize(num_bodies());
+  collision_geometries_.resize(num_bodies());
+
   if (geometry_source_is_registered()) {
     ApplyDefaultCollisionFilters();
   }

multibody/plant/multibody_plant.h

@@ -1823,6 +1823,28 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
   void SetFuseWeldedLinks(
       bool fuse, std::optional<ModelInstanceIndex> model_instance = {});
 
+  /// (Internal use only for now) For systems whose links and joints
+  /// form one or more kinematic loops (a "closed topology"), controls
+  /// whether Finalize() should deal with those automatically. The default
+  /// setting is _not_ to deal with kinematic loops (if one is encountered,
+  /// an exception is thrown).
+  ///
+  /// @note This feature is in development and is not yet functional.
+  ///
+  /// To deal with loops automatically requires modifying the system's topology
+  /// so that it is structured as a tree of links and joints, plus constraints
+  /// needed to enforce loop closure. Breaking a loop is done by splitting
+  /// a link within that loop. Mass properties are divided between the original
+  /// ("primary") link and the new ("shadow") link. Then a weld constraint is
+  /// added between the primary and shadow links to enforce loop closure. When
+  /// the weld constraint is satisfied, the original physics is restored.
+  ///
+  /// @param[in] enable Whether Finalize() should automatically model closed
+  ///   kinematic loops rather than throwing.
+  /// @throws std::exception if called after Finalize().
+  /// @see GetEnableLoopTopology(), Finalize()
+  void SetEnableLoopTopology(bool enable);
+
   /// Returns the currently-set choice for base body joint type, either for
   /// the global setting or for a specific model instance if provided.
   /// If a model instance is provided for which no explicit choice has been
@@ -1847,6 +1869,13 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
   bool GetFuseWeldedLinks(
       std::optional<ModelInstanceIndex> model_instance = {}) const;
 
+  /// (Internal use only for now) Returns the current setting for whether
+  /// Finalize() automatically deals with closed-topology (looped) systems.
+  ///
+  /// @note This function can be called pre-Finalize() or post-Finalize().
+  /// @see SetEnableLoopTopology(), Finalize()
+  bool GetEnableLoopTopology() const;
+
   /// This method must be called after all elements in the model (joints,
   /// bodies, force elements, constraints, etc.) are added and before any
   /// computations are performed.