USD to SDF: some fixes (#991)

Signed-off-by: ahcorde <ahcorde@gmail.com>

Author: ahcorde

test/usd/upAxisZ.usda

@@ -102,7 +102,9 @@ def Xform "box" (
             double3 xformOp:translate = (0, 0, 0)
             uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
 
-            def Cube "geometry"
+            def Cube "geometry" (
+              kind = "model"
+            )
             {
                 float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)]
                 rel material:binding = </Looks/Material_1>
@@ -159,7 +161,9 @@ def Xform "cylinder"
             double3 xformOp:translate = (0, 0, 0)
             uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
 
-            def Cylinder "geometry"
+            def Cylinder "geometry" (
+              kind = "model"
+            )
             {
                 float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)]
                 double height = 1
@@ -202,7 +206,9 @@ def Xform "sphere"
             double3 xformOp:translate = (0, 0, 0)
             uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
 
-            def Sphere "geometry"
+            def Sphere "geometry" (
+              kind = "model"
+            )
             {
                 float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)]
                 rel material:binding = </Looks/Material_3>
@@ -243,7 +249,9 @@ def Xform "capsule"
             double3 xformOp:translate = (0, 0, 0)
             uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"]
 
-            def Capsule "geometry"
+            def Capsule "geometry" (
+              kind = "model"
+            )
             {
                 float3[] extent = [(-0.2, -0.2, -0.5), (0.2, 0.2, 0.5)]
                 double height = 0.6
@@ -286,7 +294,9 @@ def Xform "ellipsoid"
             double3 xformOp:translate = (0, 0, 0)
             uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"]
 
-            def Sphere "geometry"
+            def Sphere "geometry" (
+              kind = "model"
+            )
             {
                 float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)]
                 rel material:binding = </Looks/Material_5>

usd/src/usd_parser/USDLinks.cc

@@ -233,7 +233,7 @@ int ParseMeshSubGeom(const pxr::UsdPrim &_prim,
 
       ignition::common::SubMesh subMeshSubset;
       subMeshSubset.SetPrimitiveType(ignition::common::SubMesh::TRISTRIPS);
-      subMeshSubset.SetName("subgeommesh");
+      subMeshSubset.SetName("subgeommesh_" + std::to_string(numSubMeshes));
 
       if (it != _usdData.Materials().end())
       {
@@ -288,12 +288,18 @@ int ParseMeshSubGeom(const pxr::UsdPrim &_prim,
       _meshGeom.SetUri(directoryMesh + ".dae");
 
       geomSubset.SetMeshShape(_meshGeom);
-      visSubset.SetName("mesh_subset");
+      visSubset.SetName("mesh_subset_" + std::to_string(numSubMeshes));
       visSubset.SetGeom(geomSubset);
 
       ignition::math::Pose3d pose;
       ignition::math::Vector3d scale(1, 1, 1);
-      GetTransform(child, _usdData, pose, scale, _link->Name());
+      std::string linkName = pxr::TfStringify(_prim.GetPath());
+      auto found = linkName.find(_link->Name());
+      if (found != std::string::npos)
+      {
+        linkName = linkName.substr(0, found + _link->Name().size());
+      }
+      GetTransform(child, _usdData, pose, scale, linkName);
       _scale *= scale;
       visSubset.SetRawPose(pose);
       _link->AddVisual(visSubset);
@@ -386,15 +392,21 @@ UsdErrors ParseMesh(
 
   ignition::math::Pose3d pose;
   ignition::math::Vector3d scale(1, 1, 1);
-  std::string linkName = _link->Name();
+  std::string linkName = pxr::TfStringify(_prim.GetPath());
+  auto found = linkName.find(_link->Name());
+  if (found != std::string::npos)
+  {
+    linkName = linkName.substr(0, found + _link->Name().size());
+  }
+
   size_t nSlash = std::count(linkName.begin(), linkName.end(), '/');
   if (nSlash == 1)
   {
     GetTransform(_prim, _usdData, pose, scale, "/");
   }
   else
   {
-    GetTransform(_prim, _usdData, pose, scale, _link->Name());
+    GetTransform(_prim, _usdData, pose, scale, linkName);
   }
 
   _pose = pose;
@@ -522,7 +534,7 @@ void ParseCylinder(
   _geom.SetType(sdf::GeometryType::CYLINDER);
 
   c.SetRadius(radius * _metersPerUnit * _scale.X());
-  c.SetLength(height * _metersPerUnit * _scale.Y());
+  c.SetLength(height * _metersPerUnit * _scale.Z());
 
   _geom.SetCylinderShape(c);
 }
@@ -632,9 +644,9 @@ UsdErrors ParseUSDLinks(
       pxr::UsdModelAPI(parent).GetKind(&kindOfSchema);
     }
 
-    if (_prim.HasAPI<pxr::UsdPhysicsRigidBodyAPI>()
-      || pxr::KindRegistry::IsA(kindOfSchema, pxr::KindTokens->model)
-      || !collisionEnabled)
+    if ((_prim.HasAPI<pxr::UsdPhysicsRigidBodyAPI>()
+      || pxr::KindRegistry::IsA(kindOfSchema, pxr::KindTokens->model))
+      && (!collisionEnabled || _prim.HasAPI<pxr::UsdPhysicsMassAPI>()))
     {
       double metersPerUnit = data.second->MetersPerUnit();
 
@@ -696,7 +708,15 @@ UsdErrors ParseUSDLinks(
 
       ignition::math::Pose3d poseCol;
       ignition::math::Vector3d scaleCol(1, 1, 1);
-      GetTransform(_prim, _usdData, poseCol, scaleCol, _link->Name());
+      std::string linkName = pxr::TfStringify(_prim.GetPath());
+      auto found = linkName.find(_link->Name());
+      if (found != std::string::npos)
+      {
+        linkName = linkName.substr(0, found + _link->Name().size());
+      }
+      GetTransform(_prim, _usdData, poseCol, scaleCol, linkName);
+
+      scaleCol *= _scale;
 
       double metersPerUnit = data.second->MetersPerUnit();
 

usd/src/usd_parser/USDWorld.cc

@@ -351,7 +351,8 @@ namespace usd
       }
 
       std::optional<sdf::Link> optionalLink;
-      if (auto linkInserted = modelPtr->LinkByName(linkName))
+      if (auto linkInserted =
+          modelPtr->LinkByName(ignition::common::basename(linkName)))
       {
         optionalLink = *linkInserted;
         auto scale = linkScaleMap.find(linkName);
@@ -362,6 +363,8 @@ namespace usd
         }
         sdf::usd::ParseUSDLinks(
           prim, linkName, optionalLink, usdData, scale->second);
+        *linkInserted = optionalLink.value();
+        linkScaleMap[linkName] = scale->second;
       }
       else
       {