parsing: Fix joint pose computation when using frames as joint parent…

… or child (#15290)

multibody/parsing/detail_sdf_parser.cc

@@ -82,6 +82,44 @@ std::pair<ModelInstanceIndex, std::string> GetResolvedModelInstanceAndLocalName(
 
   return {resolved_model_instance, unscoped_local_name};
 }
+// Returns true if `str` starts with `prefix`. The length of `prefix` has to be
+// strictly less than the size of `str`.
+bool StartsWith(const std::string_view str, const std::string_view prefix) {
+  return prefix.size() < str.size() &&
+         std::equal(str.begin(), str.begin() + prefix.size(), prefix.begin());
+}
+
+// Calculates the scoped name of a body relative to the model instance @p
+// relative_to_model_instance. If the body is a direct child of the model,
+// this simply returns the local name of the body. However, if the body is
+// a child of a nested model, the local name of the body is prefixed with the
+// scoped name of the nested model.
+std::string GetRelativeBodyName(
+    const Body<double>& body,
+    ModelInstanceIndex relative_to_model_instance,
+    const MultibodyPlant<double>& plant) {
+  const std::string& relative_to_model_absolute_name =
+      plant.GetModelInstanceName(relative_to_model_instance);
+  // If the body is inside a nested model, we need to prefix the
+  // name with the relative name of the nested model.
+  if (body.model_instance() != relative_to_model_instance) {
+    const std::string& nested_model_absolute_name =
+        plant.GetModelInstanceName(body.model_instance());
+    // The relative_to_model_absolute_name must be a prefix of the
+    // nested_model_absolute_name. Otherwise the nested model is not a
+    // descendent of the model relative to which we are computing the name.
+    const std::string required_prefix =
+        relative_to_model_absolute_name + sdf::kSdfScopeDelimiter;
+    DRAKE_DEMAND(StartsWith(nested_model_absolute_name, required_prefix));
+
+    const std::string nested_model_relative_name =
+        nested_model_absolute_name.substr(required_prefix.size());
+
+    return sdf::JoinName(nested_model_relative_name, body.name());
+  } else {
+    return body.name();
+  }
+}
 
 // Given an ignition::math::Inertial object, extract a RotationalInertia object
 // for the rotational inertia of body B, about its center of mass Bcm and,
@@ -415,8 +453,9 @@ void AddJointFromSpecification(
   // Get the pose of frame J in the frame of the child link C, as specified in
   // <joint> <pose> ... </pose></joint>. The default `relative_to` pose of a
   // joint will be the child link.
-  const RigidTransformd X_CJ =
-      ResolveRigidTransform(joint_spec.SemanticPose());
+  const RigidTransformd X_CJ = ResolveRigidTransform(
+      joint_spec.SemanticPose(),
+      GetRelativeBodyName(child_body, model_instance, *plant));
 
   // Pose of the frame J in the parent body frame P.
   std::optional<RigidTransformd> X_PJ;
@@ -429,7 +468,8 @@ void AddJointFromSpecification(
     X_PJ = X_WM * X_MJ;  // Since P == W.
   } else {
     X_PJ = ResolveRigidTransform(
-        joint_spec.SemanticPose(), joint_spec.ParentLinkName());
+        joint_spec.SemanticPose(),
+        GetRelativeBodyName(parent_body, model_instance, *plant));
   }
 
   // If P and J are coincident, we won't create a new frame for J, but use frame

multibody/parsing/test/detail_sdf_parser_test.cc

@@ -1862,6 +1862,60 @@ GTEST_TEST(SdfParser, SupportNonDefaultCanonicalLink) {
   EXPECT_EQ(GetModelFrameByName(*plant, "a::c").body().index(),
             plant->GetBodyByName("f").index());
 }
+
+// Verify that frames can be used for //joint/parent and //joint/child
+GTEST_TEST(SdfParser, FramesAsJointParentOrChild) {
+  const std::string full_name = FindResourceOrThrow(
+      "drake/multibody/parsing/test/sdf_parser_test/"
+      "frames_as_joint_parent_or_child.sdf");
+  MultibodyPlant<double> plant(0.0);
+
+  PackageMap package_map;
+  package_map.PopulateUpstreamToDrake(full_name);
+  AddModelsFromSdfFile(full_name, package_map, &plant);
+  ASSERT_TRUE(plant.HasModelInstanceNamed("parent_model"));
+
+  plant.Finalize();
+  auto context = plant.CreateDefaultContext();
+
+  const RigidTransformd X_CJc_expected = RigidTransformd::Identity();
+  const RigidTransformd X_PJp_expected(RollPitchYawd(0, 0, 0),
+                                       Vector3d(3, 3, 3));
+
+  // Frames attached to links in the same model
+  {
+    const auto& joint = plant.GetJointByName("J1");
+    const auto& frame_P = plant.GetFrameByName("L1_offset");
+    const auto& frame_C = plant.GetFrameByName("L2_offset");
+
+    const RigidTransformd X_PJp =
+        joint.frame_on_parent().CalcPose(*context, frame_P);
+    EXPECT_TRUE(CompareMatrices(X_PJp_expected.GetAsMatrix4(),
+                                X_PJp.GetAsMatrix4(), kEps));
+
+    const RigidTransformd X_CJc =
+        joint.frame_on_child().CalcPose(*context, frame_C);
+    EXPECT_TRUE(CompareMatrices(X_CJc_expected.GetAsMatrix4(),
+                                X_CJc.GetAsMatrix4(), kEps));
+  }
+  // Frames attached to links in the other (nested) models
+  {
+    const auto& joint = plant.GetJointByName("J2");
+
+    const auto& frame_P = plant.GetFrameByName("M1_base_link_offset");
+    const auto& frame_C = plant.GetFrameByName("M2_base_link_offset");
+
+    const RigidTransformd X_PJp =
+        joint.frame_on_parent().CalcPose(*context, frame_P);
+    EXPECT_TRUE(CompareMatrices(X_PJp_expected.GetAsMatrix4(),
+                                X_PJp.GetAsMatrix4(), kEps));
+
+    const RigidTransformd X_CJc =
+        joint.frame_on_child().CalcPose(*context, frame_C);
+    EXPECT_TRUE(CompareMatrices(X_CJc_expected.GetAsMatrix4(),
+                                X_CJc.GetAsMatrix4(), kEps));
+  }
+}
 }  // namespace
 }  // namespace internal
 }  // namespace multibody

multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<sdf version="1.8">
+  <model name="parent_model">
+    <!-- Testing frames attached to links in the same model -->
+    <link name="L1"/>
+    <link name="L2"/>
+
+    <frame name="L1_offset" attached_to="L1">
+      <pose>1 2 3   0 0 0</pose>
+    </frame>
+    <frame name="L2_offset" attached_to="L2">
+      <pose>4 5 6   0 0 0</pose>
+    </frame>
+
+    <joint name="J1" type="fixed">
+      <parent>L1_offset</parent>
+      <child>L2_offset</child>
+    </joint>
+
+    <!-- Testing frames attached to links in the other (nested) models -->
+    <model name="M1">
+      <link name="base_link"/>
+    </model>
+    <model name="M2">
+      <link name="base_link"/>
+    </model>
+
+    <frame name="M1_base_link_offset" attached_to="M1::base_link">
+      <pose>1 2 3   0 0 0</pose>
+    </frame>
+    <frame name="M2_base_link_offset" attached_to="M2::base_link">
+      <pose>4 5 6   0 0 0</pose>
+    </frame>
+
+    <joint name="J2" type="fixed">
+      <parent>M1_base_link_offset</parent>
+      <child>M2_base_link_offset</child>
+    </joint>
+  </model>
+</sdf>