diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index b3d22f1f2b2f..0356eae240b0 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -83,6 +83,44 @@ std::pair GetResolvedModelInstanceAndLocalName( return {resolved_model_instance, unscoped_local_name}; } +// Calculates the relative 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 +// relative name of the nested model. +std::string GetRelativeBodyName( + const Body& body, + ModelInstanceIndex relative_to_model_instance, + const MultibodyPlant& 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. + DRAKE_DEMAND(0 == nested_model_absolute_name.compare( + 0, relative_to_model_absolute_name.size(), + relative_to_model_absolute_name)); + + DRAKE_DEMAND(nested_model_absolute_name.size() > + relative_to_model_absolute_name.size() + + sdf::kSdfScopeDelimiter.size()); + + const std::string nested_model_relative_name = + nested_model_absolute_name.substr( + relative_to_model_absolute_name.size() + + sdf::kSdfScopeDelimiter.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, // expressed in the inertial frame Bi (as specified in in the SDF @@ -415,8 +453,9 @@ void AddJointFromSpecification( // Get the pose of frame J in the frame of the child link C, as specified in // ... . 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 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 diff --git a/multibody/parsing/test/detail_sdf_parser_test.cc b/multibody/parsing/test/detail_sdf_parser_test.cc index d777e146eb06..21b4b21c42cc 100644 --- a/multibody/parsing/test/detail_sdf_parser_test.cc +++ b/multibody/parsing/test/detail_sdf_parser_test.cc @@ -1803,6 +1803,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 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(); + + // Frames attached to links in the same model + { + const auto& joint_J1 = plant.GetJointByName("J1"); + + // J1p and J1c are the frames of J1 on its parent and child links + // respectively. The absolute poses of both frames should be identical. + const RigidTransformd X_WJ1_expected(RollPitchYawd(0, 0, 0), + Vector3d(4, 5, 6)); + + const RigidTransformd X_WJ1p = + joint_J1.frame_on_parent().CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WJ1_expected.GetAsMatrix4(), + X_WJ1p.GetAsMatrix4(), kEps)); + + const RigidTransformd X_WJ1c = + joint_J1.frame_on_child().CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WJ1_expected.GetAsMatrix4(), + X_WJ1c.GetAsMatrix4(), kEps)); + } + // Frames attached to links in the other (nested) models + { + const auto& joint_J2 = plant.GetJointByName("J2"); + + // J2p and J2c are the frames of J2 on its parent and child links + // respectively. The absolute poses of both frames should be identical. + const RigidTransformd X_WJ2_expected(RollPitchYawd(0, 0, 0), + Vector3d(4, 5, 6)); + const RigidTransformd X_WJ2p = + joint_J2.frame_on_parent().CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WJ2_expected.GetAsMatrix4(), + X_WJ2p.GetAsMatrix4(), kEps)); + + const RigidTransformd X_WJ2c = + joint_J2.frame_on_child().CalcPoseInWorld(*context); + EXPECT_TRUE(CompareMatrices(X_WJ2_expected.GetAsMatrix4(), + X_WJ2c.GetAsMatrix4(), kEps)); + } +} } // namespace } // namespace internal } // namespace multibody diff --git a/multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf b/multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf new file mode 100644 index 000000000000..d337804d5e0b --- /dev/null +++ b/multibody/parsing/test/sdf_parser_test/frames_as_joint_parent_or_child.sdf @@ -0,0 +1,40 @@ + + + + + + + + + 1 2 3 0 0 0 + + + 4 5 6 0 0 0 + + + + L1_offset + L2_offset + + + + + + + + + + + + 1 2 3 0 0 0 + + + 4 5 6 0 0 0 + + + + M1_base_link_offset + M2_base_link_offset + + +