usd -> sdf: Read transforms

ahcorde · ahcorde · commit 576f9bd6fdfd · 2022-03-09T14:05:05.000+01:00
Signed-off-by: ahcorde &lt;ahcorde@gmail.com&gt;
diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh
@@ -0,0 +1,120 @@
+/*
+ * 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/Vector3.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 contains 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
+    {
+      public:
+        /// \brief Scale of the schema
+        ignition::math::Vector3d scale{1, 1, 1};
+
+        /// \brief Rotation of the schema
+        std::vector<ignition::math::Quaterniond> q;
+
+        /// \brief Translatio of the schema
+        ignition::math::Vector3d translate{0, 0, 0};
+
+        /// \brief True if there is a rotation ZYX definedor false otherwise
+        bool isRotationZYX = false;
+
+        /// \brief True if there is a rotation (as a quaterion) defined
+        /// or false otherwise
+        bool isRotation = false;
+
+        /// \brief True if there is a translation defined or false otherwise
+        bool isTranslate = false;
+    };
+
+    /// \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 IGNITION_SDFORMAT_USD_VISIBLE 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
diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt
@@ -17,6 +17,7 @@ set(sources
   usd_parser/USDData.cc
   usd_parser/USDMaterial.cc
   usd_parser/USDStage.cc
+  usd_parser/USDTransforms.cc
 )
 
 ign_add_component(usd SOURCES ${sources} GET_TARGET_NAME usd_target)
@@ -46,6 +47,7 @@ set(gtest_sources
   sdf_parser/World_Sdf2Usd_TEST.cc
   usd_parser/USDData_TEST.cc
   usd_parser/USDStage_TEST.cc
+  usd_parser/USDTransforms_TEST.cc
   Conversions_TEST.cc
   UsdError_TEST.cc
   UsdUtils_TEST.cc
diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc
@@ -0,0 +1,261 @@
+/*
+ * 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/USDTransforms.hh"
+#include "sdf/usd/usd_parser/USDData.hh"
+
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Vector3.hh>
+
+namespace sdf
+{
+  // Inline bracke to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  //
+  namespace usd
+  {
+
+  void GetAllTransforms(
+    const pxr::UsdPrim &_prim,
+    USDData &_usdData,
+    std::vector<ignition::math::Pose3d> &_tfs,
+    ignition::math::Vector3d &_scale,
+    const std::string &_schemaToStop)
+  {
+    pxr::UsdPrim parent = _prim;
+    double metersPerUnit = 1.0;
+    std::string upAxis = "Y";
+
+    // this assumes that there can only be one stage
+    auto stageData = _usdData.FindStage(parent.GetPath().GetName());
+    if (stageData.second != nullptr) {
+      metersPerUnit = stageData.second->MetersPerUnit();
+      upAxis = stageData.second->UpAxis();
+    }
+
+    while (parent)
+    {
+      if (pxr::TfStringify(parent.GetPath()) == _schemaToStop)
+      {
+        return;
+      }
+
+      UDSTransforms t = ParseUSDTransform(parent);
+
+      ignition::math::Pose3d pose;
+      _scale *= t.scale;
+
+      pose.Pos() = t.translate * metersPerUnit;
+      // scaling is lost when we convert to pose, so we pre-scale the translation
+      // to make them match the scaled values.
+      if (!_tfs.empty()) {
+        auto& child = _tfs.back();
+        child.Pos().Set(
+          child.Pos().X() * t.scale[0],
+          child.Pos().Y() * t.scale[1],
+          child.Pos().Z() * t.scale[2]);
+      }
+
+      if (!t.isRotationZYX)
+      {
+        if (t.isRotation)
+        {
+          pose.Rot() = t.q[0];
+        }
+        _tfs.push_back(pose);
+      }
+      else
+      {
+        ignition::math::Pose3d poseZ = ignition::math::Pose3d(
+          ignition::math::Vector3d(0 ,0 ,0), t.q[2]);
+        ignition::math::Pose3d poseY = ignition::math::Pose3d(
+          ignition::math::Vector3d(0 ,0 ,0), t.q[1]);
+        ignition::math::Pose3d poseX = ignition::math::Pose3d(
+          ignition::math::Vector3d(0 ,0 ,0), t.q[0]);
+
+        ignition::math::Pose3d poseT = ignition::math::Pose3d(
+          t.translate * metersPerUnit,
+          ignition::math::Quaterniond(1, 0, 0, 0));
+
+        _tfs.push_back(poseZ);
+        _tfs.push_back(poseY);
+        _tfs.push_back(poseX);
+        _tfs.push_back(poseT);
+      }
+      parent = parent.GetParent();
+    }
+
+    if (upAxis == "Y")
+    {
+      ignition::math::Pose3d poseUpAxis = ignition::math::Pose3d(
+        ignition::math::Vector3d(0 ,0 ,0),
+        ignition::math::Quaterniond(IGN_PI_2, 0, 0));
+      _tfs.push_back(poseUpAxis);
+    }
+  }
+
+  void GetTransform(
+    const pxr::UsdPrim &_prim,
+    USDData &_usdData,
+    ignition::math::Pose3d &_pose,
+    ignition::math::Vector3d &_scale,
+    const std::string &_schemaToStop)
+  {
+    std::vector<ignition::math::Pose3d> tfs;
+    GetAllTransforms(_prim, _usdData, tfs, _scale, _schemaToStop);
+    for (auto & rt : tfs)
+    {
+      _pose = rt * _pose;
+    }
+  }
+
+  UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim)
+  {
+    auto variant_geom = pxr::UsdGeomGprim(_prim);
+    auto transforms = variant_geom.GetXformOpOrderAttr();
+
+    pxr::GfVec3f scale(1, 1, 1);
+    pxr::GfVec3f translate(0, 0, 0);
+    pxr::GfQuatf rotationQuad(1, 0, 0, 0);
+
+    UDSTransforms t;
+
+    pxr::VtTokenArray xformOpOrder;
+    transforms.Get(&xformOpOrder);
+    for (auto & op: xformOpOrder)
+    {
+      if (op == "xformOp:scale")
+      {
+        auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:scale"));
+        if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3f")
+        {
+          attribute.Get(&scale);
+        }
+        else if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3d")
+        {
+          pxr::GfVec3d scaleTmp(1, 1, 1);
+          attribute.Get(&scaleTmp);
+          scale[0] = scaleTmp[0];
+          scale[1] = scaleTmp[1];
+          scale[2] = scaleTmp[2];
+        }
+        t.scale = ignition::math::Vector3d(scale[0], scale[1], scale[2]);
+      }
+      else if (op == "xformOp:rotateZYX" || op == "xformOp:rotateXYZ")
+      {
+        pxr::GfVec3f rotationEuler(0, 0, 0);
+        auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:rotateZYX"));
+        if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3f")
+        {
+          attribute.Get(&rotationEuler);
+        }
+        else if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3d")
+        {
+          pxr::GfVec3d rotationEulerTmp(0, 0, 0);
+          attribute.Get(&rotationEulerTmp);
+          rotationEuler[0] = rotationEulerTmp[0];
+          rotationEuler[1] = rotationEulerTmp[1];
+          rotationEuler[2] = rotationEulerTmp[2];
+        }
+        ignition::math::Quaterniond qX, qY, qZ;
+        ignition::math::Angle angleX(rotationEuler[0] * IGN_PI / 180.0);
+        ignition::math::Angle angleY(rotationEuler[1] * IGN_PI / 180.0);
+        ignition::math::Angle angleZ(rotationEuler[2] * IGN_PI / 180.0);
+        qX = ignition::math::Quaterniond(angleX.Normalized().Radian(), 0, 0);
+        qY = ignition::math::Quaterniond(0, angleY.Normalized().Radian(), 0);
+        qZ = ignition::math::Quaterniond(0, 0, angleZ.Normalized().Radian());
+
+        t.q.push_back(qX);
+        t.q.push_back(qY);
+        t.q.push_back(qZ);
+        t.isRotationZYX = true;
+        t.isRotation = true;
+      }
+      else if (op == "xformOp:translate")
+      {
+        auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:translate"));
+        if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3f")
+        {
+          attribute.Get(&translate);
+        }
+        else if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3d")
+        {
+          pxr::GfVec3d translateTmp(0, 0, 0);
+          attribute.Get(&translateTmp);
+          translate[0] = translateTmp[0];
+          translate[1] = translateTmp[1];
+          translate[2] = translateTmp[2];
+        }
+        t.translate = ignition::math::Vector3d(translate[0], translate[1], translate[2]);
+        t.isTranslate = true;
+      }
+      else if (op == "xformOp:orient")
+      {
+        auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:orient"));
+        if (attribute.GetTypeName().GetCPPTypeName() == "GfQuatf")
+        {
+          attribute.Get(&rotationQuad);
+        }
+        else if (attribute.GetTypeName().GetCPPTypeName() == "GfQuatd")
+        {
+          pxr::GfQuatd rotationQuadTmp;
+          attribute.Get(&rotationQuadTmp);
+          rotationQuad.SetImaginary(
+            rotationQuadTmp.GetImaginary()[0],
+            rotationQuadTmp.GetImaginary()[1],
+            rotationQuadTmp.GetImaginary()[2]);
+          rotationQuad.SetReal(rotationQuadTmp.GetReal());
+        }
+        ignition::math::Quaterniond q(
+          rotationQuad.GetReal(),
+          rotationQuad.GetImaginary()[0],
+          rotationQuad.GetImaginary()[1],
+          rotationQuad.GetImaginary()[2]);
+        t.q.push_back(q);
+        t.isRotation = true;
+      }
+
+      if (op == "xformOp:transform")
+      {
+        // FIXME: Shear is lost (does sdformat support it?).
+        pxr::GfMatrix4d transform;
+        _prim.GetAttribute(pxr::TfToken("xformOp:transform")).Get(&transform);
+        const auto rot = transform.RemoveScaleShear();
+        const auto scaleShear = transform * rot.GetInverse();
+
+        t.scale[0] = scaleShear[0][0];
+        t.scale[1] = scaleShear[1][1];
+        t.scale[2] = scaleShear[2][2];
+
+        const auto rotQuat = rot.ExtractRotationQuat();
+        t.translate = ignition::math::Vector3d(transform[3][0], transform[3][1], transform[3][2]);
+        ignition::math::Quaterniond q(
+          rotQuat.GetReal(),
+          rotQuat.GetImaginary()[0],
+          rotQuat.GetImaginary()[1],
+          rotQuat.GetImaginary()[2]
+        );
+        t.q.push_back(q);
+        t.isTranslate = true;
+        t.isRotation = true;
+      }
+    }
+    return t;
+  }
+}
+}
+}
diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc
