Skip to content

Commit 68a9458

Browse files
adlarkinadlarkin
committed
add tests for ball and prismatic joints
Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>
1 parent 05d43ad commit 68a9458

File tree

2 files changed

+162
-72
lines changed

2 files changed

+162
-72
lines changed

‎test/sdf/ball_prismatic_joint.sdf

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
<?xml version="1.0" ?>
2+
<sdf version="1.6">
3+
<model name="test">
4+
<link name="link1">
5+
</link>
6+
<link name="link2">
7+
</link>
8+
<link name="link3">
9+
</link>
10+
<link name="link4">
11+
</link>
12+
13+
<joint name="ball_joint" type="ball">
14+
<pose>0 1 0 0 0 0</pose>
15+
<child>link1</child>
16+
<parent>link2</parent>
17+
</joint>
18+
<joint name="prismatic_joint" type="prismatic">
19+
<pose>0 0 0 0 0 1</pose>
20+
<child>link3</child>
21+
<parent>link4</parent>
22+
<axis>
23+
<xyz>0 0 1</xyz>
24+
</axis>
25+
</joint>
26+
27+
</model>
28+
</sdf>

‎usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc

+134-72
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,9 @@
4040
#include <pxr/usd/usdPhysics/driveAPI.h>
4141
#include <pxr/usd/usdPhysics/fixedJoint.h>
4242
#include <pxr/usd/usdPhysics/joint.h>
43+
#include <pxr/usd/usdPhysics/prismaticJoint.h>
4344
#include <pxr/usd/usdPhysics/revoluteJoint.h>
45+
#include <pxr/usd/usdPhysics/sphericalJoint.h>
4446
#pragma pop_macro ("__DEPRECATED")
4547

4648
#include "sdf/Joint.hh"
@@ -55,14 +57,42 @@
5557
// Fixture that creates a USD stage for each test case.
5658
class UsdJointStageFixture : public::testing::Test
5759
{
58-
public: UsdJointStageFixture() = default;
60+
public: UsdJointStageFixture() :
61+
worldPath("/world")
62+
{
63+
}
5964

6065
protected: void SetUp() override
6166
{
6267
this->stage = pxr::UsdStage::CreateInMemory();
6368
ASSERT_TRUE(this->stage);
6469
}
6570

71+
/// \brief Parse the contents of a SDF file and convert to USD
72+
/// \param[in] _sdfFile The full path to the SDF file to parse
73+
public: void GenerateUSD(const std::string &_sdfFile)
74+
{
75+
// load the world in the SDF file
76+
ASSERT_TRUE(sdf::testing::LoadSdfFile(_sdfFile, this->root));
77+
this->model = const_cast<sdf::Model *>(this->root.Model());
78+
ASSERT_NE(nullptr, this->model);
79+
80+
this->modelPath =
81+
std::string(this->worldPath.GetString() + "/" + this->model->Name());
82+
const auto errors = sdf::usd::ParseSdfModel(*(this->model), this->stage,
83+
this->modelPath, this->worldPath);
84+
EXPECT_TRUE(errors.empty());
85+
86+
// save the model's USD joint paths so that they can be verified
87+
for (uint64_t i = 0; i < this->model->JointCount(); ++i)
88+
{
89+
const auto joint = this->model->JointByIndex(i);
90+
const auto jointPath = this->modelPath + "/" + joint->Name();
91+
this->jointPathToSdf[jointPath] = joint;
92+
}
93+
EXPECT_EQ(this->model->JointCount(), this->jointPathToSdf.size());
94+
}
95+
6696
/// \brief Verify that a USD joint is pointing to the correct parent link.
6797
/// \param[in] _usdJoint The USD joint
6898
/// \param[in] _parentLinkPath The parent link path that _usdJoint should
@@ -194,39 +224,32 @@ class UsdJointStageFixture : public::testing::Test
194224
EXPECT_FLOAT_EQ(usdUpperLimit, _targetUpper);
195225
}
196226

