From 5e3605fd57327440cd96b0968e105ce7235b7282 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Thu, 24 Feb 2022 21:59:48 -0700 Subject: [PATCH] sdf -> usd converter: add joints (#837) Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Co-authored-by: ahcorde --- test/integration/joint_dom.cc | 2 +- test/sdf/ball_prismatic_joint.sdf | 28 ++ test/sdf/double_pendulum.sdf | 12 + test/sdf/joint_parent_world.sdf | 1 + usd/include/sdf/usd/sdf_parser/Joint.hh | 72 ++++ usd/src/CMakeLists.txt | 2 + usd/src/sdf_parser/Joint.cc | 403 ++++++++++++++++++++ usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc | 460 +++++++++++++++++++++++ usd/src/sdf_parser/Model.cc | 20 +- 9 files changed, 998 insertions(+), 2 deletions(-) create mode 100644 test/sdf/ball_prismatic_joint.sdf create mode 100644 usd/include/sdf/usd/sdf_parser/Joint.hh create mode 100644 usd/src/sdf_parser/Joint.cc create mode 100644 usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index 986e0034d..0050d6442 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -176,7 +176,7 @@ TEST(DOMJoint, LoadJointParentWorld) EXPECT_EQ(1u, model->LinkCount()); EXPECT_NE(nullptr, model->LinkByIndex(0)); EXPECT_EQ(nullptr, model->LinkByIndex(1)); - EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose()); + EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), model->RawPose()); EXPECT_EQ("", model->PoseRelativeTo()); ASSERT_TRUE(model->LinkNameExists("link")); diff --git a/test/sdf/ball_prismatic_joint.sdf b/test/sdf/ball_prismatic_joint.sdf new file mode 100644 index 000000000..357aeec57 --- /dev/null +++ b/test/sdf/ball_prismatic_joint.sdf @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + 0 1 0 0 0 0 + link1 + link2 + + + 0 0 0 0 0 1 + link3 + link4 + + 0 0 1 + + + + + diff --git a/test/sdf/double_pendulum.sdf b/test/sdf/double_pendulum.sdf index fd3d3b102..b6339f6e3 100644 --- a/test/sdf/double_pendulum.sdf +++ b/test/sdf/double_pendulum.sdf @@ -197,6 +197,12 @@ upper_link 1.0 0 0 + + 35 + + + 350 + @@ -205,6 +211,12 @@ lower_link 1.0 0 0 + + 35 + + + 350 + diff --git a/test/sdf/joint_parent_world.sdf b/test/sdf/joint_parent_world.sdf index 85142e454..0ebb67202 100644 --- a/test/sdf/joint_parent_world.sdf +++ b/test/sdf/joint_parent_world.sdf @@ -1,6 +1,7 @@ + 1 0 0 0 0 0 0 0 1 0 0 0 diff --git a/usd/include/sdf/usd/sdf_parser/Joint.hh b/usd/include/sdf/usd/sdf_parser/Joint.hh new file mode 100644 index 000000000..e452d6c51 --- /dev/null +++ b/usd/include/sdf/usd/sdf_parser/Joint.hh @@ -0,0 +1,72 @@ +/* + * 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_SDF_PARSER_JOINT_HH_ +#define SDF_USD_SDF_PARSER_JOINT_HH_ + +#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 +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Joint.hh" +#include "sdf/Model.hh" +#include "sdf/config.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracke to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse a SDF joint into a USD stage. + /// \param[in] _joint The SDF joint to parse. + /// \param[in] _stage The stage that should contain the USD representation + /// of _joint. This must be a valid, initialized stage. + /// \param[in] _path The USD path of the parsed joint in _stage, which must + /// be a valid USD path. + /// \param[in] _parentModel The model that is the parent of _joint + /// \param[in] _linkToUsdPath a map of a link's SDF name to the link's USD + /// path. This is used to determine which USD prims should be assigned as + /// the USD joint's relative links. + /// \param[in] _worldPath The USD path of the world prim. This is needed if + /// _joint's parent is the world. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no errors + /// occurred when parsing _joint to its USD representation. + UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfJoint( + const sdf::Joint &_joint, + pxr::UsdStageRefPtr &_stage, const std::string &_path, + const sdf::Model &_parentModel, + const std::unordered_map &_linkToUsdPath, + const pxr::SdfPath &_worldPath); + } + } +} + +#endif diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index ecf850406..0623359d4 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -1,6 +1,7 @@ set(sources UsdError.cc sdf_parser/Geometry.cc + sdf_parser/Joint.cc sdf_parser/Light.cc sdf_parser/Link.cc sdf_parser/Model.cc @@ -25,6 +26,7 @@ target_link_libraries(${usd_target} set(gtest_sources sdf_parser/sdf2usd_TEST.cc sdf_parser/Geometry_Sdf2Usd_TEST.cc + sdf_parser/Joint_Sdf2Usd_TEST.cc sdf_parser/Light_Sdf2Usd_TEST.cc sdf_parser/Link_Sdf2Usd_TEST.cc # TODO(adlarkin) add a test for SDF -> USD models once model parsing diff --git a/usd/src/sdf_parser/Joint.cc b/usd/src/sdf_parser/Joint.cc new file mode 100644 index 000000000..62a6b0ef6 --- /dev/null +++ b/usd/src/sdf_parser/Joint.cc @@ -0,0 +1,403 @@ +/* + * 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/sdf_parser/Joint.hh" + +#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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Error.hh" +#include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" +#include "sdf/Link.hh" +#include "sdf/Model.hh" +#include "../UsdUtils.hh" + +namespace sdf +{ +// Inline bracke to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + /// \brief Helper function for setting a USD joint's pose relative to the + /// joint's parent and child. + /// \param[in] _jointPrim The USD joint prim + /// \param[in] _joint The SDF representation of _jointPrim + /// \param[in] _parentModel The SDF model that is the parent of _joint + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when setting _joint's pose relative to _joint's parent and child + UsdErrors SetUSDJointPose(pxr::UsdPhysicsJoint &_jointPrim, + const sdf::Joint &_joint, const sdf::Model &_parentModel) + { + UsdErrors errors; + + ignition::math::Pose3d parentToJoint; + if (_joint.ParentLinkName() == "world") + { + ignition::math::Pose3d modelToJoint; + auto poseErrors = usd::PoseWrtParent(_joint, modelToJoint); + if (!poseErrors.empty()) + return poseErrors; + + // it is assumed the _parentModel's parent is the world because nested + // models are not yet supported (see issue #845) + ignition::math::Pose3d worldToModel; + poseErrors = usd::PoseWrtParent(_parentModel, worldToModel); + if (!poseErrors.empty()) + return poseErrors; + + parentToJoint = worldToModel * modelToJoint; + } + else + { + auto poseResolutionErrors = + _joint.SemanticPose().Resolve(parentToJoint, _joint.ParentLinkName()); + if (!poseResolutionErrors.empty()) + { + errors.push_back(UsdError( + sdf::Error(sdf::ErrorCode::POSE_RELATIVE_TO_INVALID, + "Unable to get the pose of joint [" + _joint.Name() + + "] w.r.t. its parent link [" + _joint.ParentLinkName() + "]."))); + for (const auto &e : poseResolutionErrors) + errors.push_back(UsdError(e)); + return errors; + } + } + + ignition::math::Pose3d childToJoint; + auto poseResolutionErrors = _joint.SemanticPose().Resolve(childToJoint, + _joint.ChildLinkName()); + if (!poseResolutionErrors.empty()) + { + errors.push_back(UsdError( + sdf::Error(sdf::ErrorCode::POSE_RELATIVE_TO_INVALID, + "Unable to get the pose of joint [" + _joint.Name() + + "] w.r.t. its child [" + _joint.ChildLinkName() + "]."))); + for (const auto &e : poseResolutionErrors) + errors.push_back(UsdError(e)); + return errors; + } + + auto parentRotation = pxr::GfQuatf( + parentToJoint.Rot().W(), + parentToJoint.Rot().X(), + parentToJoint.Rot().Y(), + parentToJoint.Rot().Z()); + + auto childRotation = pxr::GfQuatf( + childToJoint.Rot().W(), + childToJoint.Rot().X(), + childToJoint.Rot().Y(), + childToJoint.Rot().Z()); + + const auto axis = _joint.Axis(); + // TODO(anyone) Review this logic which converts a Y axis into a X axis. + if (axis && (axis->Xyz() == ignition::math::Vector3d::UnitY)) + { + if (auto jointRevolute = pxr::UsdPhysicsRevoluteJoint(_jointPrim)) + { + const ignition::math::Quaterniond fixRotation(0, 0, IGN_DTOR(90)); + ignition::math::Quaterniond parentRotationTmp = parentToJoint.Rot(); + ignition::math::Quaterniond childRotationTmp = childToJoint.Rot(); + + if (parentRotationTmp == ignition::math::Quaterniond::Identity) + { + parentRotationTmp = fixRotation * parentRotationTmp; + } + else + { + parentRotationTmp = ignition::math::Quaterniond(IGN_DTOR(-90), + IGN_PI, IGN_PI) * parentRotationTmp; + } + + childRotationTmp = fixRotation * childRotationTmp; + + parentRotation = pxr::GfQuatf( + parentRotationTmp.W(), + parentRotationTmp.X(), + parentRotationTmp.Y(), + parentRotationTmp.Z()); + childRotation = pxr::GfQuatf( + childRotationTmp.W(), + childRotationTmp.X(), + childRotationTmp.Y(), + childRotationTmp.Z()); + jointRevolute.CreateAxisAttr().Set(pxr::TfToken("X")); + } + } + + _jointPrim.CreateLocalPos0Attr().Set(pxr::GfVec3f( + parentToJoint.Pos().X(), + parentToJoint.Pos().Y(), + parentToJoint.Pos().Z())); + _jointPrim.CreateLocalRot0Attr().Set(parentRotation); + + _jointPrim.CreateLocalPos1Attr().Set(pxr::GfVec3f( + childToJoint.Pos().X(), + childToJoint.Pos().Y(), + childToJoint.Pos().Z())); + _jointPrim.CreateLocalRot1Attr().Set(childRotation); + + return errors; + } + + /// \brief Helper function to parse a SDF revolute joint to its USD + /// representation. + /// \param[in] _joint The SDF joint to parse + /// \param[in] _stage The stage that _joint should belong to. This must be a + /// valid, initialized stage. + /// \param[in] _path The path in _stage where the USD representation of _joint + /// will be defined. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when converting _joint to its USD representation. + UsdErrors ParseSdfRevoluteJoint(const sdf::Joint &_joint, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + + auto usdJoint = + pxr::UsdPhysicsRevoluteJoint::Define(_stage, pxr::SdfPath(_path)); + + const auto axis = _joint.Axis(); + + if (axis->Xyz() == ignition::math::Vector3d::UnitX || + axis->Xyz() == -ignition::math::Vector3d::UnitX) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("X")); + } + else if (axis->Xyz() == ignition::math::Vector3d::UnitY || + axis->Xyz() == -ignition::math::Vector3d::UnitY) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("Y")); + } + else if (axis->Xyz() == ignition::math::Vector3d::UnitZ || + axis->Xyz() == -ignition::math::Vector3d::UnitZ) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("Z")); + } + else + { + std::stringstream axisStr; + axisStr << axis->Xyz(); + errors.push_back(UsdError(sdf::Error(sdf::ErrorCode::ELEMENT_INVALID, + "Revolute joint [" + _joint.Name() + "] has specified an axis [" + + axisStr.str() + "], but USD only supports integer values of [0, 1] " + "when specifying joint axis unit vectors."))); + return errors; + } + + // Revolute joint limits in SDF are in radians, but USD expects degrees + // of C++ type float + auto sdfLimitDegrees = static_cast(IGN_RTOD(axis->Lower())); + usdJoint.CreateLowerLimitAttr().Set(sdfLimitDegrees); + sdfLimitDegrees = static_cast(IGN_RTOD(axis->Upper())); + usdJoint.CreateUpperLimitAttr().Set(sdfLimitDegrees); + + pxr::UsdPrim usdJointPrim = _stage->GetPrimAtPath(pxr::SdfPath(_path)); + + auto drive = + pxr::UsdPhysicsDriveAPI::Apply(usdJointPrim, pxr::TfToken("angular")); + if (!drive) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY, + "Internal error: unable to mark link at path [" + _path + + "] as a UsdPhysicsDriveAPI.")); + return errors; + } + + // TODO(ahcorde) Review damping and stiffness values + drive.CreateDampingAttr().Set(static_cast(axis->Damping())); + drive.CreateStiffnessAttr().Set(static_cast(axis->Stiffness())); + drive.CreateMaxForceAttr().Set(static_cast(axis->Effort())); + + return errors; + } + + /// \brief Helper function to parse a SDF prismatic joint to its USD + /// representation. + /// \param[in] _joint The SDF joint to parse + /// \param[in] _stage The stage that _joint should belong to. This must be a + /// valid, initialized stage. + /// \param[in] _path The path in _stage where the USD representation of _joint + /// will be defined. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when converting _joint to its USD representation. + UsdErrors ParseSdfPrismaticJoint(const sdf::Joint &_joint, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + + auto usdJoint = + pxr::UsdPhysicsPrismaticJoint::Define(_stage, pxr::SdfPath(_path)); + + const auto axis = _joint.Axis(); + + if (axis->Xyz() == ignition::math::Vector3d::UnitX || + axis->Xyz() == -ignition::math::Vector3d::UnitX) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("X")); + } + else if (axis->Xyz() == ignition::math::Vector3d::UnitY || + axis->Xyz() == -ignition::math::Vector3d::UnitY) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("Y")); + } + else if (axis->Xyz() == ignition::math::Vector3d::UnitZ || + axis->Xyz() == -ignition::math::Vector3d::UnitZ) + { + usdJoint.CreateAxisAttr().Set(pxr::TfToken("Z")); + } + else + { + std::stringstream axisStr; + axisStr << axis->Xyz(); + errors.push_back(UsdError(sdf::Error(sdf::ErrorCode::ELEMENT_INVALID, + "Prismatic joint [" + _joint.Name() + "] has specified an axis [" + + axisStr.str() + "], but USD only supports integer values of [0, 1] " + "when specifying joint axis unit vectors."))); + return errors; + } + + usdJoint.CreateLowerLimitAttr().Set( + static_cast(axis->Lower())); + usdJoint.CreateUpperLimitAttr().Set( + static_cast(axis->Upper())); + + return errors; + } + + UsdErrors ParseSdfJoint(const sdf::Joint &_joint, + pxr::UsdStageRefPtr &_stage, const std::string &_path, + const sdf::Model &_parentModel, + const std::unordered_map &_linkToUsdPath, + const pxr::SdfPath &_worldPath) + { + UsdErrors errors; + + // the joint's parent may be "world". If this is the case, the joint's + // parent should be set to the world prim, not a link + auto parentLinkPath = _worldPath; + if (_joint.ParentLinkName() != "world") + { + const auto it = _linkToUsdPath.find(_joint.ParentLinkName()); + if (it == _linkToUsdPath.end()) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Unable to find a USD path for link [" + _joint.ParentLinkName() + + "], which is the parent link of joint [" + _joint.Name() + "].")); + return errors; + } + parentLinkPath = it->second; + } + + const auto it = _linkToUsdPath.find(_joint.ChildLinkName()); + if (it == _linkToUsdPath.end()) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Unable to find a USD path for link [" + _joint.ParentLinkName() + + "], which is the child link of joint [" + _joint.Name() + "].")); + return errors; + } + const auto childLinkPath = it->second; + + UsdErrors parsingErrors; + switch (_joint.Type()) + { + case sdf::JointType::REVOLUTE: + parsingErrors = ParseSdfRevoluteJoint(_joint, _stage, _path); + break; + case sdf::JointType::BALL: + // While USD allows for cone limits that can restrict motion in a given + // range, SDF does not have limits for a ball joint. So, there's + // nothing to do after creating a UsdPhysicsSphericalJoint, since this + // joint by default has no limits (i.e., allows for circular motion) + // related issue https://github.com/ignitionrobotics/sdformat/issues/860 + pxr::UsdPhysicsSphericalJoint::Define(_stage, pxr::SdfPath(_path)); + break; + case sdf::JointType::FIXED: + pxr::UsdPhysicsFixedJoint::Define(_stage, pxr::SdfPath(_path)); + break; + case sdf::JointType::PRISMATIC: + parsingErrors = ParseSdfPrismaticJoint(_joint, _stage, _path); + break; + case sdf::JointType::CONTINUOUS: + case sdf::JointType::GEARBOX: + case sdf::JointType::REVOLUTE2: + case sdf::JointType::SCREW: + case sdf::JointType::UNIVERSAL: + case sdf::JointType::INVALID: + default: + parsingErrors.push_back(UsdError( + sdf::Error(sdf::ErrorCode::ATTRIBUTE_INVALID, + "Joint type is either invalid or not supported."))); + } + + if (!parsingErrors.empty()) + { + errors.insert(errors.end(), parsingErrors.begin(), parsingErrors.end()); + return errors; + } + + auto jointPrim = pxr::UsdPhysicsJoint::Get(_stage, pxr::SdfPath(_path)); + if (!jointPrim) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Internal error: unable to get prim at path [" + _path + + "], but a joint prim should exist at this path.")); + return errors; + } + + // define the joint's parent/child links + jointPrim.CreateBody0Rel().AddTarget(parentLinkPath); + jointPrim.CreateBody1Rel().AddTarget(childLinkPath); + + const auto poseErrors = + SetUSDJointPose(jointPrim, _joint, _parentModel); + errors.insert(errors.end(), poseErrors.begin(), poseErrors.end()); + + return errors; + } +} +} +} diff --git a/usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc new file mode 100644 index 000000000..8f44ccfcd --- /dev/null +++ b/usd/src/sdf_parser/Joint_Sdf2Usd_TEST.cc @@ -0,0 +1,460 @@ +/* + * Copyright 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 +#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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" +#include "sdf/Model.hh" +#include "sdf/Root.hh" +#include "sdf/usd/sdf_parser/Model.hh" +#include "test_config.h" +#include "test_utils.hh" + +///////////////////////////////////////////////// +// Fixture that creates a USD stage for each test case. +// This fixture also provides methods for performing joint validation checks. +class UsdJointStageFixture : public::testing::Test +{ + public: UsdJointStageFixture() : + worldPath("/world") + { + } + + protected: void SetUp() override + { + this->stage = pxr::UsdStage::CreateInMemory(); + ASSERT_TRUE(this->stage); + } + + /// \brief Parse the contents of a SDF file and convert to USD. + /// This method should be called at the beginning of a test case to set up + /// the USD contents that need to be checked/verified. + /// \param[in] _sdfFile The full path to the SDF file to parse + public: void GenerateUSD(const std::string &_sdfFile) + { + // load the world in the SDF file + ASSERT_TRUE(sdf::testing::LoadSdfFile(_sdfFile, this->root)); + this->model = const_cast(this->root.Model()); + ASSERT_NE(nullptr, this->model); + + this->modelPath = + std::string(this->worldPath.GetString() + "/" + this->model->Name()); + const auto errors = sdf::usd::ParseSdfModel(*(this->model), this->stage, + this->modelPath, this->worldPath); + EXPECT_TRUE(errors.empty()); + + // save the model's USD joint paths so that they can be verified + for (uint64_t i = 0; i < this->model->JointCount(); ++i) + { + const auto joint = this->model->JointByIndex(i); + const auto jointPath = this->modelPath + "/" + joint->Name(); + this->jointPathToSdf[jointPath] = joint; + } + EXPECT_EQ(this->model->JointCount(), this->jointPathToSdf.size()); + } + + /// \brief Verify that a USD joint is pointing to the correct parent link. + /// \param[in] _usdJoint The USD joint + /// \param[in] _parentLinkPath The parent link path that _usdJoint should + /// point to + public: void CheckParentLinkPath(const pxr::UsdPhysicsJoint *_usdJoint, + const std::string &_parentLinkPath) const + { + pxr::SdfPathVector jointTarget; + _usdJoint->GetBody0Rel().GetTargets(&jointTarget); + ASSERT_EQ(1u, jointTarget.size()); + EXPECT_EQ(_parentLinkPath, jointTarget[0].GetString()); + } + + /// \brief Verify that a USD joint is pointing to the correct child link. + /// \param[in] _usdJoint The USD joint + /// \param[in] _childLinkPath The child link path that _usdJoint should + /// point to + public: void CheckChildLinkPath(const pxr::UsdPhysicsJoint *_usdJoint, + const std::string &_childLinkPath) const + { + pxr::SdfPathVector jointTarget; + _usdJoint->GetBody1Rel().GetTargets(&jointTarget); + ASSERT_EQ(1u, jointTarget.size()); + EXPECT_EQ(_childLinkPath, jointTarget[0].GetString()); + } + + /// \brief Verify that a USD joint has a proper pose w.r.t. its parent and + /// child link. + /// \param[in] _usdJoint The USD joint + /// \param[in] _targetParentPose The pose _usdJoint should have w.r.t. its + /// parent link + /// \param[in] _targetChildPose The pose _usdJoint should have w.r.t. its + /// child link + public: void CheckRelativeLinkPoses(const pxr::UsdPhysicsJoint *_usdJoint, + const ignition::math::Pose3d &_targetParentPose, + const ignition::math::Pose3d &_targetChildPose) const + { + // helper function to compare USD position to ignition::math position + auto validatePos = + [](const pxr::GfVec3f &_usdPos, + const ignition::math::Vector3d &_targetPos) + { + EXPECT_FLOAT_EQ(_usdPos[0], static_cast(_targetPos.X())); + EXPECT_FLOAT_EQ(_usdPos[1], static_cast(_targetPos.Y())); + EXPECT_FLOAT_EQ(_usdPos[2], static_cast(_targetPos.Z())); + }; + + // helper function to compare USD rotation to ignition::math quaternion + auto validateRot = + [](const pxr::GfQuatf &_usdRot, + const ignition::math::Quaterniond &_targetRot) + { + EXPECT_FLOAT_EQ(_usdRot.GetReal(), + static_cast(_targetRot.W())); + EXPECT_FLOAT_EQ(_usdRot.GetImaginary()[0], + static_cast(_targetRot.X())); + EXPECT_FLOAT_EQ(_usdRot.GetImaginary()[1], + static_cast(_targetRot.Y())); + EXPECT_FLOAT_EQ(_usdRot.GetImaginary()[2], + static_cast(_targetRot.Z())); + }; + + pxr::GfVec3f usdPos; + pxr::GfQuatf usdRot; + + EXPECT_TRUE(_usdJoint->GetLocalPos0Attr().Get(&usdPos)); + validatePos(usdPos, _targetParentPose.Pos()); + + EXPECT_TRUE(_usdJoint->GetLocalRot0Attr().Get(&usdRot)); + validateRot(usdRot, _targetParentPose.Rot()); + + EXPECT_TRUE(_usdJoint->GetLocalPos1Attr().Get(&usdPos)); + validatePos(usdPos, _targetChildPose.Pos()); + + EXPECT_TRUE(_usdJoint->GetLocalRot1Attr().Get(&usdRot)); + validateRot(usdRot, _targetChildPose.Rot()); + } + + /// \brief Verify that a USD joint has a proper axis (this is not required + /// by all USD joint types, but is required by revolute and prismatic joints, + /// for example) + /// \param[in] _usdJoint The USD joint + /// \param[in] _targetAxis The axis _usdJoint should have. This should be + /// "X", "Y", or "Z" + /// \tparam JointTypeT A USD joint type that has a GetAxisAttr() method, which + /// returns a pxr::UsdAttribute that stores the axis in a pxr::TfToken object + public: template + void VerifyJointAxis(const JointTypeT &_usdJoint, + const std::string &_targetAxis) const + { + pxr::TfToken usdAxis; + EXPECT_TRUE(_usdJoint.GetAxisAttr().Get(&usdAxis)); + EXPECT_EQ(_targetAxis, usdAxis.GetString()); + } + + /// \brief Verify that a USD joint has the proper limits (this is not required + /// by all USD joint types, but is required by revolute and prismatic joints, + /// for example) + /// \param[in] _usdJoint The USD joint + /// \param[in] _targetLower The lower limit _usdJoint should have. For + /// revolute joints, USD interprets this as an angle, but for prismatic + /// joints, USD interprets this as a distance + /// \param[in] _targetUpper The upper limit _usdJoint should have. For + /// revolute joints, USD interprets this as an angle, but for prismatic + /// joints, USD interprets this as a distance + /// \param[in] _convertToDeg Whether _targetLower and _targetUpper need to be + /// converted to degrees (true) or not (false). If _usdJoint is a prismatic + /// joint, this value should be set to false since prismatic joint limits are + /// interpreted as a distance instead of an angle + /// \tparam JointTypeT A USD joint type that has GetLowerLimitAttr() and + /// GetUpperLimitAttr() methods. Both of these methods should return a + /// pxr::UsdAttribute that stores the limit as a float + public: template + void VerifyJointLimits(const JointTypeT &_usdJoint, + float _targetLower, float _targetUpper, bool _convertToDeg) const + { + if (_convertToDeg) + { + _targetLower = IGN_RTOD(_targetLower); + _targetUpper = IGN_RTOD(_targetUpper); + } + + float usdLowerLimit; + EXPECT_TRUE(_usdJoint.GetLowerLimitAttr().Get(&usdLowerLimit)); + EXPECT_FLOAT_EQ(usdLowerLimit, _targetLower); + + float usdUpperLimit; + EXPECT_TRUE(_usdJoint.GetUpperLimitAttr().Get(&usdUpperLimit)); + EXPECT_FLOAT_EQ(usdUpperLimit, _targetUpper); + } + + /// \brief The USD stage + public: pxr::UsdStageRefPtr stage; + + /// \brief The SDF model with joints to be parsed to USD + public: sdf::Model *model{nullptr}; + + /// \brief The USD path of the SDF world + public: const pxr::SdfPath worldPath; + + /// \brief The string representation of this->model's USD path + public: std::string modelPath; + + /// \brief Mapping of a joint's USD path to the corresponding SDF joint + public: std::unordered_map jointPathToSdf; + + /// \brief The root object of the SDF file that has been loaded. A reference + /// to the root must be kept so that pointer objects extracted from the root + /// (sdf::Model and sdf::Joint, for example) remain valid throughout the test + /// (destroying the sdf::Root object early invalidates referenced pointers) + private: sdf::Root root; +}; + +///////////////////////////////////////////////// +TEST_F(UsdJointStageFixture, RevoluteJoints) +{ + this->GenerateUSD(sdf::testing::TestFile("sdf", "double_pendulum.sdf")); + + // validate USD joints + int checkedJoints = 0; + for (const auto & prim : this->stage->Traverse()) + { + if (!prim.IsA()) + continue; + + auto iter = this->jointPathToSdf.find(prim.GetPath().GetString()); + ASSERT_NE(this->jointPathToSdf.end(), iter); + const auto sdfJoint = iter->second; + + // the double pendulum model only has revolute joints + EXPECT_TRUE(prim.IsA()); + const auto usdRevoluteJoint = + pxr::UsdPhysicsRevoluteJoint::Get(this->stage, prim.GetPath()); + ASSERT_TRUE(usdRevoluteJoint); + + // make sure joint is pointing to the proper parent/child links + this->CheckParentLinkPath(&usdRevoluteJoint, + this->modelPath + "/" + sdfJoint->ParentLinkName()); + this->CheckChildLinkPath(&usdRevoluteJoint, + this->modelPath + "/" + sdfJoint->ChildLinkName()); + + // check joint's pose w.r.t. parent and child links + ignition::math::Pose3d parentToJointPose; + auto poseErrors = sdfJoint->SemanticPose().Resolve(parentToJointPose, + sdfJoint->ParentLinkName()); + EXPECT_TRUE(poseErrors.empty()); + poseErrors.clear(); + ignition::math::Pose3d childToJointPose; + poseErrors = sdfJoint->SemanticPose().Resolve(childToJointPose, + sdfJoint->ChildLinkName()); + EXPECT_TRUE(poseErrors.empty()); + this->CheckRelativeLinkPoses(&usdRevoluteJoint, parentToJointPose, + childToJointPose); + + // check the joint's axis + this->VerifyJointAxis(usdRevoluteJoint, "X"); + + // check the joint limits + this->VerifyJointLimits(usdRevoluteJoint, + static_cast(sdfJoint->Axis()->Lower()), + static_cast(sdfJoint->Axis()->Upper()), + true); + + // revolute joints should have a UsdPhysicsDriveAPI + EXPECT_TRUE(prim.HasAPI(pxr::TfToken("angular"))); + const auto driveApiAttrPrefix = std::string("drive:angular:physics:"); + + // check damping + const auto dampingAttr = + prim.GetAttribute(pxr::TfToken(driveApiAttrPrefix + "damping")); + ASSERT_TRUE(dampingAttr); + float usdDamping; + EXPECT_TRUE(dampingAttr.Get(&usdDamping)); + EXPECT_FLOAT_EQ(usdDamping, + static_cast(sdfJoint->Axis()->Damping())); + + // check stiffness + const auto stiffnessAttr = + prim.GetAttribute(pxr::TfToken(driveApiAttrPrefix + "stiffness")); + ASSERT_TRUE(stiffnessAttr); + float usdStiffness; + EXPECT_TRUE(stiffnessAttr.Get(&usdStiffness)); + EXPECT_FLOAT_EQ(usdStiffness, + static_cast(sdfJoint->Axis()->Stiffness())); + + // check max force/effort + const auto maxForceAttr = + prim.GetAttribute(pxr::TfToken(driveApiAttrPrefix + "maxForce")); + ASSERT_TRUE(maxForceAttr); + float usdForce; + EXPECT_TRUE(maxForceAttr.Get(&usdForce)); + EXPECT_FLOAT_EQ(usdForce, static_cast(sdfJoint->Axis()->Effort())); + + checkedJoints++; + } + EXPECT_EQ(checkedJoints, 2); +} + +///////////////////////////////////////////////// +TEST_F(UsdJointStageFixture, JointParentIsWorld) +{ + this->GenerateUSD(sdf::testing::TestFile("sdf", "joint_parent_world.sdf")); + + // validate USD joints + int checkedJoints = 0; + for (const auto & prim : this->stage->Traverse()) + { + if (!prim.IsA()) + continue; + + auto iter = this->jointPathToSdf.find(prim.GetPath().GetString()); + ASSERT_NE(this->jointPathToSdf.end(), iter); + const auto sdfJoint = iter->second; + + // the only joint type in this test file is a fixed joint + EXPECT_TRUE(prim.IsA()); + const auto usdFixedJoint = + pxr::UsdPhysicsFixedJoint::Get(this->stage, prim.GetPath()); + ASSERT_TRUE(usdFixedJoint); + + // make sure joint is pointing to the proper parent/child links. + // The parent in this test should be the world + this->CheckParentLinkPath(&usdFixedJoint, this->worldPath.GetString()); + this->CheckChildLinkPath(&usdFixedJoint, + this->modelPath + "/" + sdfJoint->ChildLinkName()); + + // check joint's pose w.r.t. parent and child links. For this test case, + // we need to get the joint pose w.r.t. the world + ignition::math::Pose3d modelToJointPose; + auto poseErrors = sdfJoint->SemanticPose().Resolve(modelToJointPose); + EXPECT_TRUE(poseErrors.empty()); + poseErrors.clear(); + ignition::math::Pose3d worldToModelPose; + poseErrors = this->model->SemanticPose().Resolve(worldToModelPose); + const auto worldToJointPose = worldToModelPose * modelToJointPose; + EXPECT_TRUE(poseErrors.empty()); + poseErrors.clear(); + ignition::math::Pose3d childToJointPose; + poseErrors = sdfJoint->SemanticPose().Resolve(childToJointPose, + sdfJoint->ChildLinkName()); + EXPECT_TRUE(poseErrors.empty()); + this->CheckRelativeLinkPoses(&usdFixedJoint, worldToJointPose, + childToJointPose); + + checkedJoints++; + } + EXPECT_EQ(checkedJoints, 1); +} + +///////////////////////////////////////////////// +TEST_F(UsdJointStageFixture, BallPrismaticJoint) +{ + this->GenerateUSD(sdf::testing::TestFile("sdf", "ball_prismatic_joint.sdf")); + + // validate USD joints + int checkedBallJoints = 0; + int checkedPrismaticJoints = 0; + for (const auto & prim : this->stage->Traverse()) + { + if (!prim.IsA()) + continue; + + auto iter = this->jointPathToSdf.find(prim.GetPath().GetString()); + ASSERT_NE(this->jointPathToSdf.end(), iter); + const auto sdfJoint = iter->second; + + if (prim.IsA()) + { + checkedBallJoints++; + } + else if (prim.IsA()) + { + checkedPrismaticJoints++; + + const auto usdPrismaticJoint = + pxr::UsdPhysicsPrismaticJoint::Get(this->stage, prim.GetPath()); + ASSERT_TRUE(usdPrismaticJoint); + + // check the joint's axis + this->VerifyJointAxis(usdPrismaticJoint, "Z"); + + // check the joint limits + this->VerifyJointLimits(usdPrismaticJoint, + static_cast(sdfJoint->Axis()->Lower()), + static_cast(sdfJoint->Axis()->Upper()), + false); + } + else + { + continue; + } + + const auto usdJoint = + pxr::UsdPhysicsJoint::Get(this->stage, prim.GetPath()); + ASSERT_TRUE(usdJoint); + + // make sure joint is pointing to the proper parent/child links + this->CheckParentLinkPath(&usdJoint, + this->modelPath + "/" + sdfJoint->ParentLinkName()); + this->CheckChildLinkPath(&usdJoint, + this->modelPath + "/" + sdfJoint->ChildLinkName()); + + // check joint's pose w.r.t. parent and child links + ignition::math::Pose3d parentToJointPose; + auto poseErrors = sdfJoint->SemanticPose().Resolve(parentToJointPose, + sdfJoint->ParentLinkName()); + EXPECT_TRUE(poseErrors.empty()); + poseErrors.clear(); + ignition::math::Pose3d childToJointPose; + poseErrors = sdfJoint->SemanticPose().Resolve(childToJointPose, + sdfJoint->ChildLinkName()); + EXPECT_TRUE(poseErrors.empty()); + this->CheckRelativeLinkPoses(&usdJoint, parentToJointPose, + childToJointPose); + } + EXPECT_EQ(checkedBallJoints, 1); + EXPECT_EQ(checkedPrismaticJoints, 1); +} + +// TODO(adlarkin) Add a test case for a revolute joint with the axis being "y". +// This is a special case; see the sdf::usd::SetUSDJointPose method in +// usd/src/sdf_parser/Joint.cc for how this is handled diff --git a/usd/src/sdf_parser/Model.cc b/usd/src/sdf_parser/Model.cc index ccf0104a6..9130921bf 100644 --- a/usd/src/sdf_parser/Model.cc +++ b/usd/src/sdf_parser/Model.cc @@ -35,6 +35,7 @@ #pragma pop_macro ("__DEPRECATED") #include "sdf/Model.hh" +#include "sdf/usd/sdf_parser/Joint.hh" #include "sdf/usd/sdf_parser/Link.hh" #include "../UsdUtils.hh" @@ -46,7 +47,7 @@ inline namespace SDF_VERSION_NAMESPACE { namespace usd { UsdErrors ParseSdfModel(const sdf::Model &_model, pxr::UsdStageRefPtr &_stage, - const std::string &_path, const pxr::SdfPath &/*_worldPath*/) + const std::string &_path, const pxr::SdfPath &_worldPath) { UsdErrors errors; @@ -129,6 +130,23 @@ namespace usd } } + // Parse all of the model's joints and convert them to USD. + for (uint64_t i = 0; i < _model.JointCount(); ++i) + { + const auto joint = *(_model.JointByIndex(i)); + const auto jointPath = std::string(_path + "/" + joint.Name()); + const auto jointErrors = ParseSdfJoint(joint, _stage, jointPath, _model, + sdfLinkToUSDPath, _worldPath); + if (!jointErrors.empty()) + { + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing joint [" + joint.Name() + "].")); + errors.insert(errors.end(), jointErrors.begin(), jointErrors.end()); + return errors; + } + } + // TODO(adlarkin) finish parsing model return errors;