chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 6 Packages Projects Releases Wiki Activity

Add initial support for colliders

Browse Source 
By default every geom is a collider unless contype=conaffinity=0.

PiperOrigin-RevId: 753108713
Change-Id: I0e1a81b254c41e9103c34f69696205ff5c0bd6dc
This commit is contained in:
Robin Alazard
2025-04-30 04:14:43 -07:00
committed by Copybara-Service
parent 7742f62803
commit 45d3fef2fc
2 changed files with 134 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc
+12 -4
View File
@@ -832,7 +832,7 @@ class ModelWriter {
site_path, pxr::VtArray<pxr::TfToken>{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));
test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc
+122
View File
@@ -45,6 +45,7 @@
#include <pxr/usd/usdGeom/primvarsAPI.h>
#include <pxr/usd/usdGeom/sphere.h>
#include <pxr/usd/usdGeom/tokens.h>
#include <pxr/usd/usdPhysics/collisionAPI.h>
#include <pxr/usd/usdPhysics/rigidBodyAPI.h>
PXR_NAMESPACE_OPEN_SCOPE
@@ -607,5 +608,126 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsRigidBody) {
pxr::UsdPhysicsRigidBodyAPI);
}
TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsColliders) {
static constexpr char kXml[] = R"(
<mujoco model="test">
<worldbody>
<geom name="ground" type="plane" size="5 5 0.1"
contype="1" conaffinity="1"/>
<body name="body_0" pos="-3 0 2">
<joint type="free"/>
<geom name="body_0_col" type="sphere" size="1"
contype="1" conaffinity="1"/>
<body name="body_0_0" pos="-1 0 2">
<geom name="body_0_0_col" type="sphere" size="1"
contype="1" conaffinity="1"/>
</body>
</body>
<body name="body_1" pos="0 0 3">
<joint type="free"/>
<geom name="body_1_col_0" type="sphere" size="1"
contype="1" conaffinity="1"/>
<geom name="body_1_col_1" type="sphere" size="1" pos="-1 0 2"
contype="1" conaffinity="1"/>
</body>
<body name="body_2" pos="3 0 3">
<joint type="free"/>
<geom name="body_2_nocol" type="sphere" size="1"
contype="0" conaffinity="0"/>
</body>
</worldbody>
</mujoco>
)";
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