227+
/// \brief The USD stage
197228
public: pxr::UsdStageRefPtr stage;
229+
230+
/// \brief The SDF model with joints to be parsed to USD
231+
public: sdf::Model *model{nullptr};
232+
233+
/// \brief The USD path of the SDF world
234+
public: const pxr::SdfPath worldPath;
235+
236+
/// \brief The string representation of this->model's USD path
237+
public: std::string modelPath;
238+
239+
/// \brief Mapping of a joint's USD path to the corresponding SDF joint
240+
public: std::unordered_map<std::string, const sdf::Joint *> jointPathToSdf;
241+
242+
/// \brief The root object of the SDF file that has been loaded. A reference
243+
/// to the root must be kept so that pointer objects extracted from the root
244+
/// (sdf::Model and sdf::Joint, for example) remain valid throughout the test
245+
/// (destroying the sdf::Root object early invalidates referenced pointers)
246+
private: sdf::Root root;
198247
};
199248

200249
/////////////////////////////////////////////////
201250
TEST_F(UsdJointStageFixture, RevoluteJoints)
202251
{
203-
const auto path = sdf::testing::TestFile("sdf", "double_pendulum.sdf");
204-
sdf::Root root;
205-
206-
// load the world in the SDF file
207-
ASSERT_TRUE(sdf::testing::LoadSdfFile(path, root));
208-
const auto model = root.Model();
209-
ASSERT_NE(nullptr, model);
210-
211-
// create a dummy world path so that we can call the sdf::usd::ParseSdfModel
212-
// API
213-
const auto worldPath = pxr::SdfPath("/world");
214-
215-
const auto modelPath =
216-
std::string(worldPath.GetString() + "/" + model->Name());
217-
const auto errors =
218-
sdf::usd::ParseSdfModel(*model, this->stage, modelPath, worldPath);
219-
EXPECT_TRUE(errors.empty());
220-
221-
// save the model's USD joint paths so that they can be verified
222-
std::unordered_map<std::string, const sdf::Joint *> jointPathToSdf;
223-
for (uint64_t i = 0; i < model->JointCount(); ++i)
224-
{
225-
const auto joint = model->JointByIndex(i);
226-
const auto jointPath = modelPath + "/" + joint->Name();
227-
jointPathToSdf[jointPath] = joint;
228-
}
229-
EXPECT_EQ(model->JointCount(), jointPathToSdf.size());
252+
this->GenerateUSD(sdf::testing::TestFile("sdf", "double_pendulum.sdf"));
230253

231254
// validate USD joints
232255
int checkedJoints = 0;
@@ -235,8 +258,8 @@ TEST_F(UsdJointStageFixture, RevoluteJoints)
235258
if (!prim.IsA<pxr::UsdPhysicsJoint>())
236259
continue;
237260

238-
auto iter = jointPathToSdf.find(prim.GetPath().GetString());
239-
ASSERT_NE(jointPathToSdf.end(), iter);
261+
auto iter = this->jointPathToSdf.find(prim.GetPath().GetString());
262+
ASSERT_NE(this->jointPathToSdf.end(), iter);
240263
const auto sdfJoint = iter->second;
241264

242265
// the double pendulum model only has revolute joints
@@ -247,9 +270,9 @@ TEST_F(UsdJointStageFixture, RevoluteJoints)
247270

248271
// make sure joint is pointing to the proper parent/child links
249272
this->CheckParentLinkPath(&usdRevoluteJoint,
250-
modelPath + "/" + sdfJoint->ParentLinkName());
273+
this->modelPath + "/" + sdfJoint->ParentLinkName());
251274
this->CheckChildLinkPath(&usdRevoluteJoint,
252-
modelPath + "/" + sdfJoint->ChildLinkName());
275+
this->modelPath + "/" + sdfJoint->ChildLinkName());
253276

