sdf -> usd Parse links and ground planes (#828)

Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: Ashton Larkin <ashton@osrfoundation.org>
Co-authored-by: Ashton Larkin <ashton@osrfoundation.org>

Author: ahcorde

usd/include/sdf/usd/sdf_parser/Light.hh

@@ -47,7 +47,7 @@ namespace sdf
     /// of _light.
     /// \param[in] _path The USD path of the parsed light in _stage, which must
     /// be a valid USD path.
-    /// \return UsdErrors, which is a vector of Error objects. Each Error
+    /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError
     /// includes an error code and message. An empty vector indicates no error.
     UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfLight(
         const sdf::Light &_light,

usd/include/sdf/usd/sdf_parser/Link.hh

@@ -0,0 +1,62 @@
+/*
+ * 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_LINK_HH_
+#define SDF_USD_SDF_PARSER_LINK_HH_
+
+#include <string>
+
+// 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 <pxr/usd/usd/stage.h>
+#pragma pop_macro ("__DEPRECATED")
+
+#include "sdf/Link.hh"
+#include "sdf/usd/Export.hh"
+#include "sdf/usd/UsdError.hh"
+#include "sdf/sdf_config.h"
+
+namespace sdf
+{
+  // Inline bracke to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  //
+  namespace usd
+  {
+    /// \brief Parse an SDF link into a USD stage.
+    /// \param[in] _link The SDF link to parse.
+    /// \param[in] _stage The stage that should contain the USD representation
+    /// of _link. This must be a valid, initialized stage.
+    /// \param[in] _path The USD path of the parsed link in _stage, which must
+    /// be a valid USD path.
+    /// \param[in] _rigidBody Whether the link is a rigid body (i.e.,
+    /// non-static) or not. True for rigid body, false otherwise
+    /// \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 _link to its USD representation.
+    UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfLink(const sdf::Link &_link,
+        pxr::UsdStageRefPtr &_stage, const std::string &_path,
+        bool _rigidBody);
+  }
+  }
+}
+
+#endif

usd/include/sdf/usd/sdf_parser/Model.hh

@@ -0,0 +1,63 @@
+/*
+ * 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_MODEL_HH_
+#define SDF_USD_SDF_PARSER_MODEL_HH_
+
+#include <string>
+
+// 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 <pxr/usd/sdf/path.h>
+#include <pxr/usd/usd/stage.h>
+#pragma pop_macro ("__DEPRECATED")
+
+#include "sdf/Model.hh"
+#include "sdf/usd/Export.hh"
+#include "sdf/usd/UsdError.hh"
+#include "sdf/sdf_config.h"
+
+namespace sdf
+{
+  // Inline bracke to help doxygen filtering.
+  inline namespace SDF_VERSION_NAMESPACE {
+  //
+  namespace usd
+  {
+    /// \brief Parse an SDF model into a USD stage.
+    /// \param[in] _model The SDF model to parse.
+    /// \param[in] _stage The stage that should contain the USD representation
+    /// of _model. This must be a valid, initialized stage.
+    /// \param[in] _path The USD path of the parsed model in _stage, which must
+    /// be a valid USD path.
+    /// \param[in] _worldPath The path to the USD world prim. This is needed if
+    /// the model has any joints with the world as its parent.
+    /// \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 parsing _model to its USD representation.
+    UsdErrors IGNITION_SDFORMAT_USD_VISIBLE ParseSdfModel(
+        const sdf::Model &_model, pxr::UsdStageRefPtr &_stage,
+        const std::string &_path, const pxr::SdfPath &_worldPath);
+  }
+  }
+}
+
+#endif

usd/src/CMakeLists.txt

@@ -1,6 +1,8 @@
 set(sources
   UsdError.cc
   sdf_parser/Light.cc
+  sdf_parser/Link.cc
+  sdf_parser/Model.cc
   sdf_parser/World.cc
 )
 
@@ -20,6 +22,9 @@ target_link_libraries(${usd_target}
 set(gtest_sources
   sdf_parser/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
+  # functionality is complete
   sdf_parser/World_Sdf2Usd_TEST.cc
   UsdError_TEST.cc
   UsdUtils_TEST.cc

usd/src/UsdTestUtils.hh

@@ -20,11 +20,22 @@
 #include <gtest/gtest.h>
 #include <ignition/math/Angle.hh>
 #include <ignition/math/Pose3.hh>
+
+// 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 <pxr/base/gf/quatf.h>
 #include <pxr/base/gf/vec3d.h>
 #include <pxr/base/gf/vec3f.h>
 #include <pxr/base/tf/token.h>
 #include <pxr/usd/usd/attribute.h>
 #include <pxr/usd/usd/prim.h>
+#include <pxr/usd/usdPhysics/massAPI.h>
+#include <pxr/usd/usdPhysics/rigidBodyAPI.h>
+#pragma pop_macro ("__DEPRECATED")
 
 #include "sdf/system_util.hh"
 
@@ -95,6 +106,76 @@ void CheckPrimPose(const pxr::UsdPrim &_usdPrim,
   EXPECT_TRUE(checkedOpOrder);
 }
 
+/// \brief Compare the Inertial of a USD prim to the desired values
+/// \param[in] _usdPrim The USD prim
+/// \param[in] _targetMass Mass of the link that _usdPrim should have
+/// \param[in] _targetDiagonalInertia Diagonal Inertia that _usdPrim should have
+/// \param[in] _targetPrincipalAxes The principal axes that _usdPrim should have
+/// \param[in] _targetCenterOfMass Center of mass that _usdPrim should have
+/// \param[in] _isRigid True if _usdPrim should be a rigid body, False otherwise
+void CheckInertial(const pxr::UsdPrim &_usdPrim,
+    float _targetMass,
+    const pxr::GfVec3f &_targetDiagonalInertia,
+    const pxr::GfQuatf &_targetPrincipalAxes,
+    const pxr::GfVec3f &_targetCenterOfMass,
+    bool _isRigid)
+{
+  bool checkedMass = false;
+  if (auto massAttr =
+      _usdPrim.GetAttribute(pxr::TfToken("physics:mass")))
+  {
+    float massUSD;
+    massAttr.Get(&massUSD);
+    EXPECT_FLOAT_EQ(_targetMass, massUSD);
+    checkedMass = true;
+  }
+  EXPECT_TRUE(checkedMass);
+
+  bool checkedDiagInertia = false;
+  if (auto diagInertiaAttr =
+      _usdPrim.GetAttribute(pxr::TfToken("physics:diagonalInertia")))
+  {
+    pxr::GfVec3f diagonalInertiaUSD;
+    diagInertiaAttr.Get(&diagonalInertiaUSD);
+    EXPECT_FLOAT_EQ(_targetDiagonalInertia[0], diagonalInertiaUSD[0]);
+    EXPECT_FLOAT_EQ(_targetDiagonalInertia[1], diagonalInertiaUSD[1]);
+    EXPECT_FLOAT_EQ(_targetDiagonalInertia[2], diagonalInertiaUSD[2]);
+    checkedDiagInertia = true;
+  }
+  EXPECT_TRUE(checkedDiagInertia);
+
+  bool checkedPrincipalAxes = false;
+  if (auto principalAxesAttr =
+      _usdPrim.GetAttribute(pxr::TfToken("physics:principalAxes")))
+  {
+    pxr::GfQuatf principalAxesUSD;
+    principalAxesAttr.Get(&principalAxesUSD);
+    EXPECT_FLOAT_EQ(principalAxesUSD.GetReal(), _targetPrincipalAxes.GetReal());
+    const auto &usdImaginary = principalAxesUSD.GetImaginary();
+    const auto &targetImaginary = _targetPrincipalAxes.GetImaginary();
+    EXPECT_FLOAT_EQ(usdImaginary[0], targetImaginary[0]);
+    EXPECT_FLOAT_EQ(usdImaginary[1], targetImaginary[1]);
+    EXPECT_FLOAT_EQ(usdImaginary[2], targetImaginary[2]);
+    checkedPrincipalAxes = true;
+  }
+  EXPECT_TRUE(checkedPrincipalAxes);
+
+  bool checkedCOM = false;
+  if (auto comAttr =
+      _usdPrim.GetAttribute(pxr::TfToken("physics:centerOfMass")))
+  {
+    pxr::GfVec3f centerOfMassUSD;
+    comAttr.Get(&centerOfMassUSD);
+    EXPECT_FLOAT_EQ(_targetCenterOfMass[0], centerOfMassUSD[0]);
+    EXPECT_FLOAT_EQ(_targetCenterOfMass[1], centerOfMassUSD[1]);
+    EXPECT_FLOAT_EQ(_targetCenterOfMass[2], centerOfMassUSD[2]);
+    checkedCOM = true;
+  }
+  EXPECT_TRUE(checkedCOM);
+
+  EXPECT_EQ(_isRigid, _usdPrim.HasAPI<pxr::UsdPhysicsRigidBodyAPI>());
+  EXPECT_EQ(_isRigid, _usdPrim.HasAPI<pxr::UsdPhysicsMassAPI>());
+}
 } // namespace testing
 } // namespace usd
 }

usd/src/UsdUtils.hh

@@ -23,7 +23,7 @@
 #include <ignition/math/Vector3.hh>
 #include <ignition/math/Quaternion.hh>
 
-// TODO(adlarkin):this is to remove deprecated "warnings" in usd, these warnings
+// 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.
@@ -34,8 +34,13 @@
 #include <pxr/usd/usdGeom/xformCommonAPI.h>
 #pragma pop_macro ("__DEPRECATED")
 
+#include "sdf/Collision.hh"
 #include "sdf/Error.hh"
+#include "sdf/Geometry.hh"
+#include "sdf/Link.hh"
+#include "sdf/Model.hh"
 #include "sdf/SemanticPose.hh"
+#include "sdf/Visual.hh"
 #include "sdf/system_util.hh"
 #include "sdf/usd/Export.hh"
 #include "sdf/usd/UsdError.hh"
@@ -118,6 +123,31 @@ namespace sdf
 
       return errors;
     }
+
+    /// \brief Check if an sdf model is static and contains a single link that
+    /// contains a single visual and single collision that both have a plane
+    /// geometry.
+    /// \param[in] _model The sdf model to check
+    /// \return True if _model is static and has only one link with one visual
+    /// and one collision that have a plane geometry. False otherwise
+    /// \note This method will no longer be needed when a pxr::USDGeomPlane
+    /// class is created
+    inline bool SDFORMAT_VISIBLE IsPlane(const sdf::Model &_model)
+    {
+      if (!_model.Static() || _model.LinkCount() != 1u)
+        return false;
+
+      const auto &link = _model.LinkByIndex(0u);
+      if ((link->VisualCount() != 1u) || (link->CollisionCount() != 1u))
+        return false;
+
+      const auto &visual = link->VisualByIndex(0u);
+      if (visual->Geom()->Type() != sdf::GeometryType::PLANE)
+        return false;
+
+      const auto &collision = link->CollisionByIndex(0u);
+      return collision->Geom()->Type() == sdf::GeometryType::PLANE;
+    }
   }
   }
 }

usd/src/UsdUtils_TEST.cc

@@ -30,9 +30,11 @@
 #include <pxr/usd/usdGeom/xform.h>
 #pragma pop_macro ("__DEPRECATED")
 
+#include "sdf/Light.hh"
 #include "sdf/Link.hh"
 #include "sdf/Model.hh"
 #include "sdf/Root.hh"
+#include "sdf/World.hh"
 #include "test_config.h"
 #include "test_utils.hh"
 #include "UsdTestUtils.hh"
@@ -87,3 +89,26 @@ TEST(UsdUtils, SetPose)
 
   sdf::usd::testing::CheckPrimPose(prim, pose);
 }
+
+//////////////////////////////////////////////////
+TEST(UsdUtils, IsPlane)
+{
+  const auto path = sdf::testing::TestFile("sdf", "empty.sdf");
+  sdf::Root root;
+
+  ASSERT_TRUE(sdf::testing::LoadSdfFile(path, root));
+  ASSERT_EQ(1u, root.WorldCount());
+  const auto world = root.WorldByIndex(0u);
+  ASSERT_NE(nullptr, world);
+
+  ASSERT_EQ(1u, world->ModelCount());
+  const auto model = world->ModelByIndex(0u);
+  ASSERT_NE(nullptr, model);
+  EXPECT_TRUE(sdf::usd::IsPlane(*model));
+
+  // make the model non-static to verify it's no longer considered a plane
+  auto mutableModel = const_cast<sdf::Model *>(model);
+  ASSERT_NE(nullptr, mutableModel);
+  mutableModel->SetStatic(false);
+  EXPECT_FALSE(sdf::usd::IsPlane(*mutableModel));
+}