diff --git a/plugin/usd_decoder/kinematic_tree.cc b/plugin/usd_decoder/kinematic_tree.cc index 4e5ffc55..262ab3a0 100644 --- a/plugin/usd_decoder/kinematic_tree.cc +++ b/plugin/usd_decoder/kinematic_tree.cc @@ -55,7 +55,12 @@ bool GetJointBodies(const pxr::UsdPhysicsJoint& joint, pxr::SdfPath* from, joint.GetPath().GetAsString().c_str()); return false; } - *to = body1_paths[0]; + // body1 pointing to the default prim means it's attached to the worldbody. + if (body1_paths[0] == default_prim_path) { + *to = pxr::SdfPath(); + } else { + *to = body1_paths[0]; + } pxr::SdfPathVector body0_paths; joint.GetBody0Rel().GetTargets(&body0_paths); @@ -205,8 +210,24 @@ std::unique_ptr BuildKinematicTree(const pxr::UsdStageRefPtr stage) { continue; } - int from_idx = body_index[from]; - int to_idx = body_index[to]; + // Check if this is a constraint (not participating in kinematic tree). + bool excluded_from_articulation = false; + joint.GetExcludeFromArticulationAttr().Get(&excluded_from_articulation); + + // Check if bodies exist in body_index (don't use [] which inserts defaults) + auto from_it = body_index.find(from); + auto to_it = body_index.find(to); + + if (from_it == body_index.end() || to_it == body_index.end()) { + // For constraint joints (e.g., site-based welds), body0/body1 may point + // to sites which aren't in body_index. Add these to the world node. + if (excluded_from_articulation) { + extraction.nodes[0]->constraints.push_back(joint.GetPath()); + } + continue; + } + int from_idx = from_it->second; + int to_idx = to_it->second; if (from_idx == to_idx) { mju_error("Cycle detected: self referencing joint at node %s", to.GetString().c_str()); @@ -216,8 +237,6 @@ std::unique_ptr BuildKinematicTree(const pxr::UsdStageRefPtr stage) { // If we encounter a joint that does not participate in articulation, we // should treat it as a constraint instead. // For example, a weld constraint is represented by a fixed joint. - bool excluded_from_articulation; - joint.GetExcludeFromArticulationAttr().Get(&excluded_from_articulation); if (excluded_from_articulation) { extraction.nodes[to_idx]->constraints.push_back(joint.GetPath()); continue; diff --git a/plugin/usd_decoder/usd_decoder.cc b/plugin/usd_decoder/usd_decoder.cc index 8d45bd5d..59b7f1f6 100644 --- a/plugin/usd_decoder/usd_decoder.cc +++ b/plugin/usd_decoder/usd_decoder.cc @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -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()) { - // Handle fixed joints as weld constraints. + } else if (prim.IsA() || + prim.IsA()) { + // 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; }