Implement USD rigidbody parser (#17833) (#21616)

RobotLocomotion · Jul 1, 2024 · ef7675a · ef7675a
1 parent 5dbd2d5
commit ef7675a
Show file tree

Hide file tree

Showing 18 changed files with 1,458 additions and 94 deletions.
diff --git a/multibody/parsing/BUILD.bazel b/multibody/parsing/BUILD.bazel
@@ -28,6 +28,7 @@ filegroup(
         "test/**/*.png",
         "test/**/*.sdf",
         "test/**/*.urdf",
+        "test/**/*.usd",
         "test/**/*.usda",
         "test/**/*.xml",
         "test/**/*.zip",
@@ -271,13 +272,22 @@ drake_cc_library(
     name = "detail_usd_parser",
     srcs = select({
         "//tools:with_usd": [
+            "detail_usd_geometry.cc",
             "detail_usd_parser.cc",
         ],
         "//conditions:default": [
             "detail_usd_parser_off.cc",
         ],
     }),
-    hdrs = ["detail_usd_parser.h"],
+    hdrs = select({
+        "//tools:with_usd": [
+            "detail_usd_geometry.h",
+            "detail_usd_parser.h",
+        ],
+        "//conditions:default": [
+            "detail_usd_parser.h",
+        ],
+    }),
     copts = [
         # TODO(jwnimmer-tri) OpenUSD has a gargantuan number of warnings.
         # We should really try to patch it to fix most/all of them.
@@ -291,6 +301,7 @@ drake_cc_library(
         "//multibody/plant",
     ],
     deps = [
+        ":detail_make_model_name",
         "//common:find_runfiles",
         "//common:unused",
     ] + select({
@@ -374,6 +385,7 @@ drake_cc_library(
         ":detail_parsing_workspace",
         ":detail_sdf_parser",
         ":detail_urdf_parser",
+        ":detail_usd_parser",
     ],
 )
 

diff --git a/multibody/parsing/detail_select_parser.cc b/multibody/parsing/detail_select_parser.cc
@@ -9,6 +9,7 @@
 #include "drake/multibody/parsing/detail_mujoco_parser.h"
 #include "drake/multibody/parsing/detail_sdf_parser.h"
 #include "drake/multibody/parsing/detail_urdf_parser.h"
+#include "drake/multibody/parsing/detail_usd_parser.h"
 
 namespace drake {
 namespace multibody {
@@ -18,7 +19,7 @@ using drake::internal::DiagnosticPolicy;
 
 namespace {
 
-enum class FileType { kUnknown, kSdf, kUrdf, kMjcf, kDmd, kMesh };
+enum class FileType { kUnknown, kSdf, kUrdf, kMjcf, kDmd, kMesh, kUsd };
 FileType DetermineFileType(const DiagnosticPolicy& policy,
                            const std::string& filename) {
   if (EndsWithCaseInsensitive(filename, ".urdf")) {
@@ -36,6 +37,10 @@ FileType DetermineFileType(const DiagnosticPolicy& policy,
   if (EndsWithCaseInsensitive(filename, ".obj")) {
     return FileType::kMesh;
   }
+  if (EndsWithCaseInsensitive(filename, ".usda") ||
+      EndsWithCaseInsensitive(filename, ".usd")) {
+    return FileType::kUsd;
+  }
   policy.Error(fmt::format(
       "The file '{}' is not a recognized type."
       " Known types are: .urdf, .sdf, .xml (Mujoco), .dmd.yaml, .obj",
@@ -70,6 +75,7 @@ ParserInterface& SelectParser(const DiagnosticPolicy& policy,
   static never_destroyed<internal::UnknownParserWrapper> unknown;
   static never_destroyed<internal::DmdParserWrapper> dmd;
   static never_destroyed<internal::MeshParserWrapper> mesh;
+  static never_destroyed<internal::UsdParserWrapper> usd;
   const FileType type = DetermineFileType(policy, filename);
   switch (type) {
     case FileType::kUrdf:
@@ -82,6 +88,8 @@ ParserInterface& SelectParser(const DiagnosticPolicy& policy,
       return dmd.access();
     case FileType::kMesh:
       return mesh.access();
+    case FileType::kUsd:
+      return usd.access();
     case FileType::kUnknown:
       return unknown.access();
   }