multibody: Add parsing for PlanarJoint (RobotLocomotion#13824)

* Add parsing for PlanarJoint both SDF and URDF
beamiter · Aug 10, 2020 · 1b564b5 · 1b564b5
1 parent 25fcaba
commit 1b564b5
Show file tree

Hide file tree

Showing 6 changed files with 163 additions and 45 deletions.
diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc
@@ -18,6 +18,7 @@
 #include "drake/multibody/parsing/detail_scene_graph.h"
 #include "drake/multibody/tree/ball_rpy_joint.h"
 #include "drake/multibody/tree/fixed_offset_frame.h"
+#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/revolute_spring.h"
@@ -572,55 +573,91 @@ const Frame<double>& AddFrameFromSpecification(
   return frame;
 }
 
+Eigen::Vector3d ParseVector3(const sdf::ElementPtr node,
+                             const char* element_name) {
+  if (!node->HasElement(element_name)) {
+    throw std::runtime_error(
+        fmt::format("<{}>: Unable to find the <{}> child tag.", node->GetName(),
+                    element_name));
+  }
+
+  auto value = node->Get<ignition::math::Vector3d>(element_name);
+
+  return ToVector3(value);
+}
+
+const Frame<double>& ParseFrame(const sdf::ElementPtr node,
+                                MultibodyPlant<double>* plant,
+                                const char* element_name) {
+  if (!node->HasElement(element_name)) {
+    throw std::runtime_error(
+        fmt::format("<{}>: Unable to find the <{}> child tag.", node->GetName(),
+                    element_name));
+  }
+
+  const std::string frame_name = node->Get<std::string>(element_name);
+
+  if (!plant->HasFrameNamed(frame_name)) {
+    throw std::runtime_error(fmt::format(
+        "<{}>: Frame '{}' specified for <{}> does not exist in the model.",
+        node->GetName(), frame_name, element_name));
+  }
+
+  return plant->GetFrameByName(frame_name);
+}
+
+// TODO(eric.cousineau): Update parsing pending resolution of
+// https://github.com/osrf/sdformat/issues/288
+void AddDrakeJointFromSpecification(const sdf::ElementPtr node,
+                                    MultibodyPlant<double>* plant) {
+  if (!node->HasAttribute("type")) {
+    throw std::runtime_error(
+        "<drake:joint>: Unable to find the 'type' attribute.");
+  }
+  const std::string joint_type = node->Get<std::string>("type");
+  if (!node->HasAttribute("name")) {
+    throw std::runtime_error(
+        "<drake:joint>: Unable to find the 'name' attribute.");
+  }
+  const std::string joint_name = node->Get<std::string>("name");
+
+  // TODO(eric.cousineau): Add support for parsing joint pose.
+  if (node->HasElement("pose")) {
+    throw std::runtime_error(
+        "<drake:joint> does not yet support the <pose> child tag.");
+  }
+
+  const Frame<double>& parent_frame = ParseFrame(node, plant, "drake:parent");
+  const Frame<double>& child_frame = ParseFrame(node, plant, "drake:child");
+
+  if (joint_type == "planar") {
+    // TODO(eric.cousineau): Error out when there are unused tags.
+    Vector3d damping = ParseVector3(node, "drake:damping");
+    plant->AddJoint(std::make_unique<PlanarJoint<double>>(
+        joint_name, parent_frame, child_frame, damping));
+  } else {
+    throw std::runtime_error(
+        "ERROR: <drake:joint> '" + joint_name +
+        "' has unrecognized value for 'type' attribute: " + joint_type);
+  }
+}
+
 const LinearBushingRollPitchYaw<double>& AddBushingFromSpecification(
     const sdf::ElementPtr node, MultibodyPlant<double>* plant) {
   // Functor to read a vector valued child tag with tag name: `element_name`
   // e.g. <element_name>0 0 0</element_name>
-  // Throws an error if the tag does not exist or if the value is not properly
-  // formatted.
+  // Throws an error if the tag does not exist.
   auto read_vector = [node](const char* element_name) -> Eigen::Vector3d {
-    if (!node->HasElement(element_name)) {
-      throw std::runtime_error(
-          fmt::format("Unable to find the <{}> tag.", element_name));
-    }
-
-    auto [value, successful] = node->Get<ignition::math::Vector3d>(
-        element_name, ignition::math::Vector3d() /* default value. not used */);
-
-    if (!successful) {
-      throw std::runtime_error(fmt::format(
-          "Unable to read the value of the <{}> tag.", element_name));
-    }
-
-    return ToVector3(value);
+    return ParseVector3(node, element_name);
   };
 
   // Functor to read a child tag with tag name: `element_name` that specifies a
   // frame name, e.g. <element_name>frame_name</element_name>
-  // Throws an error if the tag does not exist or if the value
-  // is not properly formatted.
+  // Throws an error if the tag does not exist or if the frame does not exist in
+  // the plant.
   auto read_frame = [node,
                      plant](const char* element_name) -> const Frame<double>& {
-    if (!node->HasElement(element_name)) {
-      throw std::runtime_error(
-          fmt::format("Unable to find the <{}> tag.", element_name));
-    }
-
-    auto [frame_name, successful] = node->Get<std::string>(
-        element_name, std::string() /* default value. not used */);
-
-    if (!successful) {
-      throw std::runtime_error(fmt::format(
-          "Unable to read the value of the <{}> tag.", element_name));
-    }
-
-    if (!plant->HasFrameNamed(frame_name)) {
-      throw std::runtime_error(fmt::format(
-          "Frame: {} specified for <{}> does not exist in the model.",
-          frame_name, element_name));
-    }
-
-    return plant->GetFrameByName(frame_name);
+    return ParseFrame(node, plant, element_name);
   };
 
   return ParseLinearBushingRollPitchYaw(read_vector, read_frame, plant);
@@ -700,6 +737,15 @@ ModelInstanceIndex AddModelFromSpecification(
     AddFrameFromSpecification(frame, model_instance, model_frame, plant);
   }
 
+  drake::log()->trace("sdf_parser: Add drake custom joints");
+  if (model.Element()->HasElement("drake:joint")) {
+    for (sdf::ElementPtr joint_node =
+             model.Element()->GetElement("drake:joint");
+         joint_node; joint_node = joint_node->GetNextElement("drake:joint")) {
+      AddDrakeJointFromSpecification(joint_node, plant);
+    }
+  }
+
   drake::log()->trace("sdf_parser: Add linear_bushing_rpy");
   if (model.Element()->HasElement("drake:linear_bushing_rpy")) {
     for (sdf::ElementPtr bushing_node =

diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc
@@ -20,6 +20,7 @@
 #include "drake/multibody/parsing/package_map.h"
 #include "drake/multibody/tree/ball_rpy_joint.h"
 #include "drake/multibody/tree/fixed_offset_frame.h"
+#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/universal_joint.h"
@@ -445,6 +446,15 @@ void ParseJoint(ModelInstanceIndex model_instance,
     ParseJointDynamics(name, node, &damping);
     plant->AddJoint<BallRpyJoint>(name, parent_body, X_PJ,
                                   child_body, std::nullopt, damping);
+  } else if (type.compare("planar") == 0) {
+    throw_on_custom_joint(true);
+    Vector3d damping_vec(0, 0, 0);
+    XMLElement* dynamics_node = node->FirstChildElement("dynamics");
+    if (dynamics_node) {
+      ParseVectorAttribute(dynamics_node, "damping", &damping_vec);
+    }
+    plant->AddJoint<PlanarJoint>(name, parent_body, X_PJ,
+                                 child_body, std::nullopt, damping_vec);
   } else if (type.compare("universal") == 0) {
     throw_on_custom_joint(true);
     ParseJointDynamics(name, node, &damping);

diff --git a/multibody/parsing/test/detail_sdf_parser_test.cc b/multibody/parsing/test/detail_sdf_parser_test.cc
@@ -21,6 +21,7 @@
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/multibody/tree/ball_rpy_joint.h"
 #include "drake/multibody/tree/linear_bushing_roll_pitch_yaw.h"
+#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/revolute_spring.h"
@@ -595,6 +596,19 @@ GTEST_TEST(MultibodyPlantSdfParserTest, JointParsingTest) {
   EXPECT_TRUE(CompareMatrices(universal_joint.velocity_lower_limits(),
                               neg_inf2));
   EXPECT_TRUE(CompareMatrices(universal_joint.velocity_upper_limits(), inf2));
+
+  // Planar joint
+  DRAKE_EXPECT_NO_THROW(plant.GetJointByName<PlanarJoint>("planar_joint"));
+  const PlanarJoint<double>& planar_joint =
+      plant.GetJointByName<PlanarJoint>("planar_joint");
+  EXPECT_EQ(planar_joint.name(), "planar_joint");
+  EXPECT_EQ(planar_joint.parent_body().name(), "link6");
+  EXPECT_EQ(planar_joint.child_body().name(), "link7");
+  EXPECT_TRUE(CompareMatrices(planar_joint.damping(), Vector3d::Constant(0.1)));
+  EXPECT_TRUE(CompareMatrices(planar_joint.position_lower_limits(), neg_inf3));
+  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));
 }
 
 // Verifies that the SDF parser parses the joint actuator limit correctly.
@@ -923,8 +937,9 @@ GTEST_TEST(SdfParser, BushingParsing) {
         <drake:bushing_force_damping>10 11 12</drake:bushing_force_damping>
       </drake:linear_bushing_rpy>
     </model>)"),
-      std::runtime_error,
-      "Unable to find the <drake:bushing_frameC> tag.");
+                              std::runtime_error,
+                              "<drake:linear_bushing_rpy>: Unable to find the "
+                              "<drake:bushing_frameC> child tag.");
 
   // Test non-existent frame
   DRAKE_EXPECT_THROWS_MESSAGE(
@@ -945,12 +960,12 @@ GTEST_TEST(SdfParser, BushingParsing) {
       </drake:linear_bushing_rpy>
     </model>)"),
       std::runtime_error,
-      "Frame: frameZ specified for <drake:bushing_frameC> does not exist in "
+      "<drake:linear_bushing_rpy>: Frame 'frameZ' specified for "
+      "<drake:bushing_frameC> does not exist in "
       "the model.");
 
   // Test missing constants tag
-  DRAKE_EXPECT_THROWS_MESSAGE(
-      ParseTestString(R"(
+  DRAKE_EXPECT_THROWS_MESSAGE(ParseTestString(R"(
     <model name='BushingModel'>
       <link name='A'/>
       <link name='C'/>
@@ -965,8 +980,9 @@ GTEST_TEST(SdfParser, BushingParsing) {
         <drake:bushing_force_damping>10 11 12</drake:bushing_force_damping>
       </drake:linear_bushing_rpy>
     </model>)"),
-      std::runtime_error,
-      "Unable to find the <drake:bushing_torque_damping> tag.");
+                              std::runtime_error,
+                              "<drake:linear_bushing_rpy>: Unable to find the "
+                              "<drake:bushing_torque_damping> child tag.");
 }
 
 }  // namespace

diff --git a/multibody/parsing/test/detail_urdf_parser_test.cc b/multibody/parsing/test/detail_urdf_parser_test.cc
@@ -16,6 +16,7 @@
 #include "drake/multibody/parsing/detail_path_utils.h"
 #include "drake/multibody/tree/ball_rpy_joint.h"
 #include "drake/multibody/tree/linear_bushing_roll_pitch_yaw.h"
+#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/universal_joint.h"
@@ -287,6 +288,19 @@ GTEST_TEST(MultibodyPlantUrdfParserTest, JointParsingTest) {
   EXPECT_TRUE(CompareMatrices(universal_joint.velocity_lower_limits(),
                               neg_inf2));
   EXPECT_TRUE(CompareMatrices(universal_joint.velocity_upper_limits(), inf2));
+
+  // Planar joint
+  DRAKE_EXPECT_NO_THROW(plant.GetJointByName<PlanarJoint>("planar_joint"));
+  const PlanarJoint<double>& planar_joint =
+      plant.GetJointByName<PlanarJoint>("planar_joint");
+  EXPECT_EQ(planar_joint.name(), "planar_joint");
+  EXPECT_EQ(planar_joint.parent_body().name(), "link6");
+  EXPECT_EQ(planar_joint.child_body().name(), "link7");
+  EXPECT_TRUE(CompareMatrices(planar_joint.damping(), Vector3d::Constant(0.1)));
+  EXPECT_TRUE(CompareMatrices(planar_joint.position_lower_limits(), neg_inf3));
+  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));
 }
 
 GTEST_TEST(MultibodyPlantUrdfParserTest, JointParsingTagMismatchTest) {

diff --git a/multibody/parsing/test/sdf_parser_test/joint_parsing_test.sdf b/multibody/parsing/test/sdf_parser_test/joint_parsing_test.sdf
@@ -149,5 +149,25 @@ Defines an SDF model with various types of joints used for testing the parser.
         <xyz expressed_in="__model__">0 0 1</xyz>
       </axis>
     </joint>
+    <link name="link7">
+      <inertial>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.1</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.1</iyy>
+          <iyz>0</iyz>
+          <izz>0.1</izz>
+        </inertia>
+      </inertial>
+    </link>
+    <frame name='frame6' attached_to='link6'/>
+    <frame name='frame7' attached_to='link7'/>
+    <drake:joint name="planar_joint" type="planar">
+      <drake:child>frame7</drake:child>
+      <drake:parent>frame6</drake:parent>
+      <drake:damping>0.1 0.1 0.1</drake:damping>
+    </drake:joint>
   </model>
 </sdf>
diff --git a/multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf b/multibody/parsing/test/urdf_parser_test/joint_parsing_test.urdf
@@ -39,6 +39,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="link7">
+    <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"/>
@@ -77,6 +83,12 @@ Defines a URDF model with various types of joints.
     <origin rpy="0 0 0" xyz="0 0 0"/>
     <dynamics damping="0.1"/>
   </drake:joint>
+  <drake:joint name="planar_joint" type="planar">
+    <parent link="link6"/>
+    <child link="link7"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <dynamics damping="0.1 0.1 0.1"/>
+  </drake:joint>
   <!-- Normal transmission/joint, should be created with appropriate
   effort limit. -->
   <transmission>