[multibody] Auto loop breaking: add weld constraints and shadow link visualization

bindings/generated_docstrings/multibody_plant.h

@@ -8236,7 +8236,10 @@ calls will always return the same value.)""";
         struct /* num_constraints */ {
           // Source: drake/multibody/plant/multibody_plant.h
           const char* doc =
-R"""(Returns the total number of constraints specified by the user.)""";
+R"""(Returns the total number of constraints in this model. Prior to
+Finalize() these are just the constraints specified by the user.
+Finalize() may add "ephemeral" constraints of its own; see
+num_loop_constraints().)""";
         } num_constraints;
         // Symbol: drake::multibody::MultibodyPlant::num_coupler_constraints
         struct /* num_coupler_constraints */ {
@@ -8277,6 +8280,17 @@ R"""(Returns the number of joints in the model.
 See also:
     AddJoint().)""";
         } num_joints;
+        // Symbol: drake::multibody::MultibodyPlant::num_loop_constraints
+        struct /* num_loop_constraints */ {
+          // Source: drake/multibody/plant/multibody_plant.h
+          const char* doc =
+R"""(Returns the number of ephemeral weld constraints that Finalize() added
+in order to close topological loops. Each of these welds a shadow link
+to the link it is a copy of; see SetEnableLoopTopology(). These are
+included in num_constraints() and num_weld_constraints(), and are
+indistinguishable from user-added welds to the constraint solvers.
+Returns zero prior to Finalize().)""";
+        } num_loop_constraints;
         // Symbol: drake::multibody::MultibodyPlant::num_misc_continuous_states
         struct /* num_misc_continuous_states */ {
           // Source: drake/multibody/plant/multibody_plant.h
@@ -8366,7 +8380,10 @@ calls will always return the same value.)""";
         struct /* num_weld_constraints */ {
           // Source: drake/multibody/plant/multibody_plant.h
           const char* doc =
-R"""(Returns the total number of weld constraints specified by the user.)""";
+R"""(Returns the total number of weld constraints. Before Finalize() these
+are just the weld constraints specified by the user. Finalize() may
+add "ephemeral" weld constraints of its own; see
+num_loop_constraints().)""";
         } num_weld_constraints;
         // Symbol: drake::multibody::MultibodyPlant::physical_models
         struct /* physical_models */ {

bindings/generated_docstrings/multibody_tree.h

@@ -7750,6 +7750,14 @@ base body*, meaning it had no explicit joint to a parent body and is
 mobilized by an automatically-added (ephemeral) floating (6 dof) joint
 to World.
 
+Note:
+    A Link can be a floating base body even though a user-defined
+    joint names it as a child link, if that joint does not connect it
+    to World: when a closed kinematic loop has no joint to World at
+    all, one of the loop's links must still serve as a base body. What
+    this flag reports is that the Link is mobilized by an ephemeral
+    floating joint, not that the user left it jointless.
+
 Note:
     A floating base body is not necessarily modeled with a quaternion
     mobilizer, see has_quaternion_dofs(). Alternative options include

bindings/pydrake/multibody/plant_py.cc

@@ -220,6 +220,8 @@ void DoScalarDependentDefinitions(py::module_ m, T) {
             cls_doc.num_force_elements.doc)
         .def("num_constraints", &Class::num_constraints,
             cls_doc.num_constraints.doc)
+        .def("num_loop_constraints", &Class::num_loop_constraints,
+            cls_doc.num_loop_constraints.doc)
         .def("num_model_instances", &Class::num_model_instances,
             cls_doc.num_model_instances.doc)
         .def("num_positions",
@@ -257,6 +259,10 @@ void DoScalarDependentDefinitions(py::module_ m, T) {
         .def("SetUseSampledOutputPorts", &Class::SetUseSampledOutputPorts,
             py::arg("use_sampled_output_ports"),
             cls_doc.SetUseSampledOutputPorts.doc)
+        .def("SetEnableLoopTopology", &Class::SetEnableLoopTopology,
+            py::arg("enable"), cls_doc.SetEnableLoopTopology.doc)
+        .def("GetEnableLoopTopology", &Class::GetEnableLoopTopology,
+            cls_doc.GetEnableLoopTopology.doc)
         .def(
             "AddJoint",
             [](Class* self, const Joint<T>& joint) {

bindings/pydrake/multibody/test/plant_test.py

@@ -199,6 +199,11 @@ def test_multibody_plant_construction_api(self, T):
         plant.SetUseSampledOutputPorts(use_sampled_output_ports=False)
         self.assertEqual(plant.has_sampled_output_ports(), False)
         self.assertEqual(plant.time_step(), 0.0)
+        self.assertEqual(plant.GetEnableLoopTopology(), False)
+        plant.SetEnableLoopTopology(enable=True)
+        self.assertEqual(plant.GetEnableLoopTopology(), True)
+        plant.SetEnableLoopTopology(enable=False)
+        self.assertEqual(plant.num_loop_constraints(), 0)
         spatial_inertia = SpatialInertia.NaN()
         body = plant.AddRigidBody(name="new_body", M_BBo_B=spatial_inertia)
         body.default_mass()

multibody/plant/constraint_specs.h

@@ -82,6 +82,14 @@ struct WeldConstraintSpec {
   BodyIndex body_B;                   // Index of body B.
   math::RigidTransform<double> X_BQ;  // Pose of frame Q in B's body frame.
   MultibodyConstraintId id;           // Id of this constraint in the plant.
+
+  // Whether this constraint was added by MultibodyPlant::Finalize() rather than
+  // by the user; currently that means it is closing a topological loop that was
+  // broken by splitting a link into a primary and a shadow. Analogous to
+  // MultibodyElement::is_ephemeral().
+  // TODO(sherm1) When the constraint specs acquire a common base class (see
+  //  #21415), move this flag and `id` there.
+  bool is_ephemeral{false};
 };
 
 // Struct to store the specification for a fixed constraint between vertices of

multibody/plant/multibody_plant.cc

@@ -1519,17 +1519,30 @@ void MultibodyPlant<T>::Finalize() {
   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.
+  // topology is allowed). Those don't come through AddRigidBody() so have
+  // no entries in the per-body geometry arrays yet. Add additional entries
+  // for them now to keep these arrays properly sized.
   visual_geometries_.resize(num_bodies());
   collision_geometries_.resize(num_bodies());
 
+  // Next, register the shadow links with SceneGraph so that we can visualize
+  // them. This enables Meshcat to visualize the shadow link inertia ellipsoids,
+  // and can be used to add visual geometry to the shadow links if desired
+  // (typically for pedagogical or debugging purposes -- shadows links are
+  // normally invisible).
+  for (BodyIndex index(0); index < num_bodies(); ++index) {
+    const RigidBody<T>& body = get_body(index);
+    if (body.is_ephemeral()) {
+      RegisterRigidBodyWithSceneGraph(body);  // A no-op if no SceneGraph.
+    }
+  }
+
+  // Add the weld constraints that reconnect primary links to their shadow links
+  // if we had to break topological loops. This must happen before
+  // FinalizePlantOnly() below, so we have a chance to declare the parameters
+  // that determine constraint active/deactivated status.
+  AddEphemeralLoopConstraints();
+
   if (geometry_source_is_registered()) {
     ApplyDefaultCollisionFilters();
   }
@@ -1637,6 +1650,31 @@ void MultibodyPlant<T>::SetUpJointLimitsParameters() {
   }
 }
 
+template <typename T>
+void MultibodyPlant<T>::AddEphemeralLoopConstraints() {
+  // A shadow link's link frame is coincident with its primary's by
+  // construction, so the constrained frames P and Q are just the two body
+  // frames and both offsets are the identity. The graph documents that the
+  // primary link is always the parent, which sets the sign convention for the
+  // constraint multipliers.
+  // N.B. We add the spec directly rather than calling AddWeldConstraint(),
+  // which is a pre-finalize-only API.
+  for (const internal::LinkJointGraph::LoopConstraint& loop_constraint :
+       internal_tree().graph().loop_constraints()) {
+    const MultibodyConstraintId id = MultibodyConstraintId::get_new_id();
+    internal::WeldConstraintSpec spec{
+        .body_A = loop_constraint.primary_link(),
+        .X_AP = math::RigidTransform<double>::Identity(),
+        .body_B = loop_constraint.shadow_link(),
+        .X_BQ = math::RigidTransform<double>::Identity(),
+        .id = id,
+        .is_ephemeral = true};
+    // A shadow link is by definition distinct from its primary.
+    DRAKE_DEMAND(spec.IsValid());
+    weld_constraints_specs_[id] = spec;
+  }
+}
+
 template <typename T>
 void MultibodyPlant<T>::FinalizeConstraints() {
   for (auto& [constraint_id, spec] : ball_constraints_specs_) {
@@ -3467,12 +3505,32 @@ void MultibodyPlant<T>::ThrowIfUnsupportedContinuousTimeDynamics(
   // TODO(#23759,#23760,#23762,#23763,#23992): revisit this check and error
   // message as constraints are implemented for CENIC.
   if (num_constraints() > 0) {
-    throw std::logic_error(
+    // Loop constraints are not the user's doing -- Finalize() added them to
+    // close the kinematic loops in the model -- so say where they came from
+    // rather than leaving the user hunting for constraints they never added.
+    const int num_loops = num_loop_constraints();
+    const char* s = num_constraints() == 1 ? "" : "s";
+    std::string constraints;
+    if (num_loops == 0) {
+      constraints = fmt::format("{} constraint{}", num_constraints(), s);
+    } else if (num_loops == num_constraints()) {
+      constraints = fmt::format(
+          "{} constraint{}, which Finalize() added in order to close the "
+          "kinematic loops in this model (see SetEnableLoopTopology())",
+          num_loops, s);
+    } else {
+      constraints = fmt::format(
+          "{} constraint{}, {} of which Finalize() added in order to close the "
+          "kinematic loops in this model (see SetEnableLoopTopology())",
+          num_constraints(), s, num_loops);
+    }
+    throw std::logic_error(fmt::format(
         "Currently this MultibodyPlant is set to use continuous time. "
-        "Continuous time does not support constraints. Use a discrete time "
-        "model and set_discrete_contact_approximation() to set a model "
-        "approximation that uses the SAP solver instead (kSap, kSimilar, or "
-        "kLagged).");
+        "Continuous time does not support constraints, but this model has {}. "
+        "Use a discrete time model and set_discrete_contact_approximation() to "
+        "set a model approximation that uses the SAP solver instead (kSap, "
+        "kSimilar, or kLagged).",
+        constraints));
   }
 
   // TODO(#24061): consider rejecting models with joint limits here, once CENIC

multibody/plant/multibody_plant.h

@@ -1937,13 +1937,26 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
   /// MultibodyConstraintManager class to consolidate constraint management. -->
   /// @{
 
-  /// Returns the total number of constraints specified by the user.
+  /// Returns the total number of constraints in this model. Prior to
+  /// Finalize() these are just the constraints specified by the user.
+  /// Finalize() may add "ephemeral" constraints of its own; see
+  /// num_loop_constraints().
   int num_constraints() const {
     return num_coupler_constraints() + num_distance_constraints() +
            num_ball_constraints() + num_weld_constraints() +
            num_tendon_constraints();
   }
 
+  /// Returns the number of ephemeral weld constraints that Finalize() added in
+  /// order to close topological loops. Each of these welds a shadow link to the
+  /// link it is a copy of; see SetEnableLoopTopology(). These are included in
+  /// num_constraints() and num_weld_constraints(), and are indistinguishable
+  /// from user-added welds to the constraint solvers. Returns zero prior to
+  /// Finalize().
+  int num_loop_constraints() const {
+    return internal_tree().graph().num_loop_constraints();
+  }
+
   /// Returns a list of all constraint identifiers. The returned vector becomes
   /// invalid after any calls to Add*Constraint() or RemoveConstraint().
   std::vector<MultibodyConstraintId> GetConstraintIds() const;
@@ -1961,7 +1974,9 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
   /// Returns the total number of ball constraints specified by the user.
   int num_ball_constraints() const { return ssize(ball_constraints_specs_); }
 
-  /// Returns the total number of weld constraints specified by the user.
+  /// Returns the total number of weld constraints. Before Finalize() these are
+  /// just the weld constraints specified by the user. Finalize() may add
+  /// "ephemeral" weld constraints of its own; see num_loop_constraints().
   int num_weld_constraints() const { return ssize(weld_constraints_specs_); }
 
   /// Returns the total number of tendon constraints specified by the
@@ -6291,6 +6306,13 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
   // that still guarantees stability.
   void SetUpJointLimitsParameters();
 
+  // Adds an ephemeral weld constraint for each loop constraint the modeler
+  // introduced when it broke a closed kinematic loop by splitting a link into
+  // a primary link and a shadow link. Called during Finalize(), after the tree
+  // (and hence the shadow links) has been finalized, but before
+  // FinalizePlantOnly() declares the constraint parameters.
+  void AddEphemeralLoopConstraints();
+
   // Some constraints support std::optional specs, which implies that the
   // kinematics should be used to compute values such that the constraint is
   // satisfied by the default context at the moment Finalize() is called. This