diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index 70aa957c137e..7dac3c568efa 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -35,6 +35,7 @@ #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/revolute_joint.h" #include "drake/multibody/tree/revolute_spring.h" +#include "drake/multibody/tree/screw_joint.h" #include "drake/multibody/tree/spatial_inertia.h" #include "drake/multibody/tree/uniform_gravity_field_element.h" #include "drake/multibody/tree/universal_joint.h" @@ -311,6 +312,7 @@ Vector3d ExtractJointAxis( const sdf::Joint& joint_spec) { unused(model_spec); DRAKE_DEMAND(joint_spec.Type() == sdf::JointType::REVOLUTE || + joint_spec.Type() == sdf::JointType::SCREW || joint_spec.Type() == sdf::JointType::PRISMATIC || joint_spec.Type() == sdf::JointType::CONTINUOUS); @@ -358,6 +360,7 @@ double ParseJointDamping( const sdf::Joint& joint_spec) { DRAKE_DEMAND(joint_spec.Type() == sdf::JointType::REVOLUTE || joint_spec.Type() == sdf::JointType::PRISMATIC || + joint_spec.Type() == sdf::JointType::SCREW || joint_spec.Type() == sdf::JointType::UNIVERSAL || joint_spec.Type() == sdf::JointType::BALL || joint_spec.Type() == sdf::JointType::CONTINUOUS); @@ -425,6 +428,7 @@ void AddJointActuatorFromSpecification( MultibodyPlant* plant) { DRAKE_THROW_UNLESS(plant != nullptr); DRAKE_DEMAND(joint_spec.Type() == sdf::JointType::BALL || + joint_spec.Type() == sdf::JointType::SCREW || joint_spec.Type() == sdf::JointType::UNIVERSAL || joint_spec.Type() == sdf::JointType::PRISMATIC || joint_spec.Type() == sdf::JointType::REVOLUTE || @@ -463,7 +467,7 @@ void AddJointActuatorFromSpecification( return; } - // Prismatic, revolute, and continuous joints have a single axis. + // Prismatic, screw, revolute, and continuous joints have a single axis. const double effort_limit = GetEffortLimit(diagnostic, joint_spec, 0); if (effort_limit != 0) { const JointActuator& actuator = @@ -602,7 +606,8 @@ void AddJointFromSpecification( "drake:rotor_inertia", "drake:gear_ratio", "parent", - "pose"}; + "pose", + "screw_thread_pitch"}; CheckSupportedElements(diagnostic, joint_spec.Element(), supported_joint_elements); @@ -762,10 +767,16 @@ void AddJointFromSpecification( break; } case sdf::JointType::SCREW: { - // TODO(jwnimmer-tri) Use a multibody::ScrewJoint here. - diagnostic.Error(fmt::format( - "Joint type (screw) not supported for joint '{}'.", - joint_spec.Name())); + const double damping = ParseJointDamping(diagnostic, joint_spec); + // The ScrewThreadPitch() API uses the same representation as + // Drake's ScrewJoint class (meters / revolution, right-handed). + const double screw_thread_pitch = joint_spec.ScrewThreadPitch(); + Vector3d axis_J = ExtractJointAxis(diagnostic, model_spec, joint_spec); + const auto& joint = plant->AddJoint( + joint_spec.Name(), + parent_body, X_PJ, + child_body, X_CJ, axis_J, screw_thread_pitch, damping); + AddJointActuatorFromSpecification(diagnostic, joint_spec, joint, plant); break; } case sdf::JointType::GEARBOX: { diff --git a/multibody/parsing/test/detail_sdf_parser_test.cc b/multibody/parsing/test/detail_sdf_parser_test.cc index 656dff47a30f..2d09dfaf150e 100644 --- a/multibody/parsing/test/detail_sdf_parser_test.cc +++ b/multibody/parsing/test/detail_sdf_parser_test.cc @@ -32,6 +32,7 @@ #include "drake/multibody/tree/revolute_joint.h" #include "drake/multibody/tree/revolute_spring.h" #include "drake/multibody/tree/rigid_body.h" +#include "drake/multibody/tree/screw_joint.h" #include "drake/multibody/tree/universal_joint.h" #include "drake/systems/framework/context.h" @@ -986,6 +987,28 @@ TEST_F(SdfParserTest, JointParsingTest) { CompareMatrices(continuous_joint.acceleration_lower_limits(), neg_inf)); EXPECT_TRUE( CompareMatrices(continuous_joint.acceleration_upper_limits(), inf)); + + // Screw joint + DRAKE_EXPECT_NO_THROW( + plant_.GetJointByName("screw_joint", instance1)); + const ScrewJoint& screw_joint = + plant_.GetJointByName("screw_joint", instance1); + EXPECT_EQ(screw_joint.name(), "screw_joint"); + EXPECT_EQ(screw_joint.parent_body().name(), "link8"); + EXPECT_EQ(screw_joint.child_body().name(), "link9"); + EXPECT_EQ(screw_joint.screw_axis(), Vector3d::UnitX()); + EXPECT_EQ(screw_joint.screw_pitch(), 0.04); + EXPECT_EQ(screw_joint.damping(), 0.1); + EXPECT_TRUE( + CompareMatrices(screw_joint.position_lower_limits(), neg_inf)); + EXPECT_TRUE(CompareMatrices(screw_joint.position_upper_limits(), inf)); + EXPECT_TRUE( + CompareMatrices(screw_joint.velocity_lower_limits(), neg_inf)); + EXPECT_TRUE(CompareMatrices(screw_joint.velocity_upper_limits(), inf)); + EXPECT_TRUE( + CompareMatrices(screw_joint.acceleration_lower_limits(), neg_inf)); + EXPECT_TRUE( + CompareMatrices(screw_joint.acceleration_upper_limits(), inf)); } // Tests the error handling for an unsupported joint type (when actuated). @@ -1116,21 +1139,6 @@ TEST_F(SdfParserTest, ActuatedBallJointParsingTest) { ClearDiagnostics(); } -// Tests the error handling for an unsupported joint type. -TEST_F(SdfParserTest, ScrewJointParsingTest) { - ParseTestString(R"""( - - - - world - larry - -)"""); - EXPECT_THAT(FormatFirstError(), ::testing::MatchesRegex( - ".*screw.*not supported.*jerry.*")); - ClearDiagnostics(); -} - // Tests the error handling for an unsupported joint type. TEST_F(SdfParserTest, GearboxJointParsingTest) { ParseTestString(R"""( diff --git a/multibody/parsing/test/sdf_parser_test/joint_parsing_test.sdf b/multibody/parsing/test/sdf_parser_test/joint_parsing_test.sdf index 34d1a54684c3..a19761d1760d 100644 --- a/multibody/parsing/test/sdf_parser_test/joint_parsing_test.sdf +++ b/multibody/parsing/test/sdf_parser_test/joint_parsing_test.sdf @@ -2,7 +2,7 @@ - + @@ -217,6 +217,30 @@ Defines an SDF model with various types of joints used for testing the parser. + + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + link9 + link8 + 0.04 + + 1 0 0 + + 0.1 + + + diff --git a/tools/workspace/sdformat_internal/repository.bzl b/tools/workspace/sdformat_internal/repository.bzl index 98be0f236b48..03936dbb52f6 100644 --- a/tools/workspace/sdformat_internal/repository.bzl +++ b/tools/workspace/sdformat_internal/repository.bzl @@ -8,9 +8,9 @@ def sdformat_internal_repository( github_archive( name = name, repository = "gazebosim/sdformat", - commit = "sdformat13_13.1.0", + commit = "sdformat13_13.2.0", build_file = ":package.BUILD.bazel", - sha256 = "4b40bf64bc7d3f22c89a2395c2802d5315a3fd5e5dbaf29b087c5c806180a9db", # noqa + sha256 = "e1084091f65caf8aabd4cfca6df3a7bf3a9563de1715829810813840598d5de3", # noqa patches = [ ":patches/console.patch", ":patches/deprecation_unit_testing.patch",