multibody: Give better errors when joints overlap with SDF static wel…

…ds (RobotLocomotion#14534)

Closes RobotLocomotion#14518.

Indicate SDF-inserted welds by using an elaborate joint name. Since this
defeats the fail-fast error induced by having duplicate joint names (for
redundant welds), implement a more robust fail-fast error that complains
about any two joints that connect the same two bodies, regardless of
direction or other details.

multibody/parsing/detail_sdf_parser.cc

@@ -790,12 +790,17 @@ ModelInstanceIndex AddModelFromSpecification(
     }
     // Weld all links that have been added, but are not (yet) attached to the
     // world.
-    // N.B. This implementation prevents "reposturing" a static model after
-    // parsing. See #12227 for a more concrete example.
+    // N.B. This implementation complicates "reposturing" a static model after
+    // parsing. See #12227 and #14518 for more discussion.
     for (const LinkInfo& link_info : added_link_infos) {
       if (!AreWelded(*plant, plant->world_body(), *link_info.body)) {
-        plant->WeldFrames(
-            plant->world_frame(), link_info.body->body_frame(), link_info.X_WL);
+        const auto& A = plant->world_frame();
+        const auto& B = link_info.body->body_frame();
+        const std::string joint_name =
+            "sdformat_model_static_" + A.name() + "_welds_to_" + B.name();
+        plant->AddJoint(
+            std::make_unique<WeldJoint<double>>(
+                joint_name, A, B, link_info.X_WL));
       }
     }
   }

multibody/parsing/test/detail_sdf_parser_test.cc

@@ -514,7 +514,10 @@ GTEST_TEST(SdfParser, StaticModelWithJoints) {
         pair.plant->world_frame(), pair.plant->GetFrameByName("a"));
     pair.plant->Finalize();
   };
-  EXPECT_THROW(weld_and_finalize(), std::runtime_error);
+  // The message contains the elaborate joint name inserted by the parser.
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      weld_and_finalize(), std::runtime_error,
+      ".*sdformat_model_static.*");
 
   // Drake does not support "frozen" joints (#12227).
   DRAKE_EXPECT_THROWS_MESSAGE(

multibody/topology/BUILD.bazel

@@ -22,6 +22,7 @@ drake_cc_library(
         "multibody_graph.h",
     ],
     deps = [
+        "//common:sorted_pair",
         "//multibody/tree:multibody_tree_indexes",
     ],
 )

multibody/topology/multibody_graph.cc

@@ -125,6 +125,24 @@ JointIndex MultibodyGraph::AddJoint(const std::string& name,
 
   // next available index.
   const JointIndex joint_index(num_joints());
+  auto [map_iter, inserted] = bodies_to_joint_.insert(
+      {{parent_body_index, child_body_index}, joint_index});
+  if (!inserted) {
+    auto existing_joint_index = map_iter->second;
+    const auto& existing_joint = get_joint(existing_joint_index);
+    const auto& existing_parent = get_body(existing_joint.parent_body());
+    const auto& existing_child = get_body(existing_joint.child_body());
+    const auto& new_parent = get_body(parent_body_index);
+    const auto& new_child = get_body(child_body_index);
+    throw std::runtime_error(
+        "This MultibodyGraph already has a joint '" + existing_joint.name() +
+        "' connecting '" + existing_parent.name() +
+        "' to '" + existing_child.name() +
+        "'. Therefore adding joint '" + name +
+        "' connecting '" + new_parent.name() + "' to '" + new_child.name() +
+        "' is not allowed.");
+  }
+
   // provide fast name lookup.
   joint_name_to_index_.insert({name, joint_index});
 

multibody/topology/multibody_graph.h

@@ -6,6 +6,7 @@
 #include <vector>
 
 #include "drake/common/drake_copyable.h"
+#include "drake/common/sorted_pair.h"
 #include "drake/multibody/tree/multibody_tree_indexes.h"
 
 namespace drake {
@@ -207,6 +208,10 @@ class MultibodyGraph {
 
   std::unordered_map<std::string, JointTypeIndex> joint_type_name_to_index_;
 
+  // Map used to detect redundant joints.
+  using BodiesKey = SortedPair<BodyIndex>;
+  std::unordered_map<BodiesKey, JointIndex> bodies_to_joint_;
+
   // The xxx_name_to_index_ structures are multimaps because
   // bodies/joints/actuators/etc may appear with the same name in different
   // model instances. The index values are still unique across the graph.

multibody/topology/test/multibody_graph_test.cc

@@ -62,6 +62,22 @@ GTEST_TEST(MultibodyGraph, SerialChain) {
                      BodyIndex(2)),
       std::runtime_error, "AddJoint\\(\\): Duplicate joint name.*");
 
+  // We cannot add a redundant joint.
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      graph.AddJoint("other", model_instance, kRevoluteType, BodyIndex(1),
+                     BodyIndex(2)),
+      std::runtime_error,
+      "This MultibodyGraph already has a joint 'pin2' connecting 'body1'"
+      " to 'body2'. Therefore adding joint 'other' connecting 'body1' to"
+      " 'body2' is not allowed.");
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      graph.AddJoint("reverse", model_instance, kRevoluteType, BodyIndex(2),
+                     BodyIndex(1)),
+      std::runtime_error,
+      "This MultibodyGraph already has a joint 'pin2' connecting 'body1'"
+      " to 'body2'. Therefore adding joint 'reverse' connecting 'body2' to"
+      " 'body1' is not allowed.");
+
   // We cannot add an unregistered joint type.
   DRAKE_EXPECT_THROWS_MESSAGE(graph.AddJoint("screw1", model_instance, "screw",
                                              BodyIndex(1), BodyIndex(2)),