USD to SDF: Added diff drive plugin

test/usd/upAxisZ.usda

@@ -13,6 +13,30 @@ def "shapes"
         float physics:gravityMagnitude = 9.8
     }
 
+    def DistantLight "defaultLight" (
+        prepend apiSchemas = ["ShapingAPI"]
+        kind = "model"
+    )
+    {
+        float angle = 1
+        float inputs:intensity = 5000
+        float shaping:cone:angle = 180
+        float3 xformOp:rotateZYX = (0, 45, 0)
+        float3 xformOp:translate = (0, 0, 0)
+        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
+    }
+
+    def DiskLight "diskLight" (
+        prepend apiSchemas = ["ShapingAPI"]
+        kind = "model"
+    )
+    {
+        float inputs:intensity = 3000
+        float3 xformOp:rotateZYX = (0, 0, 45)
+        float3 xformOp:translate = (0, 0, 1000)
+        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
+    }
+
     def Xform "ground_plane"
     {
         float3 xformOp:rotateXYZ = (0, 0, 0)
@@ -172,9 +196,9 @@ def "shapes"
 
             def Xform "capsule_visual"
             {
-                float3 xformOp:rotateXYZ = (0, 0, 0)
+                float3 xformOp:rotateZYX = (0, 0, 90)
                 double3 xformOp:translate = (0, 0, 0)
-                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
+                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
 
                 def Capsule "geometry" (
                     prepend apiSchemas = ["PhysicsCollisionAPI"]

usd/include/sdf/usd/UsdError.hh

@@ -52,6 +52,9 @@ namespace sdf
     /// \brief Parsing of SDF object to a USD object failed.
     SDF_TO_USD_PARSING_ERROR,
 
+    /// \brief Parsing of USD object to a SDF object failed.
+    USD_TO_SDF_PARSING_ERROR,
+
     /// \brief The pxr::SdfPath does not point to a valid USD prim.
     INVALID_PRIM_PATH,
 

usd/include/sdf/usd/usd_parser/USDTransforms.hh

@@ -0,0 +1,146 @@
+/*
+ * 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_UTILS_HH_
+#define SDF_USD_USD_PARSER_UTILS_HH_
+
+#include <string>
+#include <vector>
+
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Quaternion.hh>
+#include <ignition/math/Vector3.hh>
+
+#include <ignition/utils/ImplPtr.hh>
+
+// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings
+// are reported using #pragma message so normal diagnostic flags cannot remove
+// them. This workaround requires this block to be used whenever usd is
+// included.
+#pragma push_macro ("__DEPRECATED")
+#undef __DEPRECATED
+#include <pxr/usd/usdGeom/gprim.h>
+#pragma pop_macro ("__DEPRECATED")
+
+#include "sdf/sdf_config.h"
+#include "sdf/system_util.hh"
+#include "sdf/usd/Export.hh"
+#include "sdf/usd/UsdError.hh"
+
+#include "USDData.hh"
+
+namespace sdf
+{
+  // Inline bracke to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  //
+  namespace usd
+  {
+    /// \brief This class stores the transforms of a schema
+    /// This might contain scale, translate or rotation operations
+    /// The booleans are used to check if there is a transform defined
+    /// in the schema
+    /// Rotation is splitted in a vector because this might be defined
+    /// as a rotation of 3 angles (ZYX, XYZ, etc).
+    class IGNITION_SDFORMAT_USD_VISIBLE UDSTransforms
+    {
+      /// \brief Default constructor
+      public: UDSTransforms();
+
+      /// \brief Translate
+      public: ignition::math::Vector3d Translate();
+
+      /// \brief Scale
+      public: ignition::math::Vector3d Scale();
+
+      /// \brief Rotation
+      public: std::vector<ignition::math::Quaterniond> Rotations();
+
+      /// \brief Set translate
+      public: void SetTranslate(const ignition::math::Vector3d &_translate);
+
+      /// \brief Set scale
+      public: void SetScale(const ignition::math::Vector3d &_scale);
+
+      /// \brief Add rotation
+      public: void AddRotation(const ignition::math::Quaterniond &_q);
+
+      /// \brief True if there is a rotation ZYX defined or false otherwise
+      bool RotationZYX();
+
+      /// \brief True if there is a rotation XYZ defined or false otherwise
+      bool RotationXYZ();
+
+      /// \brief True if there is a rotation (as a quaterion) defined
+      /// or false otherwise
+      bool Rotation();
+
+      /// \brief Set if there is any rotation ZYX defined
+      void SetRotationZYX(bool _rotationZYX);
+
+      /// \brief Set if there is any rotation XYZ defined
+      void SetRotationXYZ(bool _rotationXYZ);
+
+      /// \brief Set if there is any rotation defined
+      void SetRotation(bool _rotation);
+
+      /// \brief Private data pointer.
+      IGN_UTILS_IMPL_PTR(dataPtr)
+    };
+
+    /// \brief This function will parse all the parents transforms of a prim
+    /// This will stop when the name of the parent is the same as _schemaToStop
+    /// \param[in] _prim Initial prim to read the transform
+    /// \param[in] _usdData USDData structure to get info about the prim, for
+    /// example: metersperunit
+    /// \param[out] _tfs A vector with all the transforms
+    /// \param[out] _scale The scale of the prims
+    /// \param[in] _schemaToStop Name of the prim where the loop will stop
+    /// reading transforms
+    void GetAllTransforms(
+      const pxr::UsdPrim &_prim,
+      USDData &_usdData,
+      std::vector<ignition::math::Pose3d> &_tfs,
+      ignition::math::Vector3d &_scale,
+      const std::string &_schemaToStop);
+
+    /// \brief This function get the transform from a prim to the specified
+    /// schemaToStop variable
+    /// This will stop when the name of the parent is the same as _schemaToStop
+    /// \param[in] _prim Initial prim to read the transform
+    /// \param[in] _usdData USDData structure to get info about the prim, for
+    /// example: metersperunit
+    /// \param[out] _pose Pose of the prim
+    /// \param[out] _scale The scale of the prim
+    /// \param[in] _schemaToStop Name of the prim where the loop will stop
+    /// reading transforms
+    void IGNITION_SDFORMAT_USD_VISIBLE GetTransform(
+      const pxr::UsdPrim &_prim,
+      USDData &_usdData,
+      ignition::math::Pose3d &_pose,
+      ignition::math::Vector3d &_scale,
+      const std::string &_schemaToStop);
+
+    /// \brief Read the usd prim transforms. Scale, rotation or transform might
+    /// be defined as float or doubles
+    /// \param[in] _prim Prim where the transforms are read
+    UDSTransforms IGNITION_SDFORMAT_USD_VISIBLE ParseUSDTransform(
+      const pxr::UsdPrim &_prim);
+}
+}
+}
+#endif

usd/src/CMakeLists.txt

@@ -11,11 +11,17 @@ set(sources
   sdf_parser/Visual.cc
   sdf_parser/World.cc
   usd_parser/Parser.cc
+  usd_parser/polygon_helper.cc
   usd_parser/USD2SDF.cc
   usd_parser/USDData.cc
+  usd_parser/USDJoints.cc
+  usd_parser/USDLights.cc
+  usd_parser/USDLinks.cc
   usd_parser/USDMaterial.cc
   usd_parser/USDPhysics.cc
+  usd_parser/USDSensors.cc
   usd_parser/USDStage.cc
+  usd_parser/USDTransforms.cc
   usd_parser/USDWorld.cc
 )
 
@@ -46,7 +52,9 @@ set(gtest_sources
   sdf_parser/World_Sdf2Usd_TEST.cc
   usd_parser/USDData_TEST.cc
   usd_parser/USDPhysics_TEST.cc
+  usd_parser/USDLight_TEST.cc
   usd_parser/USDStage_TEST.cc
+  usd_parser/USDTransforms_TEST.cc
   Conversions_TEST.cc
   UsdError_TEST.cc
   UsdUtils_TEST.cc
@@ -57,7 +65,7 @@ ign_build_tests(
   TYPE UNIT
   SOURCES ${gtest_sources}
   LIB_DEPS ${usd_target} ignition-cmake${IGN_CMAKE_VER}::utilities
-  INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test
+  INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test usd_parser
 )
 
 if (TARGET UNIT_USDPhysics_TEST)

usd/src/cmd/usd2sdf.cc

@@ -21,6 +21,7 @@
 #include <ignition/utils/cli/CLI.hpp>
 
 #include "sdf/usd/usd_parser/Parser.hh"
+#include "sdf/usd/UsdError.hh"
 #include "sdf/config.hh"
 
 //////////////////////////////////////////////////

usd/src/sdf_parser/Link.cc

@@ -85,7 +85,7 @@ namespace usd
         return errors;
       }
 
-      if (!pxr::UsdPhysicsRigidBodyAPI::Apply(linkPrim))
+      if (!pxr::UsdPhysicsRigidBodyAPI::Apply(linkPrim.GetParent()))
       {
         errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY,
               "Internal error: unable to mark link at path [" + _path

usd/src/sdf_parser/World.cc

@@ -34,8 +34,10 @@
 #include <pxr/usd/usd/stage.h>
 #include <pxr/usd/usdGeom/tokens.h>
 #include <pxr/usd/usdPhysics/scene.h>
+#include <pxr/usd/usdPhysics/articulationRootAPI.h>
 #pragma pop_macro ("__DEPRECATED")
 
+#include "sdf/Joint.hh"
 #include "sdf/World.hh"
 #include "sdf/usd/sdf_parser/Light.hh"
 #include "sdf/usd/sdf_parser/Model.hh"
@@ -84,6 +86,32 @@ namespace usd
               "Error parsing model [" + model.Name() + "]"));
         errors.insert(errors.end(), modelErrors.begin(), modelErrors.end());
       }
+
+      if (model.JointCount() > 0)
+      {
+        for (uint64_t j = 0; j < model.JointCount(); j++)
+        {
+          const auto joint = *(model.JointByIndex(j));
+          if (joint.Type() == sdf::JointType::REVOLUTE)
+          {
+            auto prim = _stage->GetPrimAtPath(pxr::SdfPath(modelPath));
+            if (prim)
+            {
+              if (!pxr::UsdPhysicsArticulationRootAPI::Apply(prim))
+              {
+                errors.push_back(UsdError(
+                  sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY,
+                  "Internal error: unable to mark Xform at path [" +
+                  modelPath + "] as ArticulationRootAPI some feature might not"
+                  "work"));
+                errors.insert(errors.end(), modelErrors.begin(), modelErrors.end());
+                return errors;
+              }
+            }
+            break;
+          }
+        }
+      }
     }
 
     for (uint64_t i = 0; i < _world.LightCount(); ++i)

usd/src/usd_parser/USD2SDF.cc

@@ -22,6 +22,7 @@
 #include "USDWorld.hh"
 
 #include "sdf/Error.hh"
+#include "sdf/Light.hh"
 #include "sdf/Root.hh"
 #include "sdf/World.hh"