Skip to content

Commit 412ee5b

Browse files
ahcordeahcorde
committed
fixed rotations and test
Signed-off-by: ahcorde <ahcorde@gmail.com>
1 parent 3265bd6 commit 412ee5b

2 files changed

Lines changed: 39 additions & 19 deletions

File tree

‎usd/src/usd_parser/USDTransforms.cc‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -250,14 +250,15 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim)
250250
ignition::math::Angle angleX(IGN_DTOR(rotationEuler[0]));
251251
ignition::math::Angle angleY(IGN_DTOR(rotationEuler[1]));
252252
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+
253257
if (op == kXFormOpRotateZYX)
254258
{
255259
std::swap(angleX, angleZ);
256260
}
257-
t.SetRotation(ignition::math::Quaterniond(
258-
angleX.Normalized().Radian(),
259-
angleY.Normalized().Radian(),
260-
angleZ.Normalized().Radian()));
261+
t.SetRotation((qX * qY) * qZ);
261262
}
262263
else if (op == kXFormOpTranslate)
263264
{

‎usd/src/usd_parser/USDTransforms_TEST.cc‎

Lines changed: 34 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,15 @@ void checkTransforms(
5555
if (_rotation)
5656
{
5757
ASSERT_TRUE(usdTransforms.Rotation());
58-
EXPECT_EQ(_rotation, 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));
5967
}
6068
}
6169

@@ -94,31 +102,34 @@ TEST(USDTransformsTest, ParseUSDTransform)
94102
"/shapes/sphere",
95103
stage,
96104
ignition::math::Vector3d(0, 1.5, 0.5),
97-
ignition::math::Quaterniond(IGN_DTOR(-62), IGN_DTOR(31), IGN_DTOR(-69)),
105+
ignition::math::Quaterniond(
106+
IGN_DTOR(-62), IGN_DTOR(-47.5), IGN_DTOR(-53.41)),
98107
ignition::math::Vector3d(1, 1, 1)
99108
);
100109

101110
checkTransforms(
102111
"/shapes/capsule",
103112
stage,
104113
ignition::math::Vector3d(0, -3.0, 0.5),
105-
ignition::math::Quaterniond(IGN_DTOR(-55), IGN_DTOR(80), IGN_DTOR(15)),
114+
ignition::math::Quaterniond(
115+
IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2)),
106116
ignition::math::Vector3d(1, 1, 1)
107117
);
108118

109119
checkTransforms(
110120
"/shapes/capsule/capsule_link/capsule_visual",
111121
stage,
112122
ignition::math::Vector3d(0, 0, 0),
113-
ignition::math::Quaterniond(M_PI_2, 0, 0),
123+
ignition::math::Quaterniond(0, 0, M_PI_2),
114124
ignition::math::Vector3d(1, 1, 1)
115125
);
116126

117127
checkTransforms(
118128
"/shapes/ellipsoid",
119129
stage,
120130
ignition::math::Vector3d(0, 3.0, 0.5),
121-
ignition::math::Quaterniond(IGN_DTOR(15), IGN_DTOR(80), IGN_DTOR(-55)),
131+
ignition::math::Quaterniond(
132+
IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2)),
122133
ignition::math::Vector3d(1, 1, 1)
123134
);
124135

@@ -164,7 +175,8 @@ TEST(USDTransformsTest, GetAllTransform)
164175
EXPECT_EQ(
165176
ignition::math::Pose3d(
166177
ignition::math::Vector3d(0, 0.03, 0.005),
167-
ignition::math::Quaterniond(IGN_DTOR(15), IGN_DTOR(80), IGN_DTOR(-55))),
178+
ignition::math::Quaterniond(
179+
IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2))),
168180
pose);
169181
}
170182

@@ -177,8 +189,13 @@ TEST(USDTransformsTest, GetAllTransform)
177189
sdf::usd::USDData usdData(filename);
178190
usdData.Init();
179191

180-
std::function<void(const std::string &)> verifyNestedTf =
181-
[&](const std::string &_path)
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)
182199
{
183200
pxr::UsdPrim prim = stage->GetPrimAtPath(pxr::SdfPath(_path));
184201
ASSERT_TRUE(prim);
@@ -189,14 +206,16 @@ TEST(USDTransformsTest, GetAllTransform)
189206
sdf::usd::GetTransform(prim, usdData, pose, scale, "/transforms");
190207

191208
EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), scale);
192-
EXPECT_EQ(
193-
ignition::math::Pose3d(
194-
ignition::math::Vector3d(.01, .01, 0),
195-
ignition::math::Quaterniond(0, 0, IGN_DTOR(90))),
196-
pose);
209+
EXPECT_EQ(ignition::math::Pose3d(_posePrim, _qPrim), pose);
197210
};
198211

199-
verifyNestedTf("/transforms/nested_transforms_XYZ/child_transform");
200-
verifyNestedTf("/transforms/nested_transforms_ZYX/child_transform");
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));
201220
}
202221
}

0 commit comments

Comments
 (0)