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

Add support for inertia and mass to Mujoco USD interop.

Browse Source 
PiperOrigin-RevId: 763737956
Change-Id: I6cdf267d76536a26aeb6327ce90165842e32bd8a
This commit is contained in:
Sam Haves
2025-05-27 04:57:00 -07:00
committed by Copybara-Service
parent de3dc7c234
commit 441ca838a1
4 changed files with 223 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc
+120
View File
@@ -17,6 +17,8 @@
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include "src/experimental/usd/mjcPhysics/collisionAPI.h"
#include "src/experimental/usd/mjcPhysics/meshCollisionAPI.h"
#include "src/experimental/usd/mjcPhysics/sceneAPI.h"
#include "src/experimental/usd/mjcPhysics/siteAPI.h"
#include "src/experimental/usd/mjcPhysics/tokens.h"
@@ -50,6 +52,7 @@
#include <pxr/usd/usdGeom/tokens.h>
#include <pxr/usd/usdPhysics/articulationRootAPI.h>
#include <pxr/usd/usdPhysics/collisionAPI.h>
#include <pxr/usd/usdPhysics/massAPI.h>
#include <pxr/usd/usdPhysics/meshCollisionAPI.h>
#include <pxr/usd/usdPhysics/rigidBodyAPI.h>
#include <pxr/usd/usdPhysics/scene.h>
@@ -1106,6 +1109,7 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsColliders) {
EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/ground",
pxr::UsdPhysicsRigidBodyAPI);
EXPECT_PRIM_API_APPLIED(stage, "/test/ground", pxr::UsdPhysicsCollisionAPI);
EXPECT_PRIM_API_APPLIED(stage, "/test/ground", pxr::MjcPhysicsCollisionAPI);
// body_0/body_0_0 [rigidbody] (Nested body - reparented)
EXPECT_PRIM_VALID(stage, "/test/body_0/body_0_0");
@@ -1113,30 +1117,40 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsColliders) {
pxr::UsdPhysicsRigidBodyAPI);
EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_0/body_0_0",
pxr::UsdPhysicsCollisionAPI);
EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_0/body_0_0",
pxr::MjcPhysicsCollisionAPI);
// 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);
EXPECT_PRIM_API_APPLIED(stage, "/test/body_0/body_0_0/body_0_0_col",
pxr::MjcPhysicsCollisionAPI);
// body_1 [rigidbody]
EXPECT_PRIM_VALID(stage, "/test/body_1");
EXPECT_PRIM_API_APPLIED(stage, "/test/body_1", pxr::UsdPhysicsRigidBodyAPI);
EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_1",
pxr::UsdPhysicsCollisionAPI);
EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_1",
pxr::MjcPhysicsCollisionAPI);
// body_1/body_1_col_0 [collider]
EXPECT_PRIM_VALID(stage, "/test/body_1/body_1_col_0");
EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_1/body_1_col_0",
pxr::UsdPhysicsRigidBodyAPI);
EXPECT_PRIM_API_APPLIED(stage, "/test/body_1/body_1_col_0",
pxr::UsdPhysicsCollisionAPI);
EXPECT_PRIM_API_APPLIED(stage, "/test/body_1/body_1_col_0",
pxr::MjcPhysicsCollisionAPI);
// body_1/body_1_col_1 [collider]
EXPECT_PRIM_VALID(stage, "/test/body_1/body_1_col_1");
EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body_1/body_1_col_1",
pxr::UsdPhysicsRigidBodyAPI);
EXPECT_PRIM_API_APPLIED(stage, "/test/body_1/body_1_col_1",
pxr::UsdPhysicsCollisionAPI);
EXPECT_PRIM_API_APPLIED(stage, "/test/body_1/body_1_col_1",
pxr::MjcPhysicsCollisionAPI);
// body_2 [rigidbody]
EXPECT_PRIM_VALID(stage, "/test/body_2");
@@ -1169,11 +1183,117 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsColliders) {
pxr::UsdPhysicsCollisionAPI);
EXPECT_PRIM_API_APPLIED(stage, "/test/body_3/body_3_col/Mesh",
pxr::UsdPhysicsMeshCollisionAPI);
EXPECT_PRIM_API_APPLIED(stage, "/test/body_3/body_3_col/Mesh",
pxr::MjcPhysicsMeshCollisionAPI);
ExpectAttributeEqual(stage,
"/test/body_3/body_3_col/Mesh.physics:approximation",
pxr::UsdPhysicsTokens->convexHull);
}
TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsCollisionAPI) {
static constexpr char xml[] = R"(
<mujoco model="test">
<worldbody>
<body name="body">
<geom name="box" type="box" size=".05 .05 .05" mass="0.1" shellinertia="true"/>
</body>
</worldbody>
</mujoco>
)";
auto stage = OpenStageWithPhysics(xml);
ExpectAttributeEqual(stage, "/test/body/box.mjc:shellinertia", true);
}
TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsMeshCollisionAPI) {
static constexpr char xml[] = R"(
<mujoco model="test">
<asset>
<mesh name="tet_legacy" inertia="legacy" vertex="0 0 0 1 0 0 0 1 0 0 0 1"/>
<mesh name="tet_exact" inertia="exact" vertex="0 0 0 1 0 0 0 1 0 0 0 1"/>
<mesh name="tet_convex" inertia="convex" vertex="0 0 0 1 0 0 0 1 0 0 0 1"/>
<mesh name="tet_shell" inertia="shell" vertex="0 0 0 1 0 0 0 1 0 0 0 1"/>
</asset>
<worldbody>
<body name="body">
<geom name="tet_legacy" type="mesh" mesh="tet_legacy"/>
<geom name="tet_exact" type="mesh" mesh="tet_exact"/>
<geom name="tet_convex" type="mesh" mesh="tet_convex"/>
<geom name="tet_shell" type="mesh" mesh="tet_shell"/>
</body>
</worldbody>
</mujoco>
)";
auto stage = OpenStageWithPhysics(xml);
ExpectAttributeEqual(stage, "/test/body/tet_legacy/Mesh.mjc:inertia",
MjcPhysicsTokens->legacy);
ExpectAttributeEqual(stage, "/test/body/tet_exact/Mesh.mjc:inertia",
MjcPhysicsTokens->exact);
ExpectAttributeEqual(stage, "/test/body/tet_convex/Mesh.mjc:inertia",
MjcPhysicsTokens->convex);
ExpectAttributeEqual(stage, "/test/body/tet_shell/Mesh.mjc:inertia",
MjcPhysicsTokens->shell);
}
TEST_F(MjcfSdfFileFormatPluginTest, TestMassAPIApplied) {
static constexpr char xml[] = R"(
<mujoco model="test">
<worldbody>
<body name="body">
<geom name="box" type="box" size=".05 .05 .05" mass="0.1"/>
</body>
</worldbody>
</mujoco>
)";
auto stage = OpenStageWithPhysics(xml);
EXPECT_PRIM_VALID(stage, "/test/body");
EXPECT_PRIM_VALID(stage, "/test/body/box");
EXPECT_PRIM_API_APPLIED(stage, "/test/body/box", pxr::UsdPhysicsMassAPI);
EXPECT_PRIM_API_NOT_APPLIED(stage, "/test/body", pxr::UsdPhysicsMassAPI);
ExpectAttributeEqual(stage, "/test/body/box.physics:mass", 0.1f);
}
TEST_F(MjcfSdfFileFormatPluginTest, TestMassAPIAppliedToBody) {
static constexpr char xml[] = R"(
<mujoco model="test">
<worldbody>
<body name="body">
<inertial pos="1 2 3" mass="3" diaginertia="1 1 1"/>
<geom name="box" type="box" size=".05 .05 .05" mass="0.1"/>
</body>
</worldbody>
</mujoco>
)";
auto stage = OpenStageWithPhysics(xml);
EXPECT_PRIM_VALID(stage, "/test/body");
EXPECT_PRIM_VALID(stage, "/test/body/box");
EXPECT_PRIM_API_APPLIED(stage, "/test/body/box", pxr::UsdPhysicsMassAPI);
EXPECT_PRIM_API_APPLIED(stage, "/test/body", pxr::UsdPhysicsMassAPI);
// Make sure that body gets it's inertial elements from the inertial element
// and not from the subtree.
ExpectAttributeEqual(stage, "/test/body.physics:mass", 3.0f);
ExpectAttributeEqual(stage, "/test/body.physics:centerOfMass",
pxr::GfVec3f(1, 2, 3));
}
TEST_F(MjcfSdfFileFormatPluginTest, TestMassAPIDensity) {
static constexpr char xml[] = R"(
<mujoco model="test">
<worldbody>
<body name="body">
<geom name="box" type="box" size=".05 .05 .05" density="1234"/>
</body>
</worldbody>
</mujoco>
)";
auto stage = OpenStageWithPhysics(xml);
ExpectAttributeEqual(stage, "/test/body/box.physics:density", 1234.0f);
}
} // namespace
} // namespace usd
} // namespace mujoco
test/experimental/usd/test_utils.cc
+9
View File
@@ -42,6 +42,15 @@ pxr::SdfLayerRefPtr LoadLayer(
return layer;
}
pxr::UsdStageRefPtr OpenStageWithPhysics(const std::string& xml) {
pxr::SdfFileFormat::FileFormatArguments args;
args["usdMjcfToggleUsdPhysics"] = "true";
pxr::SdfLayerRefPtr layer = LoadLayer(xml, args);
auto stage = pxr::UsdStage::Open(layer);
EXPECT_THAT(stage, testing::NotNull());
return stage;
}
template <>
void ExpectAttributeEqual<pxr::SdfAssetPath>(pxr::UsdStageRefPtr stage,
pxr::SdfPath path,
test/experimental/usd/test_utils.h
+2
View File
@@ -73,6 +73,8 @@ pxr::SdfLayerRefPtr LoadLayer(
const std::string& xml,
const pxr::SdfFileFormat::FileFormatArguments& args = {});
pxr::UsdStageRefPtr OpenStageWithPhysics(const std::string& xml);
template <typename T>
void ExpectAttributeEqual(pxr::UsdStageRefPtr stage, pxr::SdfPath path,
const T& value) {