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

Fix USD equality decoding

Browse Source 
- handle when body0 or body1 are the default prim/world
- add site-based equalities to the constrainst list so they aren't skipped
- support connect constraints by checking for spherical joints
This commit is contained in:
Lou Rohan
2026-02-17 16:55:50 -06:00
parent 6a0ebac4b1
commit 42ee943ec5
2 changed files with 41 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
plugin/usd_decoder/usd_decoder.cc
+17 -4
View File
@@ -76,6 +76,7 @@
#include <pxr/usd/usdPhysics/revoluteJoint.h>
#include <pxr/usd/usdPhysics/rigidBodyAPI.h>
#include <pxr/usd/usdPhysics/scene.h>
#include <pxr/usd/usdPhysics/sphericalJoint.h>
#include <pxr/usd/usdShade/material.h>
#include <pxr/usd/usdShade/materialBindingAPI.h>
@@ -1897,8 +1898,9 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body,
eq_joint_api.GetCoef4Attr().Get(&eq->data[4]);
ParseMjcEqualityAPISolverParams(eq, equality_api, prim);
} else if (prim.IsA<pxr::UsdPhysicsFixedJoint>()) {
// Handle fixed joints as weld constraints.
} else if (prim.IsA<pxr::UsdPhysicsFixedJoint>() ||
prim.IsA<pxr::UsdPhysicsSphericalJoint>()) {
// Handle fixed joints as weld constraints, spherical joints as connect constraints
pxr::UsdPhysicsJoint joint(prim);
// A fixed joint means the bodies are welded.
pxr::UsdRelationship body0_rel = joint.GetBody0Rel();
@@ -1914,6 +1916,16 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body,
auto stage = prim.GetStage();
// Get the default prim path to identify the world body.
pxr::SdfPath default_prim_path;
if (stage->GetDefaultPrim().IsValid()) {
default_prim_path = stage->GetDefaultPrim().GetPath();
}
// Map default prim to world body (empty path means world in MuJoCo).
bool body0_is_world = body0_path.IsEmpty() || body0_path == default_prim_path;
bool body1_is_world = body1_path.IsEmpty() || body1_path == default_prim_path;
auto body0_prim = stage->GetPrimAtPath(body0_path);
auto body1_prim = stage->GetPrimAtPath(body1_path);
@@ -1944,8 +1956,9 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body,
mjs_setString(eq->name2, body1_path.GetAsString().c_str());
eq->objtype = mjOBJ_SITE;
} else {
mjs_setString(eq->name1, body0_path.GetAsString().c_str());
mjs_setString(eq->name2, body1_path.GetAsString().c_str());
// For body welds, use "world" for the world body, otherwise use the USD path.
mjs_setString(eq->name1, body0_is_world ? "world" : body0_path.GetAsString().c_str());
mjs_setString(eq->name2, body1_is_world ? "world" : body1_path.GetAsString().c_str());
eq->objtype = mjOBJ_BODY;
}