From 36d112627aa5d92e1e9bd808c13ec04a2d94f6a0 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 26 Oct 2022 16:58:07 -0700 Subject: [PATCH] Parse screw joints in URDF Signed-off-by: Steve Peters --- multibody/parsing/detail_urdf_parser.cc | 31 ++++++++++++++ multibody/parsing/parsing_doxygen.h | 9 ++-- .../parsing/test/detail_urdf_parser_test.cc | 42 +++++++++++++++++++ .../urdf_parser_test/joint_parsing_test.urdf | 14 +++++++ ...rsing_test_missing_screw_thread_pitch.urdf | 10 +++++ ..._missing_screw_thread_pitch_attribute.urdf | 11 +++++ 6 files changed, 114 insertions(+), 3 deletions(-) create mode 100644 multibody/parsing/test/urdf_parser_test/joint_parsing_test_missing_screw_thread_pitch.urdf create mode 100644 multibody/parsing/test/urdf_parser_test/joint_parsing_test_missing_screw_thread_pitch_attribute.urdf diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc index 6ec05203f77a..9d35de04c5d4 100644 --- a/multibody/parsing/detail_urdf_parser.cc +++ b/multibody/parsing/detail_urdf_parser.cc @@ -30,6 +30,7 @@ #include "drake/multibody/tree/planar_joint.h" #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/revolute_joint.h" +#include "drake/multibody/tree/screw_joint.h" #include "drake/multibody/tree/universal_joint.h" #include "drake/multibody/tree/weld_joint.h" @@ -92,6 +93,7 @@ class UrdfParser { std::string* type, std::string* parent_link_name, std::string* child_link_name); + void ParseScrewJointThreadPitch(XMLElement* node, double* screw_thread_pitch); void ParseCollisionFilterGroup(XMLElement* node); void ParseBody(XMLElement* node, MaterialMap* materials); SpatialInertia ExtractSpatialInertiaAboutBoExpressedInB( @@ -413,6 +415,27 @@ void UrdfParser::ParseJointDynamics(XMLElement* node, double* damping) { } } +void UrdfParser::ParseScrewJointThreadPitch(XMLElement* node, + double* screw_thread_pitch) { + // default value of 1 mm / rev + *screw_thread_pitch = 0.0; + XMLElement* screw_thread_pitch_node = + node->FirstChildElement("drake:screw_thread_pitch"); + if (screw_thread_pitch_node) { + if (!ParseScalarAttribute(screw_thread_pitch_node, "value", + screw_thread_pitch)) { + Error(*screw_thread_pitch_node, "A screw joint has a" + " tag that is missing the 'value'" + " attribute."); + return; + } + } else { + Error(*node, "A screw joint is missing the " + " tag."); + return; + } +} + const Body* UrdfParser::GetBodyForElement( const std::string& element_name, const std::string& link_name) { @@ -556,6 +579,14 @@ void UrdfParser::ParseJoint( } plant->AddJoint(name, *parent_body, X_PJ, *child_body, std::nullopt, damping_vec); + } else if (type.compare("screw") == 0) { + throw_on_custom_joint(true); + ParseJointDynamics(node, &damping); + double screw_thread_pitch; + ParseScrewJointThreadPitch(node, &screw_thread_pitch); + plant->AddJoint(name, *parent_body, X_PJ, *child_body, + std::nullopt, axis, screw_thread_pitch, + damping); } else if (type.compare("universal") == 0) { throw_on_custom_joint(true); ParseJointDynamics(node, &damping); diff --git a/multibody/parsing/parsing_doxygen.h b/multibody/parsing/parsing_doxygen.h index ed5beed5635c..124795d978a4 100644 --- a/multibody/parsing/parsing_doxygen.h +++ b/multibody/parsing/parsing_doxygen.h @@ -182,6 +182,7 @@ Here is the full list of custom elements: - @ref tag_drake_relaxation_time - @ref tag_drake_rigid_hydroelastic - @ref tag_drake_rotor_inertia +- @ref tag_drake_screw_thread_pitch @subsection tag_drake_acceleration drake:acceleration @@ -519,11 +520,13 @@ semantics are the same as for a standard joint. In SDFormat, the only supported `type` value is `planar`. The element must contain nested `drake:parent`, `drake:child`, and `drake:damping` elements. -In URDF, supported `type` values are one of `ball`, `planar`, or +In URDF, supported `type` values are one of `ball`, `planar`, `screw` or `universal`. The nested elements are the same as those defined by the standard -joint element. +joint element with the exception of the `screw` joint type, which requires +a nested `drake:screw_thread_pitch` element. -@see @ref tag_drake_parent, @ref tag_drake_child, @ref tag_drake_damping +@see @ref tag_drake_parent, @ref tag_drake_child, @ref tag_drake_damping, +@ref tag_drake_screw_thread_pitch @subsection tag_drake_linear_bushing_rpy drake:linear_bushing_rpy diff --git a/multibody/parsing/test/detail_urdf_parser_test.cc b/multibody/parsing/test/detail_urdf_parser_test.cc index ec2c042dfc1a..65da4b401450 100644 --- a/multibody/parsing/test/detail_urdf_parser_test.cc +++ b/multibody/parsing/test/detail_urdf_parser_test.cc @@ -23,6 +23,7 @@ #include "drake/multibody/tree/planar_joint.h" #include "drake/multibody/tree/prismatic_joint.h" #include "drake/multibody/tree/revolute_joint.h" +#include "drake/multibody/tree/screw_joint.h" #include "drake/multibody/tree/universal_joint.h" namespace drake { @@ -743,6 +744,28 @@ TEST_F(UrdfParserTest, 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")); + const ScrewJoint& screw_joint = + plant_.GetJointByName("screw_joint"); + 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)); } TEST_F(UrdfParserTest, JointParsingTagMismatchTest) { @@ -764,6 +787,25 @@ TEST_F(UrdfParserTest, JointParsingTagMismatchTest) { " type, and should be a ")); } +TEST_F(UrdfParserTest, JointParsingTagMissingScrewParametersTest) { + // Screw joint with missing thread pitch parameter. + const std::string full_name_missing_element = FindResourceOrThrow( + "drake/multibody/parsing/test/urdf_parser_test/" + "joint_parsing_test_missing_screw_thread_pitch.urdf"); + AddModelFromUrdfFile(full_name_missing_element, ""); + EXPECT_THAT(TakeError(), MatchesRegex( + ".*A screw joint is missing the " + " tag.")); + + const std::string full_name_missing_attribute = FindResourceOrThrow( + "drake/multibody/parsing/test/urdf_parser_test/" + "joint_parsing_test_missing_screw_thread_pitch_attribute.urdf"); + AddModelFromUrdfFile(full_name_missing_attribute, ""); + EXPECT_THAT(TakeError(), MatchesRegex( + ".*A screw joint has a tag" + " that is missing the 'value' attribute.")); +} + // We allow users to declare the "world" link for the purpose of declaring // "anchored" geometry (visual and collision). Specifying inertial properties // is not *strictly* an error -- a warning will be written to the console. diff --git a/multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf b/multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf index 42994fd7018f..855753d12cd5 100644 --- a/multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf +++ b/multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf @@ -51,6 +51,12 @@ Defines a URDF model with various types of joints. + + + + + + @@ -101,6 +107,14 @@ Defines a URDF model with various types of joints. + + + + + + + + diff --git a/multibody/parsing/test/urdf_parser_test/joint_parsing_test_missing_screw_thread_pitch.urdf b/multibody/parsing/test/urdf_parser_test/joint_parsing_test_missing_screw_thread_pitch.urdf new file mode 100644 index 000000000000..de8f684f67e4 --- /dev/null +++ b/multibody/parsing/test/urdf_parser_test/joint_parsing_test_missing_screw_thread_pitch.urdf @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/multibody/parsing/test/urdf_parser_test/joint_parsing_test_missing_screw_thread_pitch_attribute.urdf b/multibody/parsing/test/urdf_parser_test/joint_parsing_test_missing_screw_thread_pitch_attribute.urdf new file mode 100644 index 000000000000..d65186b89802 --- /dev/null +++ b/multibody/parsing/test/urdf_parser_test/joint_parsing_test_missing_screw_thread_pitch_attribute.urdf @@ -0,0 +1,11 @@ + + + + + + + + + + +