parsing: Use libsdformat's Interface API to parse URDF files

RobotLocomotion · Jul 1, 2021 · 11d7b7b · 11d7b7b
1 parent 2d737e4
commit 11d7b7b
Show file tree

Hide file tree

Showing 19 changed files with 790 additions and 46 deletions.
diff --git a/multibody/parsing/BUILD.bazel b/multibody/parsing/BUILD.bazel
@@ -15,6 +15,7 @@ filegroup(
         "test/**/*.config",
         "test/**/*.obj",
         "test/**/*.sdf",
+        "test/**/*.forced_nesting_sdf",
         "test/**/*.urdf",
         "test/**/*.xml",
         "test/**/*.png",
@@ -129,6 +130,7 @@ drake_cc_library(
     deps = [
         ":detail_misc",
         ":detail_scene_graph",
+        ":detail_urdf_parser",
         ":scoped_names",
         "//multibody/plant",
         "@sdformat",
@@ -151,6 +153,7 @@ drake_cc_library(
     deps = [
         ":detail_misc",
         ":package_map",
+        ":scoped_names",
         "//multibody/plant",
         "@fmt",
         "@tinyxml2",

diff --git a/multibody/parsing/detail_ignition.cc b/multibody/parsing/detail_ignition.cc
@@ -17,6 +17,13 @@ RigidTransformd ToRigidTransform(const ignition::math::Pose3d& pose) {
   return RigidTransformd(rotation, ToVector3(pose.Pos()));;
 }
 
+ignition::math::Pose3d ToIgnPose3d(const RigidTransformd& pose) {
+  const auto& quat = pose.rotation().ToQuaternion();
+  return ignition::math::Pose3d(
+      ignition::math::Vector3d(pose.translation().x(), pose.translation().y(),
+                               pose.translation().z()),
+      ignition::math::Quaterniond(quat.w(), quat.x(), quat.y(), quat.z()));
+}
 }  // namespace internal
 }  // namespace multibody
 }  // namespace drake
diff --git a/multibody/parsing/detail_ignition.h b/multibody/parsing/detail_ignition.h
@@ -17,6 +17,10 @@ Eigen::Vector3d ToVector3(const ignition::math::Vector3d& vector);
 // a RigidTransform instance.
 math::RigidTransformd ToRigidTransform(const ignition::math::Pose3d& pose);
 
+// Helper function to express a RigidTransform instance as an
+// ignition::math::Pose3d instance.
+ignition::math::Pose3d ToIgnPose3d(const math::RigidTransformd& pose);
+
 }  // namespace internal
 }  // namespace multibody
 }  // namespace drake