254277
// check joint's pose w.r.t. parent and child links
255278
ignition::math::Pose3d parentToJointPose;
@@ -315,33 +338,7 @@ TEST_F(UsdJointStageFixture, RevoluteJoints)
315338
/////////////////////////////////////////////////
316339
TEST_F(UsdJointStageFixture, JointParentIsWorld)
317340
{
318-
const auto path = sdf::testing::TestFile("sdf", "joint_parent_world.sdf");
319-
sdf::Root root;
320-
321-
// load the world in the SDF file
322-
ASSERT_TRUE(sdf::testing::LoadSdfFile(path, root));
323-
const auto model = root.Model();
324-
ASSERT_NE(nullptr, model);
325-
326-
// create a dummy world path so that we can call the sdf::usd::ParseSdfModel
327-
// API
328-
const auto worldPath = pxr::SdfPath("/world");
329-
330-
const auto modelPath =
331-
std::string(worldPath.GetString() + "/" + model->Name());
332-
const auto errors =
333-
sdf::usd::ParseSdfModel(*model, this->stage, modelPath, worldPath);
334-
EXPECT_TRUE(errors.empty());
335-
336-
// save the model's USD joint paths so that they can be verified
337-
std::unordered_map<std::string, const sdf::Joint *> jointPathToSdf;
338-
for (uint64_t i = 0; i < model->JointCount(); ++i)
339-
{
340-
const auto joint = model->JointByIndex(i);
341-
const auto jointPath = modelPath + "/" + joint->Name();
342-
jointPathToSdf[jointPath] = joint;
343-
}
344-
EXPECT_EQ(model->JointCount(), jointPathToSdf.size());
341+
this->GenerateUSD(sdf::testing::TestFile("sdf", "joint_parent_world.sdf"));
345342

346343
// validate USD joints
347344
int checkedJoints = 0;
@@ -350,8 +347,8 @@ TEST_F(UsdJointStageFixture, JointParentIsWorld)
350347
if (!prim.IsA<pxr::UsdPhysicsJoint>())
351348
continue;
352349

353-
auto iter = jointPathToSdf.find(prim.GetPath().GetString());
354-
ASSERT_NE(jointPathToSdf.end(), iter);
350+
auto iter = this->jointPathToSdf.find(prim.GetPath().GetString());
351+
ASSERT_NE(this->jointPathToSdf.end(), iter);
355352
const auto sdfJoint = iter->second;
356353

357354
// the only joint type in this test file is a fixed joint
@@ -362,9 +359,9 @@ TEST_F(UsdJointStageFixture, JointParentIsWorld)
362359

363360
// make sure joint is pointing to the proper parent/child links.
364361
// The parent in this test should be the world
365-
this->CheckParentLinkPath(&usdFixedJoint, worldPath.GetString());
362+
this->CheckParentLinkPath(&usdFixedJoint, this->worldPath.GetString());
366363
this->CheckChildLinkPath(&usdFixedJoint,
367-
modelPath + "/" + sdfJoint->ChildLinkName());
364+
this->modelPath + "/" + sdfJoint->ChildLinkName());
368365

369366
// check joint's pose w.r.t. parent and child links. For this test case,
370367
// we need to get the joint pose w.r.t. the world
@@ -373,7 +370,7 @@ TEST_F(UsdJointStageFixture, JointParentIsWorld)
373370
EXPECT_TRUE(poseErrors.empty());
374371
poseErrors.clear();
375372
ignition::math::Pose3d worldToModelPose;
376-
poseErrors = model->SemanticPose().Resolve(worldToModelPose);
373+
poseErrors = this->model->SemanticPose().Resolve(worldToModelPose);
377374
const auto worldToJointPose = worldToModelPose * modelToJointPose;
378375
EXPECT_TRUE(poseErrors.empty());
379376
poseErrors.clear();
@@ -389,11 +386,76 @@ TEST_F(UsdJointStageFixture, JointParentIsWorld)
389386
EXPECT_EQ(checkedJoints, 1);
390387
}
391388

