Fix USD equality decoding

- 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

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<Node> 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<Node> 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;

plugin/usd_decoder/usd_decoder.cc

@@ -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;
     }