From 00789154b7b0c698c9db5c73f142b559dbaa0b2f Mon Sep 17 00:00:00 2001
From: "Marco A. Gutierrez" <marco@openrobotics.org>
Date: Sat, 13 May 2023 01:17:50 +0800
Subject: [PATCH] Add test showing failure for deeply nested welds in model
 directives (#19090)

Change one usage of `DRAKE_DEMAND` to `DRAKE_THROW_UNLESS`
---
 multibody/parsing/detail_dmd_parser.cc        |  2 +-
 .../test/process_model_directives_test.cc     | 27 +++++++++++++++++++
 .../deep_child_frame_weld.dmd.yaml            | 19 +++++++++++++
 .../deep_child_weld.dmd.yaml                  | 13 +++++++++
 .../model_with_directly_nested_models.sdf     | 15 +++++++++++
 5 files changed, 75 insertions(+), 1 deletion(-)
 create mode 100644 multibody/parsing/test/process_model_directives_test/deep_child_frame_weld.dmd.yaml
 create mode 100644 multibody/parsing/test/process_model_directives_test/deep_child_weld.dmd.yaml
 create mode 100644 multibody/parsing/test/process_model_directives_test/model_with_directly_nested_models.sdf

diff --git a/multibody/parsing/detail_dmd_parser.cc b/multibody/parsing/detail_dmd_parser.cc
index 65f4bd00e69d..4e35a765688f 100644
--- a/multibody/parsing/detail_dmd_parser.cc
+++ b/multibody/parsing/detail_dmd_parser.cc
@@ -41,7 +41,7 @@ void AddWeld(
         info.child_frame_name = child_frame.name();
       }
     }
-    DRAKE_DEMAND(found);
+    DRAKE_THROW_UNLESS(found);
   }
 }
 
diff --git a/multibody/parsing/test/process_model_directives_test.cc b/multibody/parsing/test/process_model_directives_test.cc
index fc2de0ee9d54..1d38792cb8bf 100644
--- a/multibody/parsing/test/process_model_directives_test.cc
+++ b/multibody/parsing/test/process_model_directives_test.cc
@@ -371,6 +371,33 @@ GTEST_TEST(ProcessModelDirectivesTest, ErrorMessages) {
   }
 }
 
+// Test model directives failure to load welds with a deep nested child.
+GTEST_TEST(ProcessModelDirectivesTest, DeepNestedChildWelds) {
+  ModelDirectives directives = LoadModelDirectives(
+      FindResourceOrThrow(std::string(kTestDir) +
+                          "/deep_child_weld.dmd.yaml"));
+  MultibodyPlant<double> plant(0.0);
+
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      ProcessModelDirectives(directives, &plant, nullptr,
+        make_parser(&plant).get()),
+        R"(.*Failure at .* in AddWeld\(\): condition 'found' failed.*)");
+}
+
+// Test model directives failure to load welds with a child to a
+// deep nested frame.
+GTEST_TEST(ProcessModelDirectivesTest, DeepNestedChildFrameWelds) {
+  ModelDirectives directives = LoadModelDirectives(
+      FindResourceOrThrow(std::string(kTestDir) +
+                          "/deep_child_frame_weld.dmd.yaml"));
+  MultibodyPlant<double> plant(0.0);
+
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      ProcessModelDirectives(directives, &plant, nullptr,
+        make_parser(&plant).get()),
+        R"(.*Failure at .* in AddWeld\(\): condition 'found' failed.*)");
+}
+
 }  // namespace
 }  // namespace parsing
 }  // namespace multibody
diff --git a/multibody/parsing/test/process_model_directives_test/deep_child_frame_weld.dmd.yaml b/multibody/parsing/test/process_model_directives_test/deep_child_frame_weld.dmd.yaml
new file mode 100644
index 000000000000..8640b8cdfc6e
--- /dev/null
+++ b/multibody/parsing/test/process_model_directives_test/deep_child_frame_weld.dmd.yaml
@@ -0,0 +1,19 @@
+directives:
+
+- add_model:
+    name: top_level_model
+    file: package://process_model_directives_test/model_with_directly_nested_models.sdf
+
+- add_model:
+    name: simple_model
+    file: package://process_model_directives_test/simple_model.sdf
+
+- add_frame:
+    name: top_injected_frame
+    X_PF:
+      base_frame: top_level_model::robot1::robot_base
+      translation: [1, 2, 3]
+
+- add_weld:
+    parent: simple_model::base
+    child: top_injected_frame
diff --git a/multibody/parsing/test/process_model_directives_test/deep_child_weld.dmd.yaml b/multibody/parsing/test/process_model_directives_test/deep_child_weld.dmd.yaml
new file mode 100644
index 000000000000..031657f0aa2d
--- /dev/null
+++ b/multibody/parsing/test/process_model_directives_test/deep_child_weld.dmd.yaml
@@ -0,0 +1,13 @@
+directives:
+
+- add_model:
+    name: top_level_model
+    file: package://process_model_directives_test/model_with_directly_nested_models.sdf
+
+- add_model:
+    name: simple_model
+    file: package://process_model_directives_test/simple_model.sdf
+
+- add_weld:
+    parent: simple_model::base
+    child: top_level_model::robot1::robot_base
diff --git a/multibody/parsing/test/process_model_directives_test/model_with_directly_nested_models.sdf b/multibody/parsing/test/process_model_directives_test/model_with_directly_nested_models.sdf
new file mode 100644
index 000000000000..e0f390275bd2
--- /dev/null
+++ b/multibody/parsing/test/process_model_directives_test/model_with_directly_nested_models.sdf
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<sdf xmlns:drake="http://drake.mit.edu" version="1.7">
+  <model name="scoped_model">
+    <link name="base"/>
+    <model name="robot1">
+      <link name="robot_base"/>
+      <frame name="tool_frame">
+        <pose relative_to="robot_base">1 2 3  0 0 0</pose>
+      </frame>
+    </model>
+    <frame name="frame">
+      <pose relative_to="base">1 2 3  0 0 0</pose>
+    </frame>
+  </model>
+</sdf>