Skip to content

Commit 9c98e12

Browse files
adlarkinadlarkin
committed
review feedback, fix build
Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>
1 parent 758cd19 commit 9c98e12

3 files changed

Lines changed: 44 additions & 35 deletions

File tree

‎usd/src/usd_parser/USDJoints.cc‎

Lines changed: 21 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
/*
2-
* Copyright (C) 2021 Open Source Robotics Foundation
2+
* Copyright (C) 2022 Open Source Robotics Foundation
33
*
44
* Licensed under the Apache License, Version 2.0 (the "License");
55
* you may not use this file except in compliance with the License.
@@ -29,29 +29,27 @@
2929
#include <ignition/common/Util.hh>
3030

3131
#include "sdf/usd/usd_parser/USDData.hh"
32-
3332
#include "sdf/Console.hh"
3433
#include "sdf/Joint.hh"
3534
#include "sdf/JointAxis.hh"
3635

3736
namespace sdf
3837
{
39-
// Inline bracke to help doxygen filtering.
38+
// Inline bracket to help doxygen filtering.
4039
inline namespace SDF_VERSION_NAMESPACE {
4140
//
4241
namespace usd
4342
{
4443
sdf::Joint ParseJoints(
4544
const pxr::UsdPrim &_prim,
4645
const std::string &_path,
47-
USDData &_usdData)
46+
const USDData &_usdData)
4847
{
4948
sdf::Joint joint;
5049

51-
std::pair<std::string, std::shared_ptr<USDStage>> USDData =
50+
std::pair<std::string, std::shared_ptr<USDStage>> usdData =
5251
_usdData.FindStage(_prim.GetPath().GetName());
53-
54-
double metersPerUnit = USDData.second->MetersPerUnit();
52+
double metersPerUnit = usdData.second->MetersPerUnit();
5553

5654
pxr::SdfPathVector body0, body1;
5755

@@ -67,14 +65,11 @@ namespace sdf
6765
joint.SetChildLinkName(ignition::common::basename(
6866
body1[0].GetString()));
6967
}
70-
else
68+
else if (body0.size() > 0)
7169
{
72-
if (body0.size() > 0)
73-
{
74-
joint.SetParentLinkName("world");
75-
joint.SetChildLinkName(ignition::common::basename(
76-
body0[0].GetString()));
77-
}
70+
joint.SetParentLinkName("world");
71+
joint.SetChildLinkName(ignition::common::basename(
72+
body0[0].GetString()));
7873
}
7974

8075
if (body0.size() > 0 && joint.ParentLinkName().empty())
@@ -116,26 +111,28 @@ namespace sdf
116111
{
117112
pxr::UsdPhysicsRevoluteJoint(_prim).GetAxisAttr().Get(&axis);
118113
}
114+
119115
if (axis == pxr::UsdGeomTokens->x)
120116
{
121117
axisVector = ignition::math::Vector3d(1, 0, 0);
122118
}
123-
if (axis == pxr::UsdGeomTokens->y)
119+
else if (axis == pxr::UsdGeomTokens->y)
124120
{
125121
axisVector = ignition::math::Vector3d(0, 1, 0);
126122
}
127-
if (axis == pxr::UsdGeomTokens->z)
123+
else if (axis == pxr::UsdGeomTokens->z)
128124
{
129125
axisVector = ignition::math::Vector3d(0, 0, 1);
130126
}
131127

132128
pxr::GfVec3f localPose0, localPose1;
133129
pxr::GfQuatf localRot0, localRot1;
134130

135-
pxr::UsdPhysicsJoint(_prim).GetLocalPos0Attr().Get(&localPose0);
136-
pxr::UsdPhysicsJoint(_prim).GetLocalPos1Attr().Get(&localPose1);
137-
pxr::UsdPhysicsJoint(_prim).GetLocalRot0Attr().Get(&localRot0);
138-
pxr::UsdPhysicsJoint(_prim).GetLocalRot1Attr().Get(&localRot1);
131+
const auto usdPhysicsJoint = pxr::UsdPhysicsJoint(_prim);
132+
usdPhysicsJoint.GetLocalPos0Attr().Get(&localPose0);
133+
usdPhysicsJoint.GetLocalPos1Attr().Get(&localPose1);
134+
usdPhysicsJoint.GetLocalRot0Attr().Get(&localRot0);
135+
usdPhysicsJoint.GetLocalRot1Attr().Get(&localRot1);
139136

140137
trans = (localPose0 + localPose1) * metersPerUnit;
141138

@@ -223,9 +220,9 @@ namespace sdf
223220
ignition::math::Vector3d(trans[0], trans[1], trans[2]),
224221
ignition::math::Quaterniond(q1 * q2)));
225222

226-
joint.SetAxis(0, jointAxis);
227223
jointAxis.SetLower(lowerLimit * metersPerUnit);
228224
jointAxis.SetUpper(upperLimit * metersPerUnit);
225+
joint.SetAxis(0, jointAxis);
229226

230227
return joint;
231228
}
@@ -244,14 +241,13 @@ namespace sdf
244241
std::cerr << e << "\n";
245242
}
246243

247-
joint.SetRawPose(
248-
ignition::math::Pose3d(
244+
joint.SetRawPose(ignition::math::Pose3d(
249245
ignition::math::Vector3d(trans[0], trans[1], trans[2]),
250-
ignition::math::Quaterniond(q1)));
246+
q1));
251247

252-
joint.SetAxis(0, jointAxis);
253248
jointAxis.SetLower(IGN_DTOR(lowerLimit));
254249
jointAxis.SetUpper(IGN_DTOR(upperLimit));
250+
joint.SetAxis(0, jointAxis);
255251

256252
return joint;
257253
}

‎usd/src/usd_parser/USDJoints.hh‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
/*
2-
* Copyright (C) 2021 Open Source Robotics Foundation
2+
* Copyright (C) 2022 Open Source Robotics Foundation
33
*
44
* Licensed under the Apache License, Version 2.0 (the "License");
55
* you may not use this file except in compliance with the License.
@@ -24,23 +24,24 @@
2424
#pragma pop_macro ("__DEPRECATED")
2525

2626
#include "sdf/Joint.hh"
27-
2827
#include "sdf/usd/usd_parser/USDData.hh"
29-
3028
#include "sdf/config.hh"
31-
#include "sdf/usd/Export.hh"
3229

3330
namespace sdf
3431
{
35-
// Inline bracke to help doxygen filtering.
32+
// Inline bracket to help doxygen filtering.
3633
inline namespace SDF_VERSION_NAMESPACE {
3734
//
3835
namespace usd
3936
{
37+
/// \brief Parse a USD joint to its SDF representation
38+
/// \param[in] _prim The USD prim that holds the USD joint
39+
/// \param[in] _path The path to _prim
40+
/// \param[in] _usdData Object that holds data about the USD stage
4041
sdf::Joint ParseJoints(
4142
const pxr::UsdPrim &_prim,
4243
const std::string &_path,
43-
USDData &_usdData);
44+
const USDData &_usdData);
4445
}
4546
}
4647
}

‎usd/src/usd_parser/USDWorld.cc‎

Lines changed: 16 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -187,6 +187,20 @@ namespace usd
187187
}
188188
// TODO(anyone) support converting other USD light types
189189

190+
sdf::Model *modelPtr = nullptr;
191+
if (!currentModelName.empty())
192+
{
193+
modelPtr = _world.ModelByName(currentModelName);
194+
if (!modelPtr)
195+
{
196+
errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_PARSING_ERROR,
197+
"Unable to find a sdf::Model named [" + currentModelName +
198+
"] in world named [" + _world.Name() +
199+
"], but a sdf::Model with this name should exist."));
200+
return errors;
201+
}
202+
}
203+
190204
if (prim.IsA<pxr::UsdPhysicsJoint>())
191205
{
192206
sdf::Joint joint =
@@ -217,13 +231,11 @@ namespace usd
217231
continue;
218232
}
219233

220-
auto modelPtr = _world.ModelByName(currentModelName);
221234
if (!modelPtr)
222235
{
223236
errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_PARSING_ERROR,
224-
"Unable to find a sdf::Model named [" + currentModelName +
225-
"] in world named [" + _world.Name() +
226-
"], but a sdf::Model with this name should exist."));
237+
"Unable to parse link named [" + linkName +
238+
"] because the corresponding sdf::Model object wasn't found."));
227239
return errors;
228240
}
229241

0 commit comments

Comments
 (0)