USD to SDF: Added Collisions (#898)

Signed-off-by: Alejandro Hernández <ahcorde@gmail.com>
Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>

Author: ahcorde

usd/src/usd_parser/USDLinks.cc

@@ -49,6 +49,7 @@
 #include <ignition/math/Inertial.hh>
 
 #include "sdf/Box.hh"
+#include "sdf/Collision.hh"
 #include "sdf/Cylinder.hh"
 #include "sdf/Geometry.hh"
 #include "sdf/Link.hh"
@@ -309,6 +310,7 @@ int ParseMeshSubGeom(const pxr::UsdPrim &_prim,
 /// \param[in] _geom sdf geom
 /// \param[in] _scale scale mesh
 /// \param[in] _usdData metadata of the USD file
+/// \param[out] _pose The pose of the parsed mesh
 /// \return UsdErrors, which is a list of UsdError objects. An empty list means
 /// that no errors occurred when parsing the USD mesh
 UsdErrors ParseMesh(
@@ -317,7 +319,8 @@ UsdErrors ParseMesh(
   sdf::Visual &_vis,
   sdf::Geometry &_geom,
   ignition::math::Vector3d &_scale,
-  const USDData &_usdData)
+  const USDData &_usdData,
+  ignition::math::Pose3d &_pose)
 {
   UsdErrors errors;
 
@@ -393,6 +396,9 @@ UsdErrors ParseMesh(
   {
     GetTransform(_prim, _usdData, pose, scale, _link->Name());
   }
+
+  _pose = pose;
+
   meshGeom.SetScale(scale * _scale);
 
   std::string primName = pxr::TfStringify(_prim.GetPath());
@@ -633,39 +639,119 @@ UsdErrors ParseUSDLinks(
       double metersPerUnit = data.second->MetersPerUnit();
 
       if (_prim.IsA<pxr::UsdGeomSphere>())
+      {
+        ParseSphere(_prim, geom, _scale, metersPerUnit);
+        vis.SetName("visual_sphere");
+        vis.SetGeom(geom);
+        _link->AddVisual(vis);
+      }
+      else if (_prim.IsA<pxr::UsdGeomCylinder>())
+      {
+        ParseCylinder(_prim, geom, _scale, metersPerUnit);
+        vis.SetName("visual_cylinder");
+        vis.SetGeom(geom);
+        _link->AddVisual(vis);
+      }
+      else if (_prim.IsA<pxr::UsdGeomCube>())
+      {
+        ParseCube(_prim, geom, _scale, metersPerUnit);
+        vis.SetName("visual_box");
+        vis.SetGeom(geom);
+        _link->AddVisual(vis);
+      }
+      else if (_prim.IsA<pxr::UsdGeomMesh>())
+      {
+        ignition::math::Pose3d poseTmp;
+        errors = ParseMesh(
+          _prim, &_link.value(), vis, geom, _scale, _usdData, poseTmp);
+        if (!errors.empty())
         {
-          ParseSphere(_prim, geom, _scale, metersPerUnit);
-          vis.SetName("visual_sphere");
-          vis.SetGeom(geom);
-          _link->AddVisual(vis);
-        }
-        else if (_prim.IsA<pxr::UsdGeomCylinder>())
-        {
-          ParseCylinder(_prim, geom, _scale, metersPerUnit);
-          vis.SetName("visual_cylinder");
-          vis.SetGeom(geom);
-          _link->AddVisual(vis);
-        }
-        else if (_prim.IsA<pxr::UsdGeomCube>())
-        {
-          ParseCube(_prim, geom, _scale, metersPerUnit);
-          vis.SetName("visual_box");
-          vis.SetGeom(geom);
-          _link->AddVisual(vis);
+          errors.emplace_back(UsdError(
+          sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR,
+            "Error parsing mesh"));
+          return errors;
         }
-        else if (_prim.IsA<pxr::UsdGeomMesh>())
+      }
+    }
+
+    pxr::TfTokenVector schemasCollision = _prim.GetAppliedSchemas();
+    bool physxCollisionAPIenable = false;
+    for (const auto & token : schemasCollision)
+    {
+      if (std::string(token.GetText()) == "PhysxCollisionAPI")
+      {
+        physxCollisionAPIenable = true;
+        break;
+      }
+    }
+
+    if (collisionEnabled || physxCollisionAPIenable)
+    {
+      sdf::Collision col;
+
+      // add _collision extension
+      std::string collisionName = _prim.GetPath().GetName() + "_collision";
+      col.SetName(collisionName);
+      sdf::Geometry colGeom;
+
+      ignition::math::Pose3d poseCol;
+      ignition::math::Vector3d scaleCol(1, 1, 1);
+      GetTransform(_prim, _usdData, poseCol, scaleCol, _link->Name());
+
+      double metersPerUnit = data.second->MetersPerUnit();
+
+      if (_prim.IsA<pxr::UsdGeomSphere>())
+      {
+        ParseSphere(_prim, colGeom, scaleCol, metersPerUnit);
+        col.SetGeom(colGeom);
+        col.SetRawPose(poseCol);
+      }
+      else if (_prim.IsA<pxr::UsdGeomCylinder>())
+      {
+        ParseCylinder(_prim, colGeom, scaleCol, metersPerUnit);
+        col.SetGeom(colGeom);
+        col.SetRawPose(poseCol);
+      }
+      else if (_prim.IsA<pxr::UsdGeomCube>())
+      {
+        ParseCube(_prim, colGeom, scaleCol, metersPerUnit);
+        col.SetGeom(colGeom);
+        col.SetRawPose(poseCol);
+      }
+      else if (_prim.IsA<pxr::UsdGeomMesh>())
+      {
+        sdf::Visual visTmp;
+        ignition::math::Pose3d poseTmp;
+        errors = ParseMesh(
+          _prim, &_link.value(), visTmp, colGeom, scaleCol, _usdData, poseTmp);
+        if (!errors.empty())
         {
-          errors = ParseMesh(
-            _prim, &_link.value(), vis, geom, _scale, _usdData);
-          if (!errors.empty())
-          {
-            errors.emplace_back(UsdError(
-            sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR,
-              "Error parsing mesh"));
-            return errors;
-          }
+          errors.emplace_back(UsdError(
+          sdf::usd::UsdErrorCode::SDF_TO_USD_PARSING_ERROR,
+            "Error parsing mesh"));
+          return errors;
         }
+        col.SetRawPose(poseTmp);
+        col.SetGeom(colGeom);
+      }
+      else if (primType == "Plane")
+      {
+        sdf::Plane plane;
+        colGeom.SetType(sdf::GeometryType::PLANE);
+        plane.SetSize(ignition::math::Vector2d(100, 100));
+        colGeom.SetPlaneShape(plane);
+
+        ignition::math::Pose3d pose;
+        ignition::math::Vector3d scale(1, 1, 1);
+        GetTransform(
+          _prim, _usdData, pose, scale, pxr::TfStringify(_prim.GetPath()));
+        col.SetRawPose(pose);
+        col.SetGeom(colGeom);
+      }
+
+      _link->AddCollision(col);
     }
+
   }
   return errors;
 }

usd/src/usd_parser/USDLinks_TEST.cc

@@ -34,6 +34,7 @@
 
 #include "USDLinks.hh"
 
+#include "sdf/Collision.hh"
 #include "sdf/Geometry.hh"
 #include "sdf/Visual.hh"
 
@@ -70,6 +71,10 @@ TEST(USDLinksTest, LinksNameMassAndDiagonalMoments)
         auto geom = visual->Geom();
         ASSERT_NE(nullptr, geom);
         EXPECT_EQ(_type, geom->Type());
+        auto collision = _link.CollisionByIndex(0);
+        ASSERT_NE(nullptr, collision);
+        auto geomCol = collision->Geom();
+        ASSERT_NE(nullptr, geomCol);
       }
     };
 
