Skip to content

Commit 7861815

Browse files
ahcordeadlarkin
ahcorde
and
adlarkin
authoredMar 31, 2022
usd -> sdf: Read transforms (#871)
Signed-off-by: ahcorde <ahcorde@gmail.com> Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>
·
sdformat15_15.2.0sdformat12_12.5.0
1 parent 6ecfcc2 commit 7861815

File tree

8 files changed

+727
-10
lines changed

8 files changed

+727
-10
lines changed
 

‎test/usd/nested_transforms.usda

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
#usda 1.0
2+
(
3+
metersPerUnit = 0.01
4+
upAxis = "Z"
5+
)
6+
7+
def "transforms"
8+
{
9+
def Xform "nested_transforms_XYZ"
10+
{
11+
float3 xformOp:rotateXYZ = (0, 0, 90)
12+
double3 xformOp:translate = (1, 0, 0)
13+
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
14+
15+
def Xform "child_transform"
16+
{
17+
float3 xformOp:rotateXYZ = (0, 0, 0)
18+
double3 xformOp:translate = (1, 0, 0)
19+
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
20+
}
21+
}
22+
23+
def Xform "nested_transforms_ZYX"
24+
{
25+
float3 xformOp:rotateZYX = (90, 0, 0)
26+
double3 xformOp:translate = (1, 0, 0)
27+
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
28+
29+
def Xform "child_transform"
30+
{
31+
float3 xformOp:rotateZYX = (0, 0, 0)
32+
double3 xformOp:translate = (1, 0, 0)
33+
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
34+
}
35+
}
36+
}

‎test/usd/upAxisZ.usda

+8-8
Original file line numberDiff line numberDiff line change
@@ -120,9 +120,9 @@ def "shapes"
120120

121121
def Xform "sphere"
122122
{
123-
float3 xformOp:rotateXYZ = (0, 0, 0)
123+
float3 xformOp:rotateZYX = (-69, 31, -62)
124124
double3 xformOp:translate = (0, 1.5, 0.5)
125-
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
125+
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
126126

127127
def Xform "sphere_link" (
128128
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
@@ -155,9 +155,9 @@ def "shapes"
155155

156156
def Xform "capsule"
157157
{
158-
float3 xformOp:rotateXYZ = (0, 0, 0)
158+
float3 xformOp:rotateZYX = (15, 80, -55)
159159
double3 xformOp:translate = (0, -3, 0.5)
160-
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
160+
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
161161

162162
def Xform "capsule_link" (
163163
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
@@ -172,9 +172,9 @@ def "shapes"
172172

173173
def Xform "capsule_visual"
174174
{
175-
float3 xformOp:rotateXYZ = (0, 0, 0)
175+
float3 xformOp:rotateZYX = (0, 0, 90)
176176
double3 xformOp:translate = (0, 0, 0)
177-
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
177+
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
178178

179179
def Capsule "geometry" (
180180
prepend apiSchemas = ["PhysicsCollisionAPI"]
@@ -191,7 +191,7 @@ def "shapes"
191191

192192
def Xform "ellipsoid"
193193
{
194-
float3 xformOp:rotateXYZ = (0, 0, 0)
194+
float3 xformOp:rotateXYZ = (15, 80, -55)
195195
double3 xformOp:translate = (0, 3, 0.5)
196196
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
197197

@@ -230,7 +230,7 @@ def "shapes"
230230
{
231231
float inputs:intensity = 1000
232232
float intensity = 100
233-
float3 xformOp:rotateXYZ = (0, 0, 0)
233+
float3 xformOp:rotateXYZ = (0, -35, 0)
234234
double3 xformOp:translate = (0, 0, 10)
235235
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
236236
}

‎usd/include/sdf/usd/usd_parser/USDData.hh

+1-1
Original file line numberDiff line numberDiff line change
@@ -82,7 +82,7 @@ namespace sdf
8282
/// \param[in] _name Name of the path to find
8383
/// \return A pair with the name of the stage and the data
8484
public: const std::pair<std::string, std::shared_ptr<sdf::usd::USDStage>>
85-
FindStage(const std::string &_name);
85+
FindStage(const std::string &_name) const;
8686

8787
public: friend std::ostream& operator<<(
8888
std::ostream& os, const USDData& data)

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

+116
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
/*
2+
* Copyright (C) 2022 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#ifndef SDF_USD_USD_PARSER_USDTRANSFORMS_HH_
19+
#define SDF_USD_USD_PARSER_USDTRANSFORMS_HH_
20+
21+
#include <string>
22+
#include <vector>
23+
24+
#include <ignition/math/Pose3.hh>
25+
#include <ignition/math/Quaternion.hh>
26+
#include <ignition/math/Vector3.hh>
27+
28+
#include <ignition/utils/ImplPtr.hh>
29+
30+
// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings
31+
// are reported using #pragma message so normal diagnostic flags cannot remove
32+
// them. This workaround requires this block to be used whenever usd is
33+
// included.
34+
#pragma push_macro ("__DEPRECATED")
35+
#undef __DEPRECATED
36+
#include <pxr/usd/usdGeom/gprim.h>
37+
#pragma pop_macro ("__DEPRECATED")
38+
39+
#include "sdf/sdf_config.h"
40+
#include "sdf/system_util.hh"
41+
#include "sdf/usd/Export.hh"
42+
#include "sdf/usd/UsdError.hh"
43+
#include "USDData.hh"
44+
45+
namespace sdf
46+
{
47+
// Inline bracket to help doxygen filtering.
48+
inline namespace SDF_VERSION_NAMESPACE {
49+
//
50+
namespace usd
51+
{
52+
/// \brief This class stores the transforms of a schema
53+
/// This might contain scale, translate or rotation operations
54+
/// The booleans are used to check if there is a transform defined
55+
/// in the schema
56+
class IGNITION_SDFORMAT_USD_VISIBLE UDSTransforms
57+
{
58+
/// \brief Default constructor
59+
public: UDSTransforms();
60+
61+
/// \brief Translate
62+
/// \return A 3D vector with the translation
63+
public: const ignition::math::Vector3d Translation() const;
64+
65+
/// \brief Scale
66+
/// \return A 3D vector with the scale
67+
public: const ignition::math::Vector3d Scale() const;
68+
69+
/// \brief Get the Rotation
70+
/// \return Return The rotation, if one exists. If no rotation exists,
71+
/// std::nullopt is returned
72+
public: const std::optional<ignition::math::Quaterniond> Rotation() const;
73+
74+
/// \brief Set translate
75+
/// \param[in] _translate Translate to set
76+
public: void SetTranslation(const ignition::math::Vector3d &_translate);
77+
78+
/// \brief Set scale
79+
/// \param[in] _scale Scale to set
80+
public: void SetScale(const ignition::math::Vector3d &_scale);
81+
82+
/// \brief Set rotation
83+
/// \param[in] _q Quaternion that defines the rotation
84+
public: void SetRotation(const ignition::math::Quaterniond &_q);
85+
86+
/// \brief Private data pointer.
87+
IGN_UTILS_IMPL_PTR(dataPtr)
88+
};
89+
90+
/// \brief This function gets the transform from a prim to the specified
91+
/// _schemaToStop variable
92+
/// \param[in] _prim Initial prim to read the transform
93+
/// \param[in] _usdData USDData structure to get info about the prim, for
94+
/// example: metersperunit
95+
/// \param[out] _pose Pose of the prim. From _prim to _schemaToStop.
96+
/// \param[out] _scale The scale of the prim
97+
/// \param[in] _schemaToStop Name of the prim where the loop will stop
98+
/// reading transforms
99+
void IGNITION_SDFORMAT_USD_VISIBLE GetTransform(
100+
const pxr::UsdPrim &_prim,
101+
const USDData &_usdData,
102+
ignition::math::Pose3d &_pose,
103+
ignition::math::Vector3d &_scale,
104+
const std::string &_schemaToStop);
105+
106+
/// \brief Read the usd prim transforms. Scale, rotation or transform might
107+
/// be defined as float or doubles
108+
/// \param[in] _prim Prim where the transforms are read
109+
/// \return A USDTransforms class with all the transforms related to
110+
/// the prim
111+
UDSTransforms IGNITION_SDFORMAT_USD_VISIBLE ParseUSDTransform(
112+
const pxr::UsdPrim &_prim);
113+
}
114+
}
115+
}
116+
#endif

‎usd/src/CMakeLists.txt

+2
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ set(sources
1616
usd_parser/USDMaterial.cc
1717
usd_parser/USDPhysics.cc
1818
usd_parser/USDStage.cc
19+
usd_parser/USDTransforms.cc
1920
usd_parser/USDWorld.cc
2021
)
2122

@@ -47,6 +48,7 @@ set(gtest_sources
4748
usd_parser/USDData_TEST.cc
4849
usd_parser/USDPhysics_TEST.cc
4950
usd_parser/USDStage_TEST.cc
51+
usd_parser/USDTransforms_TEST.cc
5052
Conversions_TEST.cc
5153
UsdError_TEST.cc
5254
UsdUtils_TEST.cc

‎usd/src/usd_parser/USDData.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -236,7 +236,7 @@ namespace usd {
236236

237237
/////////////////////////////////////////////////
238238
const std::pair<std::string, std::shared_ptr<USDStage>>
239-
USDData::FindStage(const std::string &_name)
239+
USDData::FindStage(const std::string &_name) const
240240
{
241241
for (auto &ref : this->dataPtr->references)
242242
{

‎usd/src/usd_parser/USDTransforms.cc

+342
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,342 @@
1+
/*
2+
* Copyright (C) 2022 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#include "sdf/usd/usd_parser/USDTransforms.hh"
19+
20+
#include <optional>
21+
#include <utility>
22+
23+
#include <ignition/math/Pose3.hh>
24+
#include <ignition/math/Vector3.hh>
25+
26+
#include "sdf/usd/usd_parser/USDData.hh"
27+
28+
namespace sdf
29+
{
30+
// Inline bracket to help doxygen filtering.
31+
inline namespace SDF_VERSION_NAMESPACE {
32+
//
33+
namespace usd
34+
{
35+
36+
const char kXFormOpTranslate[] = {"xformOp:translate"};
37+
const char kXFormOpOrient[] = {"xformOp:orient"};
38+
const char kXFormOpTransform[] = {"xformOp:transform"};
39+
const char kXFormOpScale[] = {"xformOp:scale"};
40+
const char kXFormOpRotateXYZ[] = {"xformOp:rotateXYZ"};
41+
const char kXFormOpRotateZYX[] = {"xformOp:rotateZYX"};
42+
43+
const char kGfVec3fString[] = {"GfVec3f"};
44+
const char kGfVec3dString[] = {"GfVec3d"};
45+
const char kGfQuatfString[] = {"GfQuatf"};
46+
const char kGfQuatdString[] = {"GfQuatd"};
47+
48+
/// \brief Private altimeter data.
49+
class UDSTransforms::Implementation
50+
{
51+
/// \brief Scale of the schema
52+
public: ignition::math::Vector3d scale{1, 1, 1};
53+
54+
/// \brief Rotation of the schema
55+
public: std::optional<ignition::math::Quaterniond> q = std::nullopt;
56+
57+
/// \brief Translation of the schema
58+
public: ignition::math::Vector3d translate{0, 0, 0};
59+
};
60+
61+
/////////////////////////////////////////////////
62+
UDSTransforms::UDSTransforms()
63+
: dataPtr(ignition::utils::MakeImpl<Implementation>())
64+
{
65+
}
66+
67+
//////////////////////////////////////////////////
68+
const ignition::math::Vector3d UDSTransforms::Translation() const
69+
{
70+
return this->dataPtr->translate;
71+
}
72+
73+
//////////////////////////////////////////////////
74+
const ignition::math::Vector3d UDSTransforms::Scale() const
75+
{
76+
return this->dataPtr->scale;
77+
}
78+
79+
//////////////////////////////////////////////////
80+
const std::optional<ignition::math::Quaterniond> UDSTransforms::Rotation() const
81+
{
82+
return this->dataPtr->q;
83+
}
84+
85+
//////////////////////////////////////////////////
86+
void UDSTransforms::SetTranslation(
87+
const ignition::math::Vector3d &_translate)
88+
{
89+
this->dataPtr->translate = _translate;
90+
}
91+
92+
//////////////////////////////////////////////////
93+
void UDSTransforms::SetScale(
94+
const ignition::math::Vector3d &_scale)
95+
{
96+
this->dataPtr->scale = _scale;
97+
}
98+
99+
//////////////////////////////////////////////////
100+
void UDSTransforms::SetRotation(
101+
const ignition::math::Quaterniond &_q)
102+
{
103+
this->dataPtr->q = _q;
104+
}
105+
106+
//////////////////////////////////////////////////
107+
/// \brief This function will parse all the parent transforms of a prim.
108+
/// \param[in] _prim Initial prim to read the transform
109+
/// \param[in] _usdData USDData structure to get info about the prim, for
110+
/// example: metersperunit
111+
/// \param[out] _tfs A vector with all the transforms
112+
/// \param[out] _scale The scale of the prims
113+
/// \param[in] _schemaToStop Name of the prim where the loop will stop
114+
/// reading transforms
115+
void GetAllTransforms(
116+
const pxr::UsdPrim &_prim,
117+
const USDData &_usdData,
118+
std::vector<ignition::math::Pose3d> &_tfs,
119+
ignition::math::Vector3d &_scale,
120+
const std::string &_schemaToStop)
121+
{
122+
pxr::UsdPrim parent = _prim;
123+
double metersPerUnit = 1.0;
124+
std::string upAxis = "Y";
125+
126+
// this assumes that there can only be one stage
127+
const auto stageData = _usdData.FindStage(parent.GetPath().GetName());
128+
if (stageData.second) {
129+
metersPerUnit = stageData.second->MetersPerUnit();
130+
upAxis = stageData.second->UpAxis();
131+
}
132+
133+
while (parent)
134+
{
135+
if (pxr::TfStringify(parent.GetPath()) == _schemaToStop)
136+
{
137+
return;
138+
}
139+
140+
UDSTransforms t = ParseUSDTransform(parent);
141+
142+
ignition::math::Pose3d pose;
143+
_scale *= t.Scale();
144+
145+
pose.Pos() = t.Translation() * metersPerUnit;
146+
// scaling is lost when we convert to pose, so we pre-scale the
147+
// translation to make them match the scaled values.
148+
if (!_tfs.empty()) {
149+
auto& child = _tfs.back();
150+
child.Pos().Set(
151+
child.Pos().X() * t.Scale()[0],
152+
child.Pos().Y() * t.Scale()[1],
153+
child.Pos().Z() * t.Scale()[2]);
154+
}
155+
156+
if (t.Rotation())
157+
{
158+
pose.Rot() = t.Rotation().value();
159+
}
160+
_tfs.push_back(pose);
161+
parent = parent.GetParent();
162+
}
163+
164+
if (upAxis == "Y")
165+
{
166+
// Add additional rotation to match with Z up Axis.
167+
// TODO(anyone) handle upAxis == "X". This is a case that is rarely
168+
// used by other renderers
169+
ignition::math::Pose3d poseUpAxis = ignition::math::Pose3d(
170+
ignition::math::Vector3d(0, 0, 0),
171+
ignition::math::Quaterniond(IGN_PI_2, 0, 0));
172+
_tfs.push_back(poseUpAxis);
173+
}
174+
}
175+
176+
//////////////////////////////////////////////////
177+
void GetTransform(
178+
const pxr::UsdPrim &_prim,
179+
const USDData &_usdData,
180+
ignition::math::Pose3d &_pose,
181+
ignition::math::Vector3d &_scale,
182+
const std::string &_schemaToStop)
183+
{
184+
std::vector<ignition::math::Pose3d> tfs;
185+
GetAllTransforms(_prim, _usdData, tfs, _scale, _schemaToStop);
186+
for (auto & rt : tfs)
187+
{
188+
_pose = rt * _pose;
189+
}
190+
}
191+
192+
//////////////////////////////////////////////////
193+
UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim)
194+
{
195+
auto variantGeom = pxr::UsdGeomGprim(_prim);
196+
auto transforms = variantGeom.GetXformOpOrderAttr();
197+
198+
pxr::GfVec3f scale(1, 1, 1);
199+
pxr::GfVec3f translate(0, 0, 0);
200+
pxr::GfQuatf rotationQuad(1, 0, 0, 0);
201+
202+
UDSTransforms t;
203+
204+
pxr::VtTokenArray xformOpOrder;
205+
transforms.Get(&xformOpOrder);
206+
for (const auto &op : xformOpOrder)
207+
{
208+
if (op == kXFormOpScale)
209+
{
210+
auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpScale));
211+
if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString)
212+
{
213+
attribute.Get(&scale);
214+
}
215+
else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString)
216+
{
217+
pxr::GfVec3d scaleTmp(1, 1, 1);
218+
attribute.Get(&scaleTmp);
219+
scale[0] = static_cast<float>(scaleTmp[0]);
220+
scale[1] = static_cast<float>(scaleTmp[1]);
221+
scale[2] = static_cast<float>(scaleTmp[2]);
222+
}
223+
t.SetScale(ignition::math::Vector3d(scale[0], scale[1], scale[2]));
224+
}
225+
else if (op == kXFormOpRotateZYX || op == kXFormOpRotateXYZ)
226+
{
227+
pxr::GfVec3f rotationEuler(0, 0, 0);
228+
pxr::UsdAttribute attribute;
229+
if (op == kXFormOpRotateZYX)
230+
{
231+
attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateZYX));
232+
}
233+
else
234+
{
235+
attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateXYZ));
236+
}
237+
if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString)
238+
{
239+
attribute.Get(&rotationEuler);
240+
}
241+
else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString)
242+
{
243+
pxr::GfVec3d rotationEulerTmp(0, 0, 0);
244+
attribute.Get(&rotationEulerTmp);
245+
rotationEuler[0] = static_cast<float>(rotationEulerTmp[0]);
246+
rotationEuler[1] = static_cast<float>(rotationEulerTmp[1]);
247+
rotationEuler[2] = static_cast<float>(rotationEulerTmp[2]);
248+
}
249+
ignition::math::Quaterniond qX, qY, qZ;
250+
ignition::math::Angle angleX(IGN_DTOR(rotationEuler[0]));
251+
ignition::math::Angle angleY(IGN_DTOR(rotationEuler[1]));
252+
ignition::math::Angle angleZ(IGN_DTOR(rotationEuler[2]));
253+
qX = ignition::math::Quaterniond(angleX.Normalized().Radian(), 0, 0);
254+
qY = ignition::math::Quaterniond(0, angleY.Normalized().Radian(), 0);
255+
qZ = ignition::math::Quaterniond(0, 0, angleZ.Normalized().Radian());
256+
257+
// TODO(ahcorde) This part should be reviewed, revisit how rotateXYZ
258+
// and rotateZYX are handle.
259+
// Related issue https://github.com/ignitionrobotics/sdformat/issues/926
260+
// if (op == kXFormOpRotateZYX)
261+
// {
262+
// std::swap(angleX, angleZ);
263+
// }
264+
t.SetRotation((qX * qY) * qZ);
265+
}
266+
else if (op == kXFormOpTranslate)
267+
{
268+
auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpTranslate));
269+
if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString)
270+
{
271+
attribute.Get(&translate);
272+
}
273+
else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString)
274+
{
275+
pxr::GfVec3d translateTmp(0, 0, 0);
276+
attribute.Get(&translateTmp);
277+
translate[0] = static_cast<float>(translateTmp[0]);
278+
translate[1] = static_cast<float>(translateTmp[1]);
279+
translate[2] = static_cast<float>(translateTmp[2]);
280+
}
281+
t.SetTranslation(ignition::math::Vector3d(
282+
translate[0],
283+
translate[1],
284+
translate[2]));
285+
}
286+
else if (op == kXFormOpOrient)
287+
{
288+
auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpOrient));
289+
if (attribute.GetTypeName().GetCPPTypeName() == kGfQuatfString)
290+
{
291+
attribute.Get(&rotationQuad);
292+
}
293+
else if (attribute.GetTypeName().GetCPPTypeName() == kGfQuatdString)
294+
{
295+
pxr::GfQuatd rotationQuadTmp;
296+
attribute.Get(&rotationQuadTmp);
297+
rotationQuad.SetImaginary(
298+
rotationQuadTmp.GetImaginary()[0],
299+
rotationQuadTmp.GetImaginary()[1],
300+
rotationQuadTmp.GetImaginary()[2]);
301+
rotationQuad.SetReal(rotationQuadTmp.GetReal());
302+
}
303+
ignition::math::Quaterniond q(
304+
rotationQuad.GetReal(),
305+
rotationQuad.GetImaginary()[0],
306+
rotationQuad.GetImaginary()[1],
307+
rotationQuad.GetImaginary()[2]);
308+
t.SetRotation(q);
309+
}
310+
311+
if (op == kXFormOpTransform)
312+
{
313+
// TODO(koonpeng) Shear is lost (does sdformat support it?).
314+
pxr::GfMatrix4d transform;
315+
_prim.GetAttribute(pxr::TfToken(kXFormOpTransform)).Get(&transform);
316+
const auto rot = transform.RemoveScaleShear();
317+
const auto scaleShear = transform * rot.GetInverse();
318+
319+
t.SetScale(ignition::math::Vector3d(
320+
scaleShear[0][0],
321+
scaleShear[1][1],
322+
scaleShear[2][2]));
323+
324+
const auto rotQuat = rot.ExtractRotationQuat();
325+
t.SetTranslation(ignition::math::Vector3d(
326+
transform[3][0],
327+
transform[3][1],
328+
transform[3][2]));
329+
ignition::math::Quaterniond q(
330+
rotQuat.GetReal(),
331+
rotQuat.GetImaginary()[0],
332+
rotQuat.GetImaginary()[1],
333+
rotQuat.GetImaginary()[2]
334+
);
335+
t.SetRotation(q);
336+
}
337+
}
338+
return t;
339+
}
340+
}
341+
}
342+
}

‎usd/src/usd_parser/USDTransforms_TEST.cc

+221
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,221 @@
1+
/*
2+
* Copyright (C) 2022 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#include <functional>
19+
#include <optional>
20+
#include <vector>
21+
22+
#include <gtest/gtest.h>
23+
24+
#include <ignition/utilities/ExtraTestMacros.hh>
25+
26+
#include "test_config.h"
27+
#include "test_utils.hh"
28+
29+
// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings
30+
// are reported using #pragma message so normal diagnostic flags cannot remove
31+
// them. This workaround requires this block to be used whenever usd is
32+
// included.
33+
#pragma push_macro ("__DEPRECATED")
34+
#undef __DEPRECATED
35+
#include <pxr/usd/usd/stage.h>
36+
#pragma pop_macro ("__DEPRECATED")
37+
38+
#include "sdf/usd/usd_parser/USDTransforms.hh"
39+
40+
void checkTransforms(
41+
const std::string &_primName,
42+
pxr::UsdStageRefPtr &_stage,
43+
const ignition::math::Vector3d &_translation,
44+
const std::optional<ignition::math::Quaterniond> &_rotation,
45+
const ignition::math::Vector3d &_scale)
46+
{
47+
pxr::UsdPrim prim = _stage->GetPrimAtPath(pxr::SdfPath(_primName));
48+
ASSERT_TRUE(prim);
49+
50+
sdf::usd::UDSTransforms usdTransforms =
51+
sdf::usd::ParseUSDTransform(prim);
52+
53+
EXPECT_EQ(_translation, usdTransforms.Translation());
54+
EXPECT_EQ(_scale, usdTransforms.Scale());
55+
if (_rotation)
56+
{
57+
ASSERT_TRUE(usdTransforms.Rotation());
58+
EXPECT_TRUE(ignition::math::equal(
59+
_rotation.value().Roll(),
60+
usdTransforms.Rotation().value().Roll(), 0.1));
61+
EXPECT_TRUE(ignition::math::equal(
62+
_rotation.value().Pitch(),
63+
usdTransforms.Rotation().value().Pitch(), 0.1));
64+
EXPECT_TRUE(ignition::math::equal(
65+
_rotation.value().Yaw(),
66+
usdTransforms.Rotation().value().Yaw(), 0.1));
67+
}
68+
}
69+
70+
/////////////////////////////////////////////////
71+
TEST(USDTransformsTest, ParseUSDTransform)
72+
{
73+
std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda");
74+
auto stage = pxr::UsdStage::Open(filename);
75+
ASSERT_TRUE(stage);
76+
77+
checkTransforms(
78+
"/shapes/ground_plane",
79+
stage,
80+
ignition::math::Vector3d(0, 0, -0.125),
81+
ignition::math::Quaterniond(1, 0, 0, 0),
82+
ignition::math::Vector3d(1, 1, 1)
83+
);
84+
85+
checkTransforms(
86+
"/shapes/ground_plane/link/visual/geometry",
87+
stage,
88+
ignition::math::Vector3d(0, 0, 0),
89+
std::nullopt,
90+
ignition::math::Vector3d(100, 100, 0.25)
91+
);
92+
93+
checkTransforms(
94+
"/shapes/cylinder",
95+
stage,
96+
ignition::math::Vector3d(0, -1.5, 0.5),
97+
ignition::math::Quaterniond(1, 0, 0, 0),
98+
ignition::math::Vector3d(1, 1, 1)
99+
);
100+
101+
checkTransforms(
102+
"/shapes/sphere",
103+
stage,
104+
ignition::math::Vector3d(0, 1.5, 0.5),
105+
ignition::math::Quaterniond(
106+
IGN_DTOR(-62), IGN_DTOR(-47.5), IGN_DTOR(-53.41)),
107+
ignition::math::Vector3d(1, 1, 1)
108+
);
109+
110+
checkTransforms(
111+
"/shapes/capsule",
112+
stage,
113+
ignition::math::Vector3d(0, -3.0, 0.5),
114+
ignition::math::Quaterniond(
115+
IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2)),
116+
ignition::math::Vector3d(1, 1, 1)
117+
);
118+
119+
checkTransforms(
120+
"/shapes/capsule/capsule_link/capsule_visual",
121+
stage,
122+
ignition::math::Vector3d(0, 0, 0),
123+
ignition::math::Quaterniond(0, 0, M_PI_2),
124+
ignition::math::Vector3d(1, 1, 1)
125+
);
126+
127+
checkTransforms(
128+
"/shapes/ellipsoid",
129+
stage,
130+
ignition::math::Vector3d(0, 3.0, 0.5),
131+
ignition::math::Quaterniond(
132+
IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2)),
133+
ignition::math::Vector3d(1, 1, 1)
134+
);
135+
136+
checkTransforms(
137+
"/shapes/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry",
138+
stage,
139+
ignition::math::Vector3d(0, 0, 0),
140+
std::nullopt,
141+
ignition::math::Vector3d(0.4, 0.6, 1)
142+
);
143+
144+
checkTransforms(
145+
"/shapes/sun",
146+
stage,
147+
ignition::math::Vector3d(0, 0, 10),
148+
ignition::math::Quaterniond(0, IGN_DTOR(-35), 0),
149+
ignition::math::Vector3d(1, 1, 1)
150+
);
151+
}
152+
153+
/////////////////////////////////////////////////
154+
TEST(USDTransformsTest, GetAllTransform)
155+
{
156+
{
157+
std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda");
158+
auto stage = pxr::UsdStage::Open(filename);
159+
ASSERT_TRUE(stage);
160+
161+
sdf::usd::USDData usdData(filename);
162+
usdData.Init();
163+
164+
pxr::UsdPrim prim = stage->GetPrimAtPath(
165+
pxr::SdfPath(
166+
"/shapes/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry"));
167+
ASSERT_TRUE(prim);
168+
169+
ignition::math::Pose3d pose;
170+
ignition::math::Vector3d scale{1, 1, 1};
171+
172+
sdf::usd::GetTransform(prim, usdData, pose, scale, "/shapes");
173+
174+
EXPECT_EQ(ignition::math::Vector3d(0.4, 0.6, 1), scale);
175+
EXPECT_EQ(
176+
ignition::math::Pose3d(
177+
ignition::math::Vector3d(0, 0.03, 0.005),
178+
ignition::math::Quaterniond(
179+
IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2))),
180+
pose);
181+
}
182+
183+
{
184+
std::string filename =
185+
sdf::testing::TestFile("usd", "nested_transforms.usda");
186+
auto stage = pxr::UsdStage::Open(filename);
187+
ASSERT_TRUE(stage);
188+
189+
sdf::usd::USDData usdData(filename);
190+
usdData.Init();
191+
192+
std::function<void(
193+
const std::string &,
194+
const ignition::math::Vector3d &,
195+
const ignition::math::Quaterniond &)> verifyNestedTf =
196+
[&](const std::string &_path,
197+
const ignition::math::Vector3d &_posePrim,
198+
const ignition::math::Quaterniond &_qPrim)
199+
{
200+
pxr::UsdPrim prim = stage->GetPrimAtPath(pxr::SdfPath(_path));
201+
ASSERT_TRUE(prim);
202+
203+
ignition::math::Pose3d pose;
204+
ignition::math::Vector3d scale{1, 1, 1};
205+
206+
sdf::usd::GetTransform(prim, usdData, pose, scale, "/transforms");
207+
208+
EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), scale);
209+
EXPECT_EQ(ignition::math::Pose3d(_posePrim, _qPrim), pose);
210+
};
211+
212+
verifyNestedTf(
213+
"/transforms/nested_transforms_XYZ/child_transform",
214+
ignition::math::Vector3d(0.01, 0.01, 0),
215+
ignition::math::Quaterniond(0, 0, IGN_DTOR(90)));
216+
verifyNestedTf(
217+
"/transforms/nested_transforms_ZYX/child_transform",
218+
ignition::math::Vector3d(0.02, 0.0, 0),
219+
ignition::math::Quaterniond(IGN_DTOR(90), 0, 0));
220+
}
221+
}

0 commit comments

Comments
 (0)
Please sign in to comment.