SDF to USD: Added Material Physics (#975)

Friction parameters are expressed as a material in USD, this change will link the collision with the physics material Signed-off-by: ahcorde <ahcorde@gmail.com> Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Co-authored-by: Louise Poubel <louise@openrobotics.org>
gazebosim · May 5, 2022 · e18448e · e18448e
1 parent 12e7782
commit e18448e
Show file tree

Hide file tree

Showing 3 changed files with 142 additions and 0 deletions.
diff --git a/test/sdf/ellipsoid_model.sdf b/test/sdf/ellipsoid_model.sdf
@@ -20,6 +20,20 @@
             <radii>0.2 0.3 0.5</radii>
           </ellipsoid>
         </geometry>
+        <surface>
+          <contact>
+            <collide_bitmask>0xAB</collide_bitmask>
+          </contact>
+          <friction>
+            <ode>
+              <mu>0.6</mu>
+              <mu2>0.7</mu2>
+              <slip1>4</slip1>
+              <slip2>5</slip2>
+              <fdir1>1.2 3.4 5.6</fdir1>
+            </ode>
+          </friction>
+        </surface>
       </collision>
       <visual name="ellipsoid_visual">
         <geometry>

diff --git a/usd/src/sdf_parser/Collision.cc b/usd/src/sdf_parser/Collision.cc
@@ -31,9 +31,12 @@
 #include <pxr/usd/usd/stage.h>
 #include <pxr/usd/usdGeom/xform.h>
 #include <pxr/usd/usdPhysics/collisionAPI.h>
+#include <pxr/usd/usdShade/connectableAPI.h>
+#include <pxr/usd/usdShade/materialBindingAPI.h>
 #pragma pop_macro ("__DEPRECATED")
 
 #include "sdf/Collision.hh"
+#include "sdf/Surface.hh"
 #include "../UsdUtils.hh"
 #include "Geometry.hh"
 
@@ -120,6 +123,88 @@ namespace usd
       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;
+    }
+
+    if (auto surface = _collision.Surface())
+    {
+      if (auto friction = surface->Friction())
+      {
+        if (auto ode = friction->ODE())
+        {
+          const auto looksPath = pxr::SdfPath("/Looks");
+          auto looksPrim = _stage->GetPrimAtPath(looksPath);
+          if (!looksPrim)
+          {
+            looksPrim = _stage->DefinePrim(looksPath, pxr::TfToken("Scope"));
+          }
+
+          // This variable will increase with every new material to avoid
+          // collision with the names of the materials
+          static int i = 0;
+
+          auto materialPath =
+            pxr::SdfPath("/Looks/MaterialPhysics_" + std::to_string(i));
+          i++;
+
+          pxr::UsdShadeMaterial materialUsd;
+          auto usdMaterialPrim = _stage->GetPrimAtPath(materialPath);
+          if (!usdMaterialPrim)
+          {
+            materialUsd = pxr::UsdShadeMaterial::Define(_stage, materialPath);
+            usdMaterialPrim = _stage->GetPrimAtPath(materialPath);
+          }
+          else
+          {
+            materialUsd = pxr::UsdShadeMaterial(usdMaterialPrim);
+          }
+
+          const pxr::TfToken appliedSchemaNamePhysicsMaterialRootAPI(
+            "PhysicsMaterialAPI");
+          const pxr::TfToken appliedSchemaNamePhysxMaterialAPI(
+            "PhysxMaterialAPI");
+          pxr::SdfPrimSpecHandle primSpec = pxr::SdfCreatePrimInLayer(
+            _stage->GetEditTarget().GetLayer(), materialPath);
+          pxr::SdfTokenListOp listOpMaterial;
+          // Use ReplaceOperations to append in place.
+          listOpMaterial.ReplaceOperations(
+            pxr::SdfListOpTypeExplicit,
+            0,
+            0,
+            {appliedSchemaNamePhysicsMaterialRootAPI,
+             appliedSchemaNamePhysxMaterialAPI});
+          primSpec->SetInfo(
+            pxr::UsdTokens->apiSchemas, pxr::VtValue::Take(listOpMaterial));
+
+          usdMaterialPrim.CreateAttribute(
+            pxr::TfToken("physics:density"),
+            pxr::SdfValueTypeNames->Float, false).Set(1.0f);
+
+          usdMaterialPrim.CreateAttribute(
+            pxr::TfToken("physics:dynamicFriction"),
+            pxr::SdfValueTypeNames->Float, false).Set(
+              static_cast<float>(ode->Mu()));
+
+          usdMaterialPrim.CreateAttribute(
+            pxr::TfToken("physics:restitution"),
+            pxr::SdfValueTypeNames->Float, false).Set(1.0f);
+
+          usdMaterialPrim.CreateAttribute(
+            pxr::TfToken("physics:staticFriction"),
+            pxr::SdfValueTypeNames->Float, false).Set(
+              static_cast<float>(ode->Mu2()));
+
+          usdMaterialPrim.CreateAttribute(
+            pxr::TfToken("physXMaterial:frictionCombineMode"),
+            pxr::SdfValueTypeNames->Token, false).Set(pxr::TfToken("average"));
+
+          usdMaterialPrim.CreateAttribute(
+            pxr::TfToken("physXMaterial:restitutionCombineMode"),
+            pxr::SdfValueTypeNames->Token, false).Set(pxr::TfToken("average"));
+
+          pxr::UsdShadeMaterialBindingAPI(geomPrim).Bind(materialUsd);
+        }
+      }
     }
 
     return errors;

