diff --git a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc index 8ddba52d..832d1cf3 100644 --- a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc +++ b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc @@ -832,7 +832,7 @@ class ModelWriter { site_path, pxr::VtArray{kTokens->xformOpTransform}); } - void WriteGeom(mjsGeom *geom, const mjsBody *body) { + void WriteGeom(mjsGeom *geom, const mjsBody *body, bool write_physics) { const int body_id = mjs_getId(body->element); const auto &body_path = body_paths_[body_id]; @@ -866,6 +866,14 @@ class ModelWriter { return; } + // Apply the PhysicsCollisionAPI schema if we are writing physics and the + // geom participates in collisions. + if (write_physics && (model_->geom_contype[geom_id] != 0 || + model_->geom_conaffinity[geom_id] != 0)) { + ApplyApiSchema(data_, geom_path, + pxr::UsdPhysicsTokens->PhysicsCollisionAPI); + } + mjsDefault *spec_default = mjs_getDefault(geom->element); pxr::TfToken valid_class_name = GetValidPrimName(*spec_default->name); pxr::SdfPath geom_class_path = class_path_.AppendChild(valid_class_name); @@ -940,10 +948,10 @@ class ModelWriter { } } - void WriteGeoms(mjsBody *body) { + void WriteGeoms(mjsBody *body, bool write_physics) { mjsGeom *geom = mjs_asGeom(mjs_firstChild(body, mjOBJ_GEOM, false)); while (geom) { - WriteGeom(geom, body); + WriteGeom(geom, body, write_physics); geom = mjs_asGeom(mjs_nextChild(body, geom->element, false)); } } @@ -1106,7 +1114,7 @@ class ModelWriter { WriteBody(body, write_physics); } WriteSites(body); - WriteGeoms(body); + WriteGeoms(body, write_physics); WriteCameras(body); WriteLights(body); body = mjs_asBody(mjs_nextElement(spec_, body->element)); diff --git a/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc b/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc index 78f10f46..2273ac5a 100644 --- a/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc +++ b/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc @@ -45,6 +45,7 @@ #include #include #include +#include #include PXR_NAMESPACE_OPEN_SCOPE @@ -607,5 +608,126 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsRigidBody) { pxr::UsdPhysicsRigidBodyAPI); } +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsColliders) { + static constexpr char kXml[] = R"( + + + + + + + + + + + + + + + + + + + + + + )"; + + pxr::SdfFileFormat::FileFormatArguments args; + args["usdMjcfToggleUsdPhysics"] = "true"; + pxr::SdfLayerRefPtr layer = LoadLayer(kXml, args); + auto stage = pxr::UsdStage::Open(layer); + + EXPECT_THAT(stage, testing::NotNull()); + EXPECT_PRIM_VALID(stage, "/test"); + + // Expected hierarchy under /test: + // + // ground [collider] + // + // body_0/body_0 [rigidbody] + // body_0/body_0/body_0_col [collider] + // + // body_0/body_0_0 [rigidbody] <-- Note: USD reparents nested rigid bodies + // body_0/body_0/body_0_0/body_0_0_col [collider] + // + // body_1/body_1 [rigidbody] + // body_1/body_1/body_1_col_0 [collider] + // body_1/body_1/body_1_col_1 [collider] + // + // body_2/body_2 [rigidbody] + // body_2/body_2/body_2_nocol [] + + // ground [collider] (Static collider) + EXPECT_PRIM_VALID(stage, "/test/ground"); + EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/ground", + pxr::UsdPhysicsRigidBodyAPI); + EXPECT_PRIM_API_APPLIED(stage, "/test/ground", pxr::UsdPhysicsCollisionAPI); + + // body_0/body_0 [rigidbody] + EXPECT_PRIM_VALID(stage, "/test/body_0/body_0"); + EXPECT_PRIM_API_APPLIED(stage, "/test/body_0/body_0", + pxr::UsdPhysicsRigidBodyAPI); + EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_0/body_0", + pxr::UsdPhysicsCollisionAPI); + // body_0/body_0/body_0_col [collider] + EXPECT_PRIM_VALID(stage, "/test/body_0/body_0/body_0_col"); + EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_0/body_0/body_0_col", + pxr::UsdPhysicsRigidBodyAPI); + EXPECT_PRIM_API_APPLIED(stage, "/test/body_0/body_0/body_0_col", + pxr::UsdPhysicsCollisionAPI); + + // body_0/body_0_0 [rigidbody] (Nested body - reparented) + EXPECT_PRIM_VALID(stage, "/test/body_0/body_0_0"); + EXPECT_PRIM_API_APPLIED(stage, "/test/body_0/body_0_0", + pxr::UsdPhysicsRigidBodyAPI); + EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_0/body_0_0", + pxr::UsdPhysicsCollisionAPI); + // body_0/body_0_0/body_0_0_col [collider] + EXPECT_PRIM_VALID(stage, "/test/body_0/body_0_0/body_0_0_col"); + EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_0/body_0_0/body_0_0_col", + pxr::UsdPhysicsRigidBodyAPI); + EXPECT_PRIM_API_APPLIED(stage, "/test/body_0/body_0_0/body_0_0_col", + pxr::UsdPhysicsCollisionAPI); + + // body_1/body_1 [rigidbody] + EXPECT_PRIM_VALID(stage, "/test/body_1/body_1"); + EXPECT_PRIM_API_APPLIED(stage, "/test/body_1/body_1", + pxr::UsdPhysicsRigidBodyAPI); + EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_1/body_1", + pxr::UsdPhysicsCollisionAPI); + // body_1/body_1/body_1_col_0 [collider] + EXPECT_PRIM_VALID(stage, "/test/body_1/body_1/body_1_col_0"); + EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_1/body_1/body_1_col_0", + pxr::UsdPhysicsRigidBodyAPI); + EXPECT_PRIM_API_APPLIED(stage, "/test/body_1/body_1/body_1_col_0", + pxr::UsdPhysicsCollisionAPI); + // body_1/body_1/body_1_col_1 [collider] + EXPECT_PRIM_VALID(stage, "/test/body_1/body_1/body_1_col_1"); + EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_1/body_1/body_1_col_1", + pxr::UsdPhysicsRigidBodyAPI); + EXPECT_PRIM_API_APPLIED(stage, "/test/body_1/body_1/body_1_col_1", + pxr::UsdPhysicsCollisionAPI); + + // body_2/body_2 [rigidbody] + EXPECT_PRIM_VALID(stage, "/test/body_2/body_2"); + EXPECT_PRIM_API_APPLIED(stage, "/test/body_2/body_2", + pxr::UsdPhysicsRigidBodyAPI); + EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_2/body_2", + pxr::UsdPhysicsCollisionAPI); + // body_2/body_2/body_2_nocol [] (No physics APIs applied) + EXPECT_PRIM_VALID(stage, "/test/body_2/body_2/body_2_nocol"); + EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_2/body_2/body_2_nocol", + pxr::UsdPhysicsRigidBodyAPI); + EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_2/body_2/body_2_nocol", + pxr::UsdPhysicsCollisionAPI); +} + } // namespace } // namespace mujoco