[multibody] Parse screw joints from URDF (RobotLocomotion#18191)

Parse screw joints in URDF
Add URDF continuous joint parsing test

parsing_doxygen: Fix invalid link/joint hierarchy
A <link> never contains a <joint>, so remove the
incorrect link/joint XPath expressions.

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<double> ExtractSpatialInertiaAboutBoExpressedInB(
@@ -413,6 +415,27 @@ void UrdfParser::ParseJointDynamics(XMLElement* node, double* damping) {
   }
 }
 
+void UrdfParser::ParseScrewJointThreadPitch(XMLElement* node,
+                                            double* screw_thread_pitch) {
+  // Always set a value for the output-only argument, even if parsing fails.
+  *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"
+            " <drake:screw_thread_pitch> tag that is missing the 'value'"
+            " attribute.");
+      return;
+    }
+  } else {
+      Error(*node, "A screw joint is missing the <drake:screw_thread_pitch>"
+            " tag.");
+      return;
+  }
+}
+
 const Body<double>* UrdfParser::GetBodyForElement(
     const std::string& element_name,
     const std::string& link_name) {
@@ -556,6 +579,14 @@ void UrdfParser::ParseJoint(
     }
     plant->AddJoint<PlanarJoint>(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<ScrewJoint>(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);

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
 
@@ -395,7 +396,7 @@ for translation; the third is for rotation. See that class for discussion of
 units and detailed semantics.
 
 URDF Note: The comparable feature in URDF is the standard
-`/robot/link/joint/dynamics/@damping` attribute.
+`/robot/joint/dynamics/@damping` attribute.
 
 @subsection tag_drake_declare_convex drake:declare_convex
 
@@ -441,8 +442,8 @@ the visual or collision geometry of the model.
 
 @subsection tag_drake_gear_ratio drake:gear_ratio
 
-- SDFormat path: `//model/link/joint/drake:gear_ratio`
-- URDF path: `/robot/link/joint/actuator/drake:gear_ratio@value`
+- SDFormat path: `//model/joint/drake:gear_ratio`
+- URDF path: `/robot/joint/actuator/drake:gear_ratio@value`
 - Syntax: Non-negative floating point value.
 
 @subsubsection tag_drake_gear_ratio_semantics Semantics
@@ -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
 
@@ -696,8 +699,8 @@ to be rigid, as opposed to compliant, in hydroelastic contact models.
 
 @subsection tag_drake_rotor_inertia drake:rotor_inertia
 
-- SDFormat path: `//model/link/joint/drake:rotor_inertia`
-- URDF path: `/robot/link/joint/actuator/drake:rotor_inertia@value`
+- SDFormat path: `//model/joint/drake:rotor_inertia`
+- URDF path: `/robot/joint/actuator/drake:rotor_inertia@value`
 - Syntax: Non-negative floating point value.
 
 @subsubsection tag_drake_rotor_inertia_semantics Semantics
@@ -708,4 +711,19 @@ object. Units are kg⋅m² for revolute joints, and kg for prismatic joints.
 @see drake::multibody::JointActuator, @ref tag_drake_gear_ratio,
 @ref reflected_inertia "Reflected Inertia"
 
+@subsection tag_drake_screw_thread_pitch drake:screw_thread_pitch
+
+- SDFormat path: `//model/joint/drake:screw_thread_pitch`
+- URDF path: `/robot/joint/actuator/drake:screw_thread_pitch@value`
+- Syntax: Non-zero floating point value.
+
+@subsubsection tag_drake_screw_thread_pitch_semantics Semantics
+
+Applies the indicated thread pitch value to the appropriate ScrewJoint object.
+This kinematic parameter specifies the axial distance traveled for each
+revolution of the joint. Units are m/revolution, with a positive value
+corresponding to a right-handed thread.
+
+@see drake::multibody::ScrewJoint
+
 */

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 {
@@ -723,6 +724,48 @@ TEST_F(UrdfParserTest, JointParsingTest) {
   EXPECT_TRUE(CompareMatrices(planar_joint.position_upper_limits(), inf3));
   EXPECT_TRUE(CompareMatrices(planar_joint.velocity_lower_limits(), neg_inf3));
   EXPECT_TRUE(CompareMatrices(planar_joint.velocity_upper_limits(), inf3));
+
+  // Continuous joint
+  DRAKE_EXPECT_NO_THROW(
+      plant_.GetJointByName<RevoluteJoint>("continuous_joint"));
+  const RevoluteJoint<double>& continuous_joint =
+      plant_.GetJointByName<RevoluteJoint>("continuous_joint");
+  EXPECT_EQ(continuous_joint.name(), "continuous_joint");
+  EXPECT_EQ(continuous_joint.parent_body().name(), "link7");
+  EXPECT_EQ(continuous_joint.child_body().name(), "link8");
+  EXPECT_EQ(continuous_joint.revolute_axis(), Vector3d::UnitZ());
+  EXPECT_TRUE(
+      CompareMatrices(continuous_joint.position_lower_limits(), neg_inf));
+  EXPECT_TRUE(CompareMatrices(continuous_joint.position_upper_limits(), inf));
+  EXPECT_TRUE(
+      CompareMatrices(continuous_joint.velocity_lower_limits(), neg_inf));
+  EXPECT_TRUE(CompareMatrices(continuous_joint.velocity_upper_limits(), inf));
+  EXPECT_TRUE(
+      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<ScrewJoint>("screw_joint"));
+  const ScrewJoint<double>& screw_joint =
+      plant_.GetJointByName<ScrewJoint>("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) {
@@ -744,6 +787,25 @@ TEST_F(UrdfParserTest, JointParsingTagMismatchTest) {
                   " type, and should be a <drake:joint>"));
 }
 
+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 <drake:screw_thread_pitch>"
+                  " 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 <drake:screw_thread_pitch> 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.

multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf

@@ -45,6 +45,18 @@ Defines a URDF model with various types of joints.
       <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
     </inertial>
   </link>
+  <link name="link8">
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+  </link>
+  <link name="link9">
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+    </inertial>
+  </link>
   <joint name="fixed_joint" type="fixed">
     <parent link="world"/>
     <child link="link1"/>
@@ -89,6 +101,20 @@ Defines a URDF model with various types of joints.
     <origin rpy="0 0 0" xyz="0 0 0"/>
     <dynamics damping="0.1 0.1 0.1"/>
   </drake:joint>
+  <joint name="continuous_joint" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="link7"/>
+    <child link="link8"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <drake:joint name="screw_joint" type="screw">
+    <axis xyz="1 0 0"/>
+    <parent link="link8"/>
+    <child link="link9"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <drake:screw_thread_pitch value="0.04"/>
+    <dynamics damping="0.1"/>
+  </drake:joint>
   <!-- Normal transmission/joint, should be created with appropriate
   effort limit. -->
   <transmission>

multibody/parsing/test/urdf_parser_test/joint_parsing_test_missing_screw_thread_pitch.urdf

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<robot xmlns:drake="http://drake.mit.edu" xmlns:xacro="http://ros.org/wiki/xacro" name="joint_parsing_test_missing_screw_thread_pitch">
+  <link name="link1"/>
+  <link name="link2"/>
+  <drake:joint name="screw_joint" type="screw">
+    <axis xyz="1 0 0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+  </drake:joint>
+</robot>

multibody/parsing/test/urdf_parser_test/joint_parsing_test_missing_screw_thread_pitch_attribute.urdf

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<robot xmlns:drake="http://drake.mit.edu" xmlns:xacro="http://ros.org/wiki/xacro" name="joint_parsing_test_missing_screw_thread_pitch_attribute">
+  <link name="link1"/>
+  <link name="link2"/>
+  <drake:joint name="screw_joint" type="screw">
+    <axis xyz="1 0 0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+    <drake:screw_thread_pitch/>
+  </drake:joint>
+</robot>