diff --git a/usd/src/sdf_parser/sdf2usd_TEST.cc b/usd/src/sdf_parser/sdf2usd_TEST.cc
@@ -98,6 +98,16 @@ TEST(check_cmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF))
   }
 }
 
+// helper method for getting attributes of a prim
+template<typename T>
+void getAttr(const pxr::UsdPrim &_prim, const std::string &_attributeName,
+    T &_value)
+{
+  const auto attr = _prim.GetAttribute(pxr::TfToken(_attributeName));
+  ASSERT_TRUE(attr);
+  attr.Get(&_value);
+}
+
 TEST(check_cmd_model, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF))
 {
   std::string pathBase = PROJECT_SOURCE_PATH;
@@ -129,5 +139,38 @@ TEST(check_cmd_model, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF))
       pxr::SdfPath("/ellipsoid/ellipsoid_link/ellipsoid_visual")));
     ASSERT_TRUE(stage->GetPrimAtPath(
       pxr::SdfPath("/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry")));
+
+    // check material physics
+    const auto materialPhysicsPrim =
+      stage->GetPrimAtPath(pxr::SdfPath("/Looks/MaterialPhysics_0"));
+    ASSERT_TRUE(materialPhysicsPrim);
+    float floatAttrVal;
+    getAttr(materialPhysicsPrim, "physics:density", floatAttrVal);
+    EXPECT_FLOAT_EQ(floatAttrVal, 1.0f);
+    getAttr(materialPhysicsPrim, "physics:dynamicFriction", floatAttrVal);
+    EXPECT_FLOAT_EQ(floatAttrVal, 0.6f);
+    getAttr(materialPhysicsPrim, "physics:restitution", floatAttrVal);
+    EXPECT_FLOAT_EQ(floatAttrVal, 1.0f);
+    getAttr(materialPhysicsPrim, "physics:staticFriction", floatAttrVal);
+    EXPECT_FLOAT_EQ(floatAttrVal, 0.7f);
+    pxr::TfToken tokenAttrVal;
+    getAttr(materialPhysicsPrim, "physXMaterial:frictionCombineMode",
+        tokenAttrVal);
+    EXPECT_EQ(tokenAttrVal, pxr::TfToken("average"));
+    getAttr(materialPhysicsPrim, "physXMaterial:restitutionCombineMode",
+        tokenAttrVal);
+    EXPECT_EQ(tokenAttrVal, pxr::TfToken("average"));
+
+    // make sure the collision's friction parameters were properly converted
+    const auto collisionGeometryPrim = stage->GetPrimAtPath(
+      pxr::SdfPath("/ellipsoid/ellipsoid_link/ellipsoid_collision/geometry"));
+    ASSERT_TRUE(collisionGeometryPrim);
+    const pxr::TfToken frictionRelKey("material:binding");
+    ASSERT_TRUE(collisionGeometryPrim.HasRelationship(frictionRelKey));
+    const auto rel = collisionGeometryPrim.GetRelationship(frictionRelKey);
+    pxr::SdfPathVector paths;
+    rel.GetTargets(&paths);
+    ASSERT_EQ(1u, paths.size());
+    EXPECT_EQ(pxr::SdfPath("/Looks/MaterialPhysics_0"), paths[0]);
   }
 }