@@ -89,6 +94,13 @@ TEST(USDLinksTest, LinksNameMassAndDiagonalMoments)
   sdf::usd::ParseUSDLinks(
     boxLinkGeometry, "/box/box_link", linkSDF, usdData, scale);
 
+  const auto boxLinkCollision = stage->GetPrimAtPath(
+    pxr::SdfPath("/box/box_link/box_visual/collision"));
+  ASSERT_TRUE(boxLinkCollision);
+
+  sdf::usd::ParseUSDLinks(
+    boxLinkCollision, "/box/box_link", linkSDF, usdData, scale);
+
   ASSERT_TRUE(linkSDF);
   checkLink(linkSDF.value(), "box_link", 1.0,
             ignition::math::Vector3d(0.1666, 0.1666, 0.1666),
@@ -111,6 +123,14 @@ TEST(USDLinksTest, LinksNameMassAndDiagonalMoments)
     cylinderLinkGeometry, "/cylinder/cylinder_link",
     linkCylinderSDF, usdData, scale);
 
+  const auto cylinderLinkCollision = stage->GetPrimAtPath(
+    pxr::SdfPath("/cylinder/cylinder_link/cylinder_visual/collision"));
+  EXPECT_TRUE(cylinderLinkCollision);
+
+  sdf::usd::ParseUSDLinks(
+    cylinderLinkCollision, "/cylinder/cylinder_link",
+    linkCylinderSDF, usdData, scale);
+
   checkLink(linkCylinderSDF.value(), "cylinder_link", 1.7,
             ignition::math::Vector3d(0.1458, 0.1458, 0.125),
             1u, sdf::GeometryType::CYLINDER);