392-
// TODO(adlarkin) Add the following test cases:
393-
// 1. prismatic
394-
// - prismatic joints share a few things with revolute joints that need to
395-
// be checked: axisAttr, jointLimits
396-
// - parent/child link reference paths and pose w.r.t. parent/child links
397-
// 2. Revolute joint with the axis being "y"
398-
// - this is a special case; see the sdf::usd::SetUSDJointPose method in
399-
// usd/src/sdf_parser/Joint.cc for how this is handled
389+
/////////////////////////////////////////////////
390+
TEST_F(UsdJointStageFixture, BallPrismaticJoint)
391+
{
392+
this->GenerateUSD(sdf::testing::TestFile("sdf", "ball_prismatic_joint.sdf"));
393+
394+
// validate USD joints
395+
int checkedBallJoints = 0;
396+
int checkedPrismaticJoints = 0;
397+
for (const auto & prim : this->stage->Traverse())
398+
{
399+
if (!prim.IsA<pxr::UsdPhysicsJoint>())
400+
continue;
401+
402+
auto iter = this->jointPathToSdf.find(prim.GetPath().GetString());
403+
ASSERT_NE(this->jointPathToSdf.end(), iter);
404+
const auto sdfJoint = iter->second;
405+
406+
if (prim.IsA<pxr::UsdPhysicsSphericalJoint>())
407+
{
408+
checkedBallJoints++;
409+
}
410+
else if (prim.IsA<pxr::UsdPhysicsPrismaticJoint>())
411+
{
412+
checkedPrismaticJoints++;
413+
414+
const auto usdPrismaticJoint =
415+
pxr::UsdPhysicsPrismaticJoint::Get(this->stage, prim.GetPath());
416+
ASSERT_TRUE(usdPrismaticJoint);
417+
418+
// check the joint's axis
419+
this->VerifyJointAxis(usdPrismaticJoint, "Z");
420+
421+
// check the joint limits
422+
this->VerifyJointLimits(usdPrismaticJoint,
423+
static_cast<float>(sdfJoint->Axis()->Lower()),
424+
static_cast<float>(sdfJoint->Axis()->Upper()),
425+
false);
426+
}
427+
else
428+
{
429+
continue;
430+
}
431+
432+
const auto usdJoint =
433+
pxr::UsdPhysicsJoint::Get(this->stage, prim.GetPath());
434+
ASSERT_TRUE(usdJoint);
435+
436+
// make sure joint is pointing to the proper parent/child links
437+
this->CheckParentLinkPath(&usdJoint,
438+
this->modelPath + "/" + sdfJoint->ParentLinkName());
439+
this->CheckChildLinkPath(&usdJoint,
440+
this->modelPath + "/" + sdfJoint->ChildLinkName());
441+
442+
// check joint's pose w.r.t. parent and child links
443+
ignition::math::Pose3d parentToJointPose;
444+
auto poseErrors = sdfJoint->SemanticPose().Resolve(parentToJointPose,
445+
sdfJoint->ParentLinkName());
446+
EXPECT_TRUE(poseErrors.empty());
447+
poseErrors.clear();
448+
ignition::math::Pose3d childToJointPose;
449+
poseErrors = sdfJoint->SemanticPose().Resolve(childToJointPose,
450+
sdfJoint->ChildLinkName());
451+
EXPECT_TRUE(poseErrors.empty());
452+
this->CheckRelativeLinkPoses(&usdJoint, parentToJointPose,
453+
childToJointPose);
454+
}
455+
EXPECT_EQ(checkedBallJoints, 1);
456+
EXPECT_EQ(checkedPrismaticJoints, 1);
457+
}
458+
459+
// TODO(adlarkin) Add a test case for a revolute joint with the axis being "y".
460+
// This is a special case; see the sdf::usd::SetUSDJointPose method in
461+
// usd/src/sdf_parser/Joint.cc for how this is handled

0 commit comments

Comments
 (0)