From 3a64d8889f59db1cc92513802a930fb3ced2b374 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 1 Apr 2022 20:32:15 +0200 Subject: [PATCH] SDF to USD: Added collision tag (#925) Signed-off-by: ahcorde Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- usd/src/CMakeLists.txt | 2 + usd/src/cmd/CMakeLists.txt | 1 + usd/src/sdf_parser/Collision.cc | 129 ++++++++++++++++++++ usd/src/sdf_parser/Collision.hh | 59 +++++++++ usd/src/sdf_parser/Geometry.cc | 35 ------ usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc | 43 ++++++- usd/src/sdf_parser/Link.cc | 20 +++ 7 files changed, 248 insertions(+), 41 deletions(-) create mode 100644 usd/src/sdf_parser/Collision.cc create mode 100644 usd/src/sdf_parser/Collision.hh diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index da9d774be..716fb7977 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -1,6 +1,7 @@ set(sources Conversions.cc UsdError.cc + sdf_parser/Collision.cc sdf_parser/Geometry.cc sdf_parser/Joint.cc sdf_parser/Light.cc @@ -81,6 +82,7 @@ endif() set(sdf2usd_test_sources Conversions.cc + sdf_parser/Collision.cc sdf_parser/Geometry.cc sdf_parser/Joint.cc sdf_parser/Light.cc diff --git a/usd/src/cmd/CMakeLists.txt b/usd/src/cmd/CMakeLists.txt index 4ce7dd845..9b03d0671 100644 --- a/usd/src/cmd/CMakeLists.txt +++ b/usd/src/cmd/CMakeLists.txt @@ -1,6 +1,7 @@ if(TARGET ${usd_target}) add_executable(sdf2usd sdf2usd.cc + ../sdf_parser/Collision.cc ../sdf_parser/Model.cc ../sdf_parser/Joint.cc ../sdf_parser/Link.cc diff --git a/usd/src/sdf_parser/Collision.cc b/usd/src/sdf_parser/Collision.cc new file mode 100644 index 000000000..38bcf4d45 --- /dev/null +++ b/usd/src/sdf_parser/Collision.cc @@ -0,0 +1,129 @@ +/* + * 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 "Collision.hh" + +#include + +#include + +// TODO(adlarkin) 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 +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Collision.hh" +#include "../UsdUtils.hh" +#include "Geometry.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ + UsdErrors ParseSdfCollision(const sdf::Collision &_collision, + pxr::UsdStageRefPtr &_stage, const std::string &_path) + { + UsdErrors errors; + const pxr::SdfPath sdfCollisionPath(_path); + auto usdCollisionXform = pxr::UsdGeomXform::Define( + _stage, sdfCollisionPath); + if (!usdCollisionXform) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_USD_DEFINITION, + "Not able to define a Geom Xform at path [" + _path + "]")); + return errors; + } + + // Purpose is a builtin attribute with 4 posible values. + // - *default* indicates that a prim has “no special purpose” and should + // generally be included in all traversals + // - *render* should generally only be included when performing a + // “final quality” render + // - *proxy* should generally only be included when performing a lightweight + // proxy render (such as OpenGL) + // - *guide* should generally only be included when an interactive + // application has been explicitly asked to “show guides” + // For collisions we need to define this as guide because we don't need + // to render anything + auto collisionPrim = _stage->GetPrimAtPath(sdfCollisionPath); + collisionPrim.CreateAttribute(pxr::TfToken("purpose"), + pxr::SdfValueTypeNames->Token, false).Set(pxr::TfToken("guide")); + + ignition::math::Pose3d pose; + auto poseErrors = usd::PoseWrtParent(_collision, pose); + if (!poseErrors.empty()) + { + for (const auto &e : poseErrors) + errors.push_back(e); + return errors; + } + + poseErrors = usd::SetPose(pose, _stage, sdfCollisionPath); + if (!poseErrors.empty()) + { + for (const auto &e : poseErrors) + errors.push_back(e); + errors.push_back(UsdError(UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Unable to set the pose of the prim corresponding to the " + "SDF collision named [" + _collision.Name() + "]")); + return errors; + } + + const auto geometry = *(_collision.Geom()); + const auto geometryPath = std::string(_path + "/geometry"); + auto geomErrors = ParseSdfGeometry(geometry, _stage, geometryPath); + if (!geomErrors.empty()) + { + errors.insert(errors.end(), geomErrors.begin(), geomErrors.end()); + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing geometry attached to _collision [" + + _collision.Name() + "]")); + return errors; + } + + auto geomPrim = _stage->GetPrimAtPath(pxr::SdfPath(geometryPath)); + if (!geomPrim) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, + "Internal error: unable to get prim at path [" + + geometryPath + "], but a geom prim should exist at this path")); + return errors; + } + + if (!pxr::UsdPhysicsCollisionAPI::Apply(geomPrim)) + { + errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY, + "Internal error: unable to apply a collision to the prim at path [" + + geometryPath + "]")); + } + + return errors; + } +} +} +} diff --git a/usd/src/sdf_parser/Collision.hh b/usd/src/sdf_parser/Collision.hh new file mode 100644 index 000000000..e4641ed5b --- /dev/null +++ b/usd/src/sdf_parser/Collision.hh @@ -0,0 +1,59 @@ +/* + * 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_COLLISION_HH_ +#define SDF_USD_SDF_PARSER_COLLISION_HH_ + +#include + +// TODO(adlarkin) 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 +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Collision.hh" +#include "sdf/config.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse an SDF collision into a USD stage. + /// \param[in] _collision The SDF collision to parse. + /// \param[in] _stage The stage that should contain the USD representation + /// of _collision. + /// \param[in] _path The USD path of the parsed collision in _stage, + /// which must be a valid USD path. + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error. + UsdErrors ParseSdfCollision( + const sdf::Collision &_collision, + pxr::UsdStageRefPtr &_stage, + const std::string &_path); + } + } +} + +#endif diff --git a/usd/src/sdf_parser/Geometry.cc b/usd/src/sdf_parser/Geometry.cc index d8310911e..0b0ce3298 100644 --- a/usd/src/sdf_parser/Geometry.cc +++ b/usd/src/sdf_parser/Geometry.cc @@ -45,7 +45,6 @@ #include #include #include -#include #include #include #pragma pop_macro ("__DEPRECATED") @@ -600,40 +599,6 @@ namespace usd "Geometry type is either invalid or not supported"))); } - // Set the collision for this geometry. - // In SDF, the collisions of a link are defined separately from - // the link's visual geometries (in case a user wants to define a simpler - // collision, for example). In USD, the collision can be attached to the - // geometry so that the collision shape is the geometry shape. - // The current approach that's taken here (attaching a collision to the - // geometry) assumes that for every visual in a link, the visual should have - // a collision that matches its geometry. This approach currently ignores - // collisions defined in a link since it instead creates collisions for a - // link that match a link's visual geometries. - // TODO(adlarkin) support the option of a different collision shape (i.e., - // don't ignore collisions defined in a link), - // especially for meshes - here's more information about how to do this: - // https://graphics.pixar.com/usd/release/wp_rigid_body_physics.html?highlight=collision#turning-meshes-into-shapes - if (errors.empty()) - { - auto geomPrim = _stage->GetPrimAtPath(pxr::SdfPath(_path)); - if (!geomPrim) - { - errors.push_back(UsdError(sdf::usd::UsdErrorCode::INVALID_PRIM_PATH, - "Internal error: unable to get prim at path [" - + _path + "], but a geom prim should exist at this path")); - return errors; - } - - if (!pxr::UsdPhysicsCollisionAPI::Apply(geomPrim)) - { - errors.push_back(UsdError(sdf::usd::UsdErrorCode::FAILED_PRIM_API_APPLY, - "Internal error: unable to apply a collision to the prim at path [" - + _path + "]")); - return errors; - } - } - return errors; } } diff --git a/usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc b/usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc index e167491e9..3dd7d5f5d 100644 --- a/usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc +++ b/usd/src/sdf_parser/Geometry_Sdf2Usd_TEST.cc @@ -153,15 +153,23 @@ TEST_F(UsdStageFixture, Geometry) const std::string groundPlaneVisualPath = groundPlaneLinkPath + "/visual"; const std::string groundPlaneGeometryPath = groundPlaneVisualPath + "/geometry"; + const std::string groundPlaneCollisionPath = + groundPlaneLinkPath + "/collision"; + const std::string groundPlaneCollisionGeometryPath = + groundPlaneCollisionPath + "/geometry"; pxr::GfVec3f scale; pxr::VtArray extent; double size, height, radius, length; + const auto groundPlaneCollisionGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(groundPlaneCollisionGeometryPath)); + ASSERT_TRUE(groundPlaneCollisionGeometry); + EXPECT_TRUE( + groundPlaneCollisionGeometry.HasAPI()); const auto groundPlaneGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(groundPlaneGeometryPath)); ASSERT_TRUE(groundPlaneGeometry); - EXPECT_TRUE(groundPlaneGeometry.HasAPI()); const auto usdGroundPlane = pxr::UsdGeomCube(groundPlaneGeometry); ASSERT_TRUE(usdGroundPlane); usdGroundPlane.GetSizeAttr().Get(&size); @@ -182,8 +190,14 @@ TEST_F(UsdStageFixture, Geometry) const std::string boxGeometryPath = boxVisualPath + "/geometry"; const auto boxGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(boxGeometryPath)); + const std::string boxCollisionPath = boxLinkPath + "/collision"; + const std::string boxCollisionGeometryPath = boxCollisionPath + "/geometry"; + const auto boxCollisionGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(boxCollisionGeometryPath)); + ASSERT_TRUE(boxCollisionGeometry); + EXPECT_TRUE(boxCollisionGeometry.HasAPI()); + ASSERT_TRUE(boxGeometry); - EXPECT_TRUE(boxGeometry.HasAPI()); const auto usdCube = pxr::UsdGeomCube(boxGeometry); ASSERT_TRUE(usdCube); usdCube.GetSizeAttr().Get(&size); @@ -201,10 +215,16 @@ TEST_F(UsdStageFixture, Geometry) const std::string cylinderLinkPath = cylinderPath + "/link"; const std::string cylinderVisualPath = cylinderLinkPath + "/visual"; const std::string cylinderGeometryPath = cylinderVisualPath + "/geometry"; + const std::string cylinderCollisionPath = cylinderLinkPath + "/collision"; + const std::string cylinderCollisionGeometryPath = + cylinderCollisionPath + "/geometry"; + const auto cylinderCollisionGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(cylinderCollisionGeometryPath)); + ASSERT_TRUE(cylinderCollisionGeometry); + EXPECT_TRUE(cylinderCollisionGeometry.HasAPI()); const auto cylinderGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(cylinderGeometryPath)); ASSERT_TRUE(cylinderGeometry); - EXPECT_TRUE(cylinderGeometry.HasAPI()); const auto usdCylinder = pxr::UsdGeomCylinder(cylinderGeometry); ASSERT_TRUE(usdCylinder); usdCylinder.GetRadiusAttr().Get(&radius); @@ -220,10 +240,16 @@ TEST_F(UsdStageFixture, Geometry) const std::string sphereLinkPath = spherePath + "/link"; const std::string sphereVisualPath = sphereLinkPath + "/sphere_vis"; const std::string sphereGeometryPath = sphereVisualPath + "/geometry"; + const std::string sphereCollisionPath = sphereLinkPath + "/collision"; + const std::string sphereCollisionGeometryPath = + sphereCollisionPath + "/geometry"; + const auto sphereCollisionGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(sphereCollisionGeometryPath)); + ASSERT_TRUE(sphereCollisionGeometry); + EXPECT_TRUE(sphereCollisionGeometry.HasAPI()); const auto sphereGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(sphereGeometryPath)); ASSERT_TRUE(sphereGeometry); - EXPECT_TRUE(sphereGeometry.HasAPI()); const auto usdSphere = pxr::UsdGeomSphere(sphereGeometry); ASSERT_TRUE(usdSphere); usdSphere.GetRadiusAttr().Get(&radius); @@ -239,7 +265,6 @@ TEST_F(UsdStageFixture, Geometry) const auto capsuleGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(capsuleGeometryPath)); ASSERT_TRUE(capsuleGeometry); - EXPECT_TRUE(capsuleGeometry.HasAPI()); const auto usdCapsule = pxr::UsdGeomCapsule(capsuleGeometry); ASSERT_TRUE(usdCapsule); usdCapsule.GetRadiusAttr().Get(&radius); @@ -256,10 +281,16 @@ TEST_F(UsdStageFixture, Geometry) const std::string meshVisualPath = meshLinkPath + "/visual"; const std::string meshGeometryPath = meshVisualPath + "/geometry"; const std::string meshGeometryMeshPath = meshGeometryPath + "/Cube"; + const std::string capsuleCollisionPath = capsuleLinkPath + "/collision"; + const std::string capsuleCollisionGeometryPath = + capsuleCollisionPath + "/geometry"; + const auto capsuleCollisionGeometry = this->stage->GetPrimAtPath( + pxr::SdfPath(capsuleCollisionGeometryPath)); + ASSERT_TRUE(capsuleCollisionGeometry); + EXPECT_TRUE(capsuleCollisionGeometry.HasAPI()); const auto meshGeometryParentPrim = this->stage->GetPrimAtPath( pxr::SdfPath(meshGeometryPath)); ASSERT_TRUE(meshGeometryParentPrim); - EXPECT_TRUE(meshGeometryParentPrim.HasAPI()); const auto meshGeometry = this->stage->GetPrimAtPath( pxr::SdfPath(meshGeometryMeshPath)); ASSERT_TRUE(meshGeometry); diff --git a/usd/src/sdf_parser/Link.cc b/usd/src/sdf_parser/Link.cc index 90786cf67..e1cb918cb 100644 --- a/usd/src/sdf_parser/Link.cc +++ b/usd/src/sdf_parser/Link.cc @@ -37,6 +37,7 @@ #include "sdf/Link.hh" #include "../UsdUtils.hh" +#include "Collision.hh" #include "Light.hh" #include "Sensor.hh" #include "Visual.hh" @@ -142,6 +143,25 @@ namespace usd } } + // parse all of the link's collisions and convert them to USD + for (uint64_t i = 0; i < _link.CollisionCount(); ++i) + { + const auto collision = *(_link.CollisionByIndex(i)); + const auto collisionPath = std::string(_path + "/" + collision.Name()); + auto errorsCollision = ParseSdfCollision(collision, _stage, + collisionPath); + if (!errorsCollision.empty()) + { + errors.insert(errors.end(), errorsCollision.begin(), + errorsCollision.end()); + errors.push_back(UsdError( + sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing collision [" + collision.Name() + + "] attached to link [" + _link.Name() + "]")); + return errors; + } + } + // convert the link's sensors for (uint64_t i = 0; i < _link.SensorCount(); ++i) {