@@ -131,6 +151,14 @@ TEST(USDLinksTest, LinksNameMassAndDiagonalMoments)
     sphereLinkGeometry, "/sphere/sphere_link",
     linkSphereSDF, usdData, scale);
 
+  const auto sphereLinkCollision = stage->GetPrimAtPath(
+    pxr::SdfPath("/sphere/sphere_link/sphere_visual/collision"));
+  EXPECT_TRUE(sphereLinkCollision);
+
+  sdf::usd::ParseUSDLinks(
+    sphereLinkCollision, "/sphere/sphere_link",
+    linkSphereSDF, usdData, scale);
+
   ASSERT_TRUE(linkSphereSDF);
   checkLink(linkSphereSDF.value(), "sphere_link", 2,
             ignition::math::Vector3d(0.1, 0.1, 0.1),
@@ -144,6 +172,14 @@ TEST(USDLinksTest, LinksNameMassAndDiagonalMoments)
   sdf::usd::ParseUSDLinks(
     capsuleLink, "/capsule/capsule_link", linkCapsuleSDF, usdData, scale);
 
+  const auto capsuleLinkCollision = stage->GetPrimAtPath(
+    pxr::SdfPath("/capsule/capsule_link/capsule_visual/collision"));
+  EXPECT_TRUE(capsuleLinkCollision);
+
+  sdf::usd::ParseUSDLinks(
+    capsuleLinkCollision, "/capsule/capsule_link",
+    linkCapsuleSDF, usdData, scale);
+
   checkLink(linkCapsuleSDF.value(), "capsule_link", 1,
             ignition::math::Vector3d(0.074154, 0.074154, 0.018769),
             0u, sdf::GeometryType::EMPTY);
@@ -157,6 +193,14 @@ TEST(USDLinksTest, LinksNameMassAndDiagonalMoments)
     ellipsoidLink, "/ellipsoid/ellipsoid_link",
     linkEllipsoidSDF, usdData, scale);
 
+  const auto ellipsoidLinkCollision = stage->GetPrimAtPath(
+    pxr::SdfPath("/ellipsoid/ellipsoid_link/ellipsoid_visual/collision"));
+  EXPECT_TRUE(ellipsoidLinkCollision);
+
+  sdf::usd::ParseUSDLinks(
+    ellipsoidLinkCollision, "/ellipsoid/ellipsoid_link",
+    linkEllipsoidSDF, usdData, scale);
+
   checkLink(linkEllipsoidSDF.value(), "ellipsoid_link", 1,
             ignition::math::Vector3d(0.068, 0.058, 0.026),
             0u, sdf::GeometryType::EMPTY);