diff --git a/test/usd/nested_transforms.usda b/test/usd/nested_transforms.usda new file mode 100644 index 000000000..32e46edf4 --- /dev/null +++ b/test/usd/nested_transforms.usda @@ -0,0 +1,36 @@ +#usda 1.0 +( + metersPerUnit = 0.01 + upAxis = "Z" +) + +def "transforms" +{ + def Xform "nested_transforms_XYZ" + { + float3 xformOp:rotateXYZ = (0, 0, 90) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "child_transform" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + } + } + + def Xform "nested_transforms_ZYX" + { + float3 xformOp:rotateZYX = (90, 0, 0) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] + + def Xform "child_transform" + { + float3 xformOp:rotateZYX = (0, 0, 0) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] + } + } +} diff --git a/test/usd/upAxisZ.usda b/test/usd/upAxisZ.usda index 6933533c8..72f3527f5 100644 --- a/test/usd/upAxisZ.usda +++ b/test/usd/upAxisZ.usda @@ -120,9 +120,9 @@ def "shapes" def Xform "sphere" { - float3 xformOp:rotateXYZ = (0, 0, 0) + float3 xformOp:rotateZYX = (-69, 31, -62) double3 xformOp:translate = (0, 1.5, 0.5) - uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] def Xform "sphere_link" ( prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] @@ -155,9 +155,9 @@ def "shapes" def Xform "capsule" { - float3 xformOp:rotateXYZ = (0, 0, 0) + float3 xformOp:rotateZYX = (15, 80, -55) double3 xformOp:translate = (0, -3, 0.5) - uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] def Xform "capsule_link" ( prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] @@ -172,9 +172,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"] @@ -191,7 +191,7 @@ def "shapes" def Xform "ellipsoid" { - float3 xformOp:rotateXYZ = (0, 0, 0) + float3 xformOp:rotateXYZ = (15, 80, -55) double3 xformOp:translate = (0, 3, 0.5) uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] @@ -230,7 +230,7 @@ def "shapes" { float inputs:intensity = 1000 float intensity = 100 - float3 xformOp:rotateXYZ = (0, 0, 0) + float3 xformOp:rotateXYZ = (0, -35, 0) double3 xformOp:translate = (0, 0, 10) uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] } diff --git a/usd/include/sdf/usd/usd_parser/USDData.hh b/usd/include/sdf/usd/usd_parser/USDData.hh index d76e861dd..9936858cb 100644 --- a/usd/include/sdf/usd/usd_parser/USDData.hh +++ b/usd/include/sdf/usd/usd_parser/USDData.hh @@ -82,7 +82,7 @@ namespace sdf /// \param[in] _name Name of the path to find /// \return A pair with the name of the stage and the data public: const std::pair> - FindStage(const std::string &_name); + FindStage(const std::string &_name) const; public: friend std::ostream& operator<<( std::ostream& os, const USDData& data) diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh new file mode 100644 index 000000000..4b0d75da5 --- /dev/null +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -0,0 +1,116 @@ +/* + * 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_USDTRANSFORMS_HH_ +#define SDF_USD_USD_PARSER_USDTRANSFORMS_HH_ + +#include +#include + +#include +#include +#include + +#include + +// 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 +#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 bracket 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 + class IGNITION_SDFORMAT_USD_VISIBLE UDSTransforms + { + /// \brief Default constructor + public: UDSTransforms(); + + /// \brief Translate + /// \return A 3D vector with the translation + public: const ignition::math::Vector3d Translation() const; + + /// \brief Scale + /// \return A 3D vector with the scale + public: const ignition::math::Vector3d Scale() const; + + /// \brief Get the Rotation + /// \return Return The rotation, if one exists. If no rotation exists, + /// std::nullopt is returned + public: const std::optional Rotation() const; + + /// \brief Set translate + /// \param[in] _translate Translate to set + public: void SetTranslation(const ignition::math::Vector3d &_translate); + + /// \brief Set scale + /// \param[in] _scale Scale to set + public: void SetScale(const ignition::math::Vector3d &_scale); + + /// \brief Set rotation + /// \param[in] _q Quaternion that defines the rotation + public: void SetRotation(const ignition::math::Quaterniond &_q); + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + + /// \brief This function gets the transform from a prim to the specified + /// _schemaToStop variable + /// \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. From _prim to _schemaToStop. + /// \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, + const 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 + /// \return A USDTransforms class with all the transforms related to + /// the prim + UDSTransforms IGNITION_SDFORMAT_USD_VISIBLE ParseUSDTransform( + const pxr::UsdPrim &_prim); +} +} +} +#endif diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index ae2145d26..3f1858473 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -16,6 +16,7 @@ set(sources usd_parser/USDMaterial.cc usd_parser/USDPhysics.cc usd_parser/USDStage.cc + usd_parser/USDTransforms.cc usd_parser/USDWorld.cc ) @@ -47,6 +48,7 @@ set(gtest_sources usd_parser/USDData_TEST.cc usd_parser/USDPhysics_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/USDData.cc b/usd/src/usd_parser/USDData.cc index 535b24c44..e0dcbbc84 100644 --- a/usd/src/usd_parser/USDData.cc +++ b/usd/src/usd_parser/USDData.cc @@ -236,7 +236,7 @@ namespace usd { ///////////////////////////////////////////////// const std::pair> - USDData::FindStage(const std::string &_name) + USDData::FindStage(const std::string &_name) const { for (auto &ref : this->dataPtr->references) { diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc new file mode 100644 index 000000000..7b30b0259 --- /dev/null +++ b/usd/src/usd_parser/USDTransforms.cc @@ -0,0 +1,342 @@ +/* + * 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 +#include + +#include +#include + +#include "sdf/usd/usd_parser/USDData.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + +const char kXFormOpTranslate[] = {"xformOp:translate"}; +const char kXFormOpOrient[] = {"xformOp:orient"}; +const char kXFormOpTransform[] = {"xformOp:transform"}; +const char kXFormOpScale[] = {"xformOp:scale"}; +const char kXFormOpRotateXYZ[] = {"xformOp:rotateXYZ"}; +const char kXFormOpRotateZYX[] = {"xformOp:rotateZYX"}; + +const char kGfVec3fString[] = {"GfVec3f"}; +const char kGfVec3dString[] = {"GfVec3d"}; +const char kGfQuatfString[] = {"GfQuatf"}; +const char kGfQuatdString[] = {"GfQuatd"}; + +/// \brief Private altimeter data. +class UDSTransforms::Implementation +{ + /// \brief Scale of the schema + public: ignition::math::Vector3d scale{1, 1, 1}; + + /// \brief Rotation of the schema + public: std::optional q = std::nullopt; + + /// \brief Translation of the schema + public: ignition::math::Vector3d translate{0, 0, 0}; +}; + +///////////////////////////////////////////////// +UDSTransforms::UDSTransforms() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +////////////////////////////////////////////////// +const ignition::math::Vector3d UDSTransforms::Translation() const +{ + return this->dataPtr->translate; +} + +////////////////////////////////////////////////// +const ignition::math::Vector3d UDSTransforms::Scale() const +{ + return this->dataPtr->scale; +} + +////////////////////////////////////////////////// +const std::optional UDSTransforms::Rotation() const +{ + return this->dataPtr->q; +} + +////////////////////////////////////////////////// +void UDSTransforms::SetTranslation( + const ignition::math::Vector3d &_translate) +{ + this->dataPtr->translate = _translate; +} + +////////////////////////////////////////////////// +void UDSTransforms::SetScale( + const ignition::math::Vector3d &_scale) +{ + this->dataPtr->scale = _scale; +} + +////////////////////////////////////////////////// +void UDSTransforms::SetRotation( + const ignition::math::Quaterniond &_q) +{ + this->dataPtr->q = _q; +} + +////////////////////////////////////////////////// +/// \brief This function will parse all the parent transforms of a prim. +/// \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, + const USDData &_usdData, + std::vector &_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 + const auto stageData = _usdData.FindStage(parent.GetPath().GetName()); + if (stageData.second) { + 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.Translation() * 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.Rotation()) + { + pose.Rot() = t.Rotation().value(); + } + _tfs.push_back(pose); + parent = parent.GetParent(); + } + + if (upAxis == "Y") + { + // Add additional rotation to match with Z up Axis. + // TODO(anyone) handle upAxis == "X". This is a case that is rarely + // used by other renderers + 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, + const USDData &_usdData, + ignition::math::Pose3d &_pose, + ignition::math::Vector3d &_scale, + const std::string &_schemaToStop) +{ + std::vector tfs; + GetAllTransforms(_prim, _usdData, tfs, _scale, _schemaToStop); + for (auto & rt : tfs) + { + _pose = rt * _pose; + } +} + +////////////////////////////////////////////////// +UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) +{ + auto variantGeom = pxr::UsdGeomGprim(_prim); + auto transforms = variantGeom.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 (const auto &op : xformOpOrder) + { + if (op == kXFormOpScale) + { + auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpScale)); + if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) + { + attribute.Get(&scale); + } + else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString) + { + pxr::GfVec3d scaleTmp(1, 1, 1); + attribute.Get(&scaleTmp); + scale[0] = static_cast(scaleTmp[0]); + scale[1] = static_cast(scaleTmp[1]); + scale[2] = static_cast(scaleTmp[2]); + } + t.SetScale(ignition::math::Vector3d(scale[0], scale[1], scale[2])); + } + else if (op == kXFormOpRotateZYX || op == kXFormOpRotateXYZ) + { + pxr::GfVec3f rotationEuler(0, 0, 0); + pxr::UsdAttribute attribute; + if (op == kXFormOpRotateZYX) + { + attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateZYX)); + } + else + { + attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateXYZ)); + } + if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) + { + attribute.Get(&rotationEuler); + } + else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString) + { + pxr::GfVec3d rotationEulerTmp(0, 0, 0); + attribute.Get(&rotationEulerTmp); + rotationEuler[0] = static_cast(rotationEulerTmp[0]); + rotationEuler[1] = static_cast(rotationEulerTmp[1]); + rotationEuler[2] = static_cast(rotationEulerTmp[2]); + } + ignition::math::Quaterniond qX, qY, qZ; + ignition::math::Angle angleX(IGN_DTOR(rotationEuler[0])); + ignition::math::Angle angleY(IGN_DTOR(rotationEuler[1])); + ignition::math::Angle angleZ(IGN_DTOR(rotationEuler[2])); + 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()); + + // TODO(ahcorde) This part should be reviewed, revisit how rotateXYZ + // and rotateZYX are handle. + // Related issue https://github.com/ignitionrobotics/sdformat/issues/926 + // if (op == kXFormOpRotateZYX) + // { + // std::swap(angleX, angleZ); + // } + t.SetRotation((qX * qY) * qZ); + } + else if (op == kXFormOpTranslate) + { + auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpTranslate)); + if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) + { + attribute.Get(&translate); + } + else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString) + { + pxr::GfVec3d translateTmp(0, 0, 0); + attribute.Get(&translateTmp); + translate[0] = static_cast(translateTmp[0]); + translate[1] = static_cast(translateTmp[1]); + translate[2] = static_cast(translateTmp[2]); + } + t.SetTranslation(ignition::math::Vector3d( + translate[0], + translate[1], + translate[2])); + } + else if (op == kXFormOpOrient) + { + auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpOrient)); + if (attribute.GetTypeName().GetCPPTypeName() == kGfQuatfString) + { + attribute.Get(&rotationQuad); + } + else if (attribute.GetTypeName().GetCPPTypeName() == kGfQuatdString) + { + 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.SetRotation(q); + } + + if (op == kXFormOpTransform) + { + // TODO(koonpeng) Shear is lost (does sdformat support it?). + pxr::GfMatrix4d transform; + _prim.GetAttribute(pxr::TfToken(kXFormOpTransform)).Get(&transform); + const auto rot = transform.RemoveScaleShear(); + const auto scaleShear = transform * rot.GetInverse(); + + t.SetScale(ignition::math::Vector3d( + scaleShear[0][0], + scaleShear[1][1], + scaleShear[2][2])); + + const auto rotQuat = rot.ExtractRotationQuat(); + t.SetTranslation(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.SetRotation(q); + } + } + return t; +} +} +} +} diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc new file mode 100644 index 000000000..b6a5d9bfa --- /dev/null +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -0,0 +1,221 @@ +/* + * 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 +#include +#include + +#include + +#include + +#include "test_config.h" +#include "test_utils.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 +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/usd/usd_parser/USDTransforms.hh" + +void checkTransforms( + const std::string &_primName, + pxr::UsdStageRefPtr &_stage, + const ignition::math::Vector3d &_translation, + const std::optional &_rotation, + const ignition::math::Vector3d &_scale) +{ + pxr::UsdPrim prim = _stage->GetPrimAtPath(pxr::SdfPath(_primName)); + ASSERT_TRUE(prim); + + sdf::usd::UDSTransforms usdTransforms = + sdf::usd::ParseUSDTransform(prim); + + EXPECT_EQ(_translation, usdTransforms.Translation()); + EXPECT_EQ(_scale, usdTransforms.Scale()); + if (_rotation) + { + ASSERT_TRUE(usdTransforms.Rotation()); + EXPECT_TRUE(ignition::math::equal( + _rotation.value().Roll(), + usdTransforms.Rotation().value().Roll(), 0.1)); + EXPECT_TRUE(ignition::math::equal( + _rotation.value().Pitch(), + usdTransforms.Rotation().value().Pitch(), 0.1)); + EXPECT_TRUE(ignition::math::equal( + _rotation.value().Yaw(), + usdTransforms.Rotation().value().Yaw(), 0.1)); + } +} + +///////////////////////////////////////////////// +TEST(USDTransformsTest, ParseUSDTransform) +{ + std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + checkTransforms( + "/shapes/ground_plane", + stage, + ignition::math::Vector3d(0, 0, -0.125), + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/shapes/ground_plane/link/visual/geometry", + stage, + ignition::math::Vector3d(0, 0, 0), + std::nullopt, + ignition::math::Vector3d(100, 100, 0.25) + ); + + checkTransforms( + "/shapes/cylinder", + stage, + ignition::math::Vector3d(0, -1.5, 0.5), + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/shapes/sphere", + stage, + ignition::math::Vector3d(0, 1.5, 0.5), + ignition::math::Quaterniond( + IGN_DTOR(-62), IGN_DTOR(-47.5), IGN_DTOR(-53.41)), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/shapes/capsule", + stage, + ignition::math::Vector3d(0, -3.0, 0.5), + ignition::math::Quaterniond( + IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2)), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/shapes/capsule/capsule_link/capsule_visual", + stage, + ignition::math::Vector3d(0, 0, 0), + ignition::math::Quaterniond(0, 0, M_PI_2), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/shapes/ellipsoid", + stage, + ignition::math::Vector3d(0, 3.0, 0.5), + ignition::math::Quaterniond( + IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2)), + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/shapes/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry", + stage, + ignition::math::Vector3d(0, 0, 0), + std::nullopt, + ignition::math::Vector3d(0.4, 0.6, 1) + ); + + checkTransforms( + "/shapes/sun", + stage, + ignition::math::Vector3d(0, 0, 10), + ignition::math::Quaterniond(0, IGN_DTOR(-35), 0), + ignition::math::Vector3d(1, 1, 1) + ); +} + +///////////////////////////////////////////////// +TEST(USDTransformsTest, GetAllTransform) +{ + { + std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + sdf::usd::USDData usdData(filename); + usdData.Init(); + + pxr::UsdPrim prim = stage->GetPrimAtPath( + pxr::SdfPath( + "/shapes/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry")); + ASSERT_TRUE(prim); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale{1, 1, 1}; + + sdf::usd::GetTransform(prim, usdData, pose, scale, "/shapes"); + + EXPECT_EQ(ignition::math::Vector3d(0.4, 0.6, 1), scale); + EXPECT_EQ( + ignition::math::Pose3d( + ignition::math::Vector3d(0, 0.03, 0.005), + ignition::math::Quaterniond( + IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2))), + pose); + } + + { + std::string filename = + sdf::testing::TestFile("usd", "nested_transforms.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + sdf::usd::USDData usdData(filename); + usdData.Init(); + + std::function verifyNestedTf = + [&](const std::string &_path, + const ignition::math::Vector3d &_posePrim, + const ignition::math::Quaterniond &_qPrim) + { + pxr::UsdPrim prim = stage->GetPrimAtPath(pxr::SdfPath(_path)); + ASSERT_TRUE(prim); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale{1, 1, 1}; + + sdf::usd::GetTransform(prim, usdData, pose, scale, "/transforms"); + + EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), scale); + EXPECT_EQ(ignition::math::Pose3d(_posePrim, _qPrim), pose); + }; + + verifyNestedTf( + "/transforms/nested_transforms_XYZ/child_transform", + ignition::math::Vector3d(0.01, 0.01, 0), + ignition::math::Quaterniond(0, 0, IGN_DTOR(90))); + verifyNestedTf( + "/transforms/nested_transforms_ZYX/child_transform", + ignition::math::Vector3d(0.02, 0.0, 0), + ignition::math::Quaterniond(IGN_DTOR(90), 0, 0)); + } +}