Merge pull request #15 from andrewkaufman/articulation-root

andrewkaufman · web-flow · commit cc3b2fcd3352 · 2025-10-03T09:15:17.000-07:00
Body : Remove ArticulationRootAPI from non-jointed bodies
diff --git a/mujoco_usd_converter/_impl/body.py b/mujoco_usd_converter/_impl/body.py
@@ -76,7 +76,7 @@ def convert_body(parent: Usd.Prim, name: str, body: mujoco.MjsBody, data: Conver
     safe_names = data.name_cache.getPrimNames(body_prim, [x.name for x in body.bodies])
     for child_body, safe_name in zip(body.bodies, safe_names):
         child_body_prim = convert_body(parent=body_prim, name=safe_name, body=child_body, data=data)
-        if child_body_prim and body == data.spec.worldbody:
+        if child_body_prim and body == data.spec.worldbody and has_articulated_children(child_body):
             child_body_over = data.content[Tokens.Physics].OverridePrim(child_body_prim.GetPath())
             UsdPhysics.ArticulationRootAPI.Apply(child_body_over)
 
@@ -91,6 +91,16 @@ def is_kinematic(body: mujoco.MjsBody, physics_prim: Usd.Prim) -> bool:
     return kinematic_attr and kinematic_attr.Get()
 
 
+def has_articulated_children(body: mujoco.MjsBody) -> bool:
+    # Check if this body has child bodies with non-free joints
+    for child_body in body.bodies:
+        if child_body.joints:
+            for joint in child_body.joints:
+                if joint.type != mujoco.mjtJoint.mjJNT_FREE:
+                    return True
+    return False
+
+
 def extract_inertia(fullinertia: np.ndarray) -> tuple[Gf.Quatf, Gf.Vec3f]:
     mat = np.zeros((3, 3))
     mat[0, 0] = fullinertia[0]
diff --git a/tests/data/bodies.xml b/tests/data/bodies.xml
@@ -4,6 +4,7 @@
             <geom type="box" size="0.1 0.1 0.1"/>
             <body name="nested_body" pos="0 1 0">
                 <geom type="box" size="0.1 0.1 0.1"/>
+                <joint type="hinge"/>
             </body>
         </body>
 
diff --git a/tests/testBodies.py b/tests/testBodies.py
@@ -18,7 +18,7 @@ def setUp(self):
         self.stage: Usd.Stage = Usd.Stage.Open(asset.path)
         self.assertIsValidUsd(self.stage)
 
-    def test_bodies(self):
+    def test_articulation_roots(self):
         # Root body is an Articulation Root
         prim = self.stage.GetPrimAtPath("/bodies/Geometry/root_body")
         self.assertTrue(prim.HasAPI(UsdPhysics.ArticulationRootAPI))
@@ -29,6 +29,11 @@ def test_bodies(self):
         self.assertFalse(prim.HasAPI(UsdPhysics.ArticulationRootAPI))
         self.assertTrue(prim.HasAPI(UsdPhysics.RigidBodyAPI))
 
+        # Kinematic Body is not an articulation root as there are no joints to its child bodies
+        prim = self.stage.GetPrimAtPath("/bodies/Geometry/kinematic_body")
+        self.assertFalse(prim.HasAPI(UsdPhysics.ArticulationRootAPI))
+        self.assertTrue(prim.HasAPI(UsdPhysics.RigidBodyAPI))
+
     def test_kinematic_body(self):
         # mocap body is kinematic
         prim = self.stage.GetPrimAtPath("/bodies/Geometry/kinematic_body")
