Parse screw joints in URDF

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
RobotLocomotion · Nov 2, 2022 · 36d1126 · 36d1126
1 parent a2fcd07
commit 36d1126
Show file tree

Hide file tree

Showing 6 changed files with 114 additions and 3 deletions.
diff --git 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<double> 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"
+            " <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);

diff --git 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
@@ -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<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) {
@@ -764,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.

diff --git 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.
       <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"/>
@@ -101,6 +107,14 @@ Defines a URDF model with various types of joints.
     <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>

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
@@ -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>
diff --git a/...arsing/test/urdf_parser_test/joint_parsing_test_missing_screw_thread_pitch_attribute.urdf b/...arsing/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>