USD -> SDF: Parse Physics and generated SDF file (#863)

Signed-off-by: ahcorde <[email protected]>
Co-authored-by: Ashton Larkin <[email protected]>

test/usd/upAxisY.usda

@@ -6,12 +6,6 @@
 
 def "shapes"
 {
-    def PhysicsScene "physics"
-    {
-        vector3f physics:gravityDirection = (0, 0, -1)
-        float physics:gravityMagnitude = 9.8
-    }
-
     def Xform "ground_plane"
     {
         float3 xformOp:rotateXYZ = (0, 0, 0)

usd/include/sdf/usd/usd_parser/Parser.hh

@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef SDF_USD_USD_PARSER_PARSER_HH
+#define SDF_USD_USD_PARSER_PARSER_HH
+
+#include <string>
+
+#include "sdf/config.hh"
+#include "sdf/usd/Export.hh"
+#include "sdf/usd/UsdError.hh"
+
+namespace sdf
+{
+  // Inline bracket to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  //
+  namespace usd
+  {
+    /// \brief Parse a USD file and convert it to a SDF file
+    /// \param[in] _inputFilenameUsd Path of the USD file to parse
+    /// \param[in] _outputFilenameSdf Path where the SDF file will be located
+    /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError
+    /// includes an error code and message. An empty vector indicates no error
+    /// occurred when parsing the USD file to its SDF representation.
+    UsdErrors IGNITION_SDFORMAT_USD_VISIBLE parseUSDFile(
+      const std::string &_inputFilenameUsd,
+      const std::string &_outputFilenameSdf);
+  }
+  }
+}
+#endif

usd/src/CMakeLists.txt

@@ -10,9 +10,13 @@ set(sources
   sdf_parser/Sensor.cc
   sdf_parser/Visual.cc
   sdf_parser/World.cc
+  usd_parser/Parser.cc
+  usd_parser/USD2SDF.cc
   usd_parser/USDData.cc
   usd_parser/USDMaterial.cc
+  usd_parser/USDPhysics.cc
   usd_parser/USDStage.cc
+  usd_parser/USDWorld.cc
 )
 
 ign_add_component(usd SOURCES ${sources} GET_TARGET_NAME usd_target)
@@ -41,6 +45,7 @@ set(gtest_sources
   sdf_parser/Visual_Sdf2Usd_TEST.cc
   sdf_parser/World_Sdf2Usd_TEST.cc
   usd_parser/USDData_TEST.cc
+  usd_parser/USDPhysics_TEST.cc
   usd_parser/USDStage_TEST.cc
   Conversions_TEST.cc
   UsdError_TEST.cc
@@ -55,4 +60,9 @@ ign_build_tests(
   INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test
 )
 
+if (TARGET UNIT_USDPhysics_TEST)
+  target_sources(UNIT_USDPhysics_TEST PRIVATE
+  usd_parser/USDPhysics.cc)
+endif()
+
 add_subdirectory(cmd)

usd/src/cmd/usd2sdf.cc

@@ -20,6 +20,7 @@
 
 #include <ignition/utils/cli/CLI.hpp>
 
+#include "sdf/usd/usd_parser/Parser.hh"
 #include "sdf/config.hh"
 
 //////////////////////////////////////////////////
@@ -43,10 +44,17 @@ struct Options
   std::string outputFilename{"output.sdf"};
 };
 
-void runCommand(const Options &/*_opt*/)
+void runCommand(const Options &_opt)
 {
-  // TODO(ahcorde): Call here the USD to SDF converter code
-  std::cerr << "USD to SDF conversion is not implemented yet.\n";
+  const auto errors =
+    sdf::usd::parseUSDFile(_opt.inputFilename, _opt.outputFilename);
+  if (!errors.empty())
+  {
+    std::cerr << "Errors occurred when generating [" << _opt.outputFilename
+              << "] from [" << _opt.inputFilename << "]:\n";
+    for (const auto &e : errors)
+      std::cerr << "\t" << e << "\n";
+  }
 }
 
 void addFlags(CLI::App &_app)

usd/src/usd_parser/Parser.cc

@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "sdf/usd/usd_parser/Parser.hh"
+#include "USD2SDF.hh"
+
+#include "sdf/Root.hh"
+
+namespace sdf
+{
+inline namespace SDF_VERSION_NAMESPACE {
+namespace usd
+{
+  UsdErrors parseUSDFile(
+      const std::string &_inputFilenameUsd,
+      const std::string &_outputFilenameSdf)
+  {
+    UsdErrors errors;
+    USD2SDF usd2sdf;
+    sdf::Root root;
+    errors = usd2sdf.Read(_inputFilenameUsd, root);
+    if (!errors.empty())
+    {
+      return errors;
+    }
+
+    std::ofstream out(_outputFilenameSdf.c_str(), std::ios::out);
+    if (!out)
+    {
+      errors.emplace_back(UsdError(
+        UsdErrorCode::SDF_TO_USD_PARSING_ERROR,
+        "Unable to open file [" + _outputFilenameSdf + "] for writing"));
+      return errors;
+    }
+    out << root.ToElement()->ToString("");
+    out.close();
+    return errors;
+  }
+}
+}
+}

usd/src/usd_parser/USD2SDF.cc

@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "USD2SDF.hh"
+
+#include "sdf/Console.hh"
+#include "sdf/Types.hh"
+#include "USDWorld.hh"
+
+#include "sdf/Error.hh"
+#include "sdf/Root.hh"
+#include "sdf/World.hh"
+
+#include "sdf/usd/UsdError.hh"
+
+namespace sdf {
+inline namespace SDF_VERSION_NAMESPACE {
+namespace usd {
+////////////////////////////////////////////////
+UsdErrors USD2SDF::Read(const std::string &_fileName,
+  sdf::Root &_root)
+{
+  UsdErrors errors;
+
+  sdf::World sdfWorld;
+
+  errors = parseUSDWorld(_fileName, sdfWorld);
+  if (!errors.empty())
+  {
+    errors.emplace_back(UsdError(
+      UsdErrorCode::SDF_TO_USD_PARSING_ERROR,
+      "Error parsing usd file [" + _fileName + "]"));
+    return errors;
+  }
+
+  const auto addWorldErrors = _root.AddWorld(sdfWorld);
+  if (!addWorldErrors.empty())
+  {
+    for (const auto & error: addWorldErrors)
+    {
+      errors.emplace_back(error);
+    }
+    errors.emplace_back(UsdError(sdf::Error(
+      sdf::ErrorCode::ELEMENT_INVALID,
+      "Error adding the world [" + sdfWorld.Name() + "] to the SDF DOM")));
+  }
+
+  return errors;
+}
+}
+}
+}

usd/src/usd_parser/USD2SDF.hh

@@ -0,0 +1,56 @@
+/*
+ * Copyright 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef USD_PARSER_USD2SDF_HH_
+#define USD_PARSER_USD2SDF_HH_
+
+#include <string>
+
+#include "sdf/sdf_config.h"
+#include "sdf/usd/UsdError.hh"
+
+#include "sdf/Root.hh"
+
+namespace sdf
+{
+// Inline bracket to help doxygen filtering.
+inline namespace SDF_VERSION_NAMESPACE {
+  namespace usd
+  {
+    /// \brief USD to SDF converter
+    /// This class helps generate the SDF file
+    class USD2SDF
+    {
+      /// \brief constructor
+      public: USD2SDF() = default;
+
+      /// \brief convert USD file to a sdf::Root object
+      /// \param[in] _fileName string containing USD filename.
+      /// \param[out] _root Root element to populate with the equivalent sdf
+      /// information from _fileName.
+      /// \return UsdErrors, which is a list of UsdError objects. An empty list
+      /// means no errors occurred when populating _root with the contents
+      /// of _fileName
+      public: UsdErrors Read(
+        const std::string &_fileName,
+        sdf::Root &_root);
+    };
+  }
+}
+}
+
+#endif

usd/src/usd_parser/USDPhysics.cc

@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "USDPhysics.hh"
+
+#pragma push_macro ("__DEPRECATED")
+#undef __DEPRECATED
+#include <pxr/base/gf/vec3d.h>
+#include <pxr/usd/usdPhysics/scene.h>
+#pragma pop_macro ("__DEPRECATED")
+
+#include "sdf/World.hh"
+
+namespace sdf
+{
+inline namespace SDF_VERSION_NAMESPACE {
+namespace usd
+{
+  void ParseUSDPhysicsScene(
+    const pxr::UsdPhysicsScene &_scene,
+    sdf::World &_world,
+    double _metersPerUnit)
+  {
+    ignition::math::Vector3d worldGravity{0, 0, -1};
+    float magnitude {9.8f};
+    const auto gravityAttr = _scene.GetGravityDirectionAttr();
+    if (gravityAttr)
+    {
+      pxr::GfVec3f gravity;
+      gravityAttr.Get(&gravity);
+      if (!ignition::math::equal(0.0f, gravity[0]) &&
+          !ignition::math::equal(0.0f, gravity[1]) &&
+          !ignition::math::equal(0.0f, gravity[2]))
+      {
+        worldGravity[0] = gravity[0];
+        worldGravity[1] = gravity[1];
+        worldGravity[2] = gravity[2];
+      }
+    }
+
+    const auto magnitudeAttr = _scene.GetGravityMagnitudeAttr();
+    if (magnitudeAttr)
+    {
+      magnitudeAttr.Get(&magnitude);
+      if (!std::isnan(magnitude) && !std::isinf(magnitude))
+      {
+        magnitude = magnitude * _metersPerUnit;
+      }
+    }
+    _world.SetGravity(worldGravity * magnitude);
+  }
+}
+}
+}