mj_usd plugin: add joints export.

Note that ball joint is missing, will come later. PiperOrigin-RevId: 776057523 Change-Id: I8d13c3ea9e7f3d894af73a4640eb93202f8a2a03
2025-06-26 03:01:31 -07:00
parent 48205fb570
commit 3441e0aa03
4 changed files with 626 additions and 25 deletions
@@ -21,9 +21,9 @@
 #include <utility>
 #include <vector>

+#include <mujoco/experimental/usd/mjcPhysics/tokens.h>
 #include <mujoco/mjspec.h>
 #include <mujoco/mujoco.h>
-#include <mujoco/experimental/usd/mjcPhysics/tokens.h>
 #include "mjcf/utils.h"
 #include <pxr/base/arch/attributes.h>
 #include <pxr/base/gf/matrix4d.h>
@@ -55,6 +55,11 @@
 #include <pxr/usd/usdGeom/metrics.h>
 #include <pxr/usd/usdGeom/tokens.h>
 #include <pxr/usd/usdLux/tokens.h>
+#include <pxr/usd/usdPhysics/fixedJoint.h>
+#include <pxr/usd/usdPhysics/joint.h>
+#include <pxr/usd/usdPhysics/prismaticJoint.h>
+#include <pxr/usd/usdPhysics/revoluteJoint.h>
+#include <pxr/usd/usdPhysics/sphericalJoint.h>
 #include <pxr/usd/usdPhysics/tokens.h>
 #include <pxr/usd/usdShade/tokens.h>
 #include <pxr/usdImaging/usdImaging/tokens.h>
@@ -102,6 +107,7 @@ TF_DEFINE_PRIVATE_TOKENS(kTokens,
                         (repeat)
                         ((sourceMesh, pxr::UsdGeomTokens->Mesh))
                         ((inputsNormal, "inputs:normal"))
+                         ((joint, "Joint"))
                        );

 // Using to satisfy TF_REGISTRY_FUNCTION macro below and avoid operating in PXR_NS.
@@ -169,6 +175,9 @@ class ModelWriter {
  ~ModelWriter() { mj_deleteModel(model_); }

  void Write(bool write_physics) {
+    // Set working parameters.
+    write_physics_ = write_physics;
+
    // Create top level class holder.
    class_path_ = CreateClassSpec(data_, pxr::SdfPath::AbsoluteRootPath(),
                                  pxr::TfToken("__class__"));
@@ -176,9 +185,6 @@ class ModelWriter {
    // Create the world body.
    body_paths_[kWorldIndex] = WriteWorldBody(kWorldIndex);

-    // Set working parameters.
-    write_physics_ = write_physics;
-
    SetLayerMetadata(data_, pxr::SdfFieldKeys->Documentation,
                     "Generated by mujoco model writer.");
    // Mujoco is Z up by default.
@@ -1494,6 +1500,219 @@ class ModelWriter {
    }
  }

+  void WriteJoints(mjsBody *body) {
+    if (!write_physics_) return;
+
+    int body_id = mjs_getId(body->element);
+    if (body_id == kWorldIndex) return;
+
+    mjsJoint *joint = mjs_asJoint(mjs_firstChild(body, mjOBJ_JOINT, false));
+
+    if (!joint) {
+      // If no joint is found, then we pass nullptr to create a FixedJoint.
+      // WriteJoint properly handles the case where the parent is the worldbody.
+      WriteJoint(nullptr, body);
+    } else {
+      WriteJoint(joint, body);
+      if (mjs_asJoint(mjs_nextChild(body, joint->element, false))) {
+        TF_WARN(
+            "Multiple joints found for body %d. Only writing the first one.",
+            body_id);
+      }
+    }
+  }
+
+  // Write the joint. If null, then a FixedJoint is created.
+  void WriteJoint(mjsJoint *joint, const mjsBody *parent_mj_body) {
+    // Default to fixed joint if joint is null.
+    pxr::TfToken joint_prim_type = pxr::UsdPhysicsTokens->PhysicsFixedJoint;
+
+    int joint_id = -1;
+    if (joint) {
+      joint_id = mjs_getId(joint->element);
+      mjtJoint type = (mjtJoint)model_->jnt_type[joint_id];
+      switch (type) {
+        case mjJNT_FREE:
+          // Free joints are guaranteed to only ever be on the top-level body so
+          // we just write no joint. As a top-level body with no joint it will
+          // be considered a floating-base body.
+          return;
+        case mjJNT_HINGE:
+          joint_prim_type = pxr::UsdPhysicsTokens->PhysicsRevoluteJoint;
+          break;
+        case mjJNT_SLIDE:
+          joint_prim_type = pxr::UsdPhysicsTokens->PhysicsPrismaticJoint;
+          break;
+        default:
+          TF_WARN("Unsupported joint type '%d' for joint '%s'. Skipping.",
+                  (int)type, joint->name->c_str());
+          return;
+      }
+    }
+
+    int body_id = mjs_getId(parent_mj_body->element);
+
+    // the joint connects the current body as body1, to its parent body as
+    // body0.
+    int body1_id_usd = body_id;
+    int body0_id_usd = model_->body_parentid[body_id];
+
+    const pxr::SdfPath &body1_path_usd = body_paths_[body1_id_usd];
+    auto joint_name = joint ? *joint->name : "FixedJoint";
+    pxr::TfToken joint_name_token =
+        GetAvailablePrimName(joint_name, kTokens->joint, body1_path_usd);
+    pxr::SdfPath joint_path = CreatePrimSpec(data_, body1_path_usd,
+                                             joint_name_token, joint_prim_type);
+
+    // Set body0 and body1 relationships
+    // For the initial joints that connect to the world, we signal this by
+    // keeping the body0 relationship empty.
+    if (body0_id_usd != kWorldIndex) {
+      const pxr::SdfPath &body0_path_usd = body_paths_[body0_id_usd];
+      CreateRelationshipSpec(data_, joint_path,
+                             pxr::UsdPhysicsTokens->physicsBody0,
+                             body0_path_usd, pxr::SdfVariabilityUniform);
+    }
+    CreateRelationshipSpec(data_, joint_path,
+                           pxr::UsdPhysicsTokens->physicsBody1, body1_path_usd,
+                           pxr::SdfVariabilityUniform);
+
+    // Joint frame in MuJoCo is defined by jnt_pos and jnt_axis in body1's frame
+    // For FixedJoint, these are both unity.
+    pxr::GfVec3d mj_jnt_pos = pxr::GfVec3d(0.0);
+    pxr::GfVec3d mj_jnt_axis = pxr::GfVec3d(0.0, 0.0, 1.0);
+    if (joint) {
+      mj_jnt_pos = pxr::GfVec3d(&model_->jnt_pos[joint_id * 3]);
+      mj_jnt_axis = pxr::GfVec3d(&model_->jnt_axis[joint_id * 3]);
+    }
+
+    // Local joint frame for body1
+    pxr::GfVec3f local_pos1(mj_jnt_pos);
+    pxr::GfRotation().SetRotateInto(pxr::GfVec3f::ZAxis(), mj_jnt_axis);
+    pxr::GfQuatf local_rot1(
+        pxr::GfRotation()
+            .SetRotateInto(pxr::GfVec3f::ZAxis(), mj_jnt_axis)
+            .GetQuat());
+
+    SetAttributeDefault(
+        data_,
+        CreateAttributeSpec(data_, joint_path,
+                            pxr::UsdPhysicsTokens->physicsLocalPos1,
+                            pxr::SdfValueTypeNames->Float3),
+        local_pos1);
+    if (joint_prim_type == pxr::UsdPhysicsTokens->PhysicsRevoluteJoint ||
+        joint_prim_type == pxr::UsdPhysicsTokens->PhysicsPrismaticJoint) {
+      SetAttributeDefault(
+          data_,
+          CreateAttributeSpec(data_, joint_path,
+                              pxr::UsdPhysicsTokens->physicsLocalRot1,
+                              pxr::SdfValueTypeNames->Quatf),
+          local_rot1);
+    } else {
+      SetAttributeDefault(
+          data_,
+          CreateAttributeSpec(data_, joint_path,
+                              pxr::UsdPhysicsTokens->physicsLocalRot1,
+                              pxr::SdfValueTypeNames->Quatf),
+          pxr::GfQuatf::GetIdentity());
+    }
+
+    // Calculate local joint frame for body0
+    pxr::GfMatrix4d body1_transform_local =
+        MujocoPosQuatToTransform(&model_->body_pos[body1_id_usd * 3],
+                                 &model_->body_quat[body1_id_usd * 4]);
+    pxr::GfVec3d jnt_pos_parent_local =
+        body1_transform_local.Transform(mj_jnt_pos);
+    pxr::GfVec3d jnt_axis_parent_local =
+        body1_transform_local.TransformDir(mj_jnt_axis);
+
+    pxr::GfVec3f local_pos0(jnt_pos_parent_local);
+
+    SetAttributeDefault(
+        data_,
+        CreateAttributeSpec(data_, joint_path,
+                            pxr::UsdPhysicsTokens->physicsLocalPos0,
+                            pxr::SdfValueTypeNames->Float3),
+        local_pos0);
+
+    if (joint_prim_type == pxr::UsdPhysicsTokens->PhysicsRevoluteJoint ||
+        joint_prim_type == pxr::UsdPhysicsTokens->PhysicsPrismaticJoint) {
+      pxr::GfQuatf other_rot0(
+          pxr::GfRotation()
+              .SetRotateInto(pxr::GfVec3f::ZAxis(), jnt_axis_parent_local)
+              .GetQuat());
+
+      SetAttributeDefault(
+          data_,
+          CreateAttributeSpec(data_, joint_path,
+                              pxr::UsdPhysicsTokens->physicsLocalRot0,
+                              pxr::SdfValueTypeNames->Quatf),
+          other_rot0);
+    } else {
+      // Fixed joints have no frame and no axis per se. We simply need the
+      // rotation quaternion of the body its on.
+      SetAttributeDefault(
+          data_,
+          CreateAttributeSpec(data_, joint_path,
+                              pxr::UsdPhysicsTokens->physicsLocalRot0,
+                              pxr::SdfValueTypeNames->Quatf),
+          body1_transform_local.ExtractRotationQuat());
+    }
+
+    if (joint) {
+      mjtJoint type = (mjtJoint)model_->jnt_type[joint_id];
+
+      // Joint-specific attributes
+      if (type == mjJNT_HINGE || type == mjJNT_SLIDE) {
+        // The joint motion occurs around/along the Z-axis of the joint frame
+        // established by localRot0/1.
+        SetAttributeDefault(
+            data_,
+            CreateAttributeSpec(data_, joint_path,
+                                pxr::UsdPhysicsTokens->physicsAxis,
+                                pxr::SdfValueTypeNames->Token),
+            pxr::UsdPhysicsTokens->z);  // "Z" axis
+      }
+
+      if (model_->jnt_limited[joint_id]) {
+        float lower_limit = model_->jnt_range[joint_id * 2];
+        float upper_limit = model_->jnt_range[joint_id * 2 + 1];
+
+        if (type == mjJNT_HINGE) {
+          // Convert radians to degrees for USD
+          // As per the XML Reference, "mjModel always uses radians"
+          lower_limit *= (180.0 / mjPI);
+          upper_limit *= (180.0 / mjPI);
+          SetAttributeDefault(
+              data_,
+              CreateAttributeSpec(data_, joint_path,
+                                  pxr::UsdPhysicsTokens->physicsLowerLimit,
+                                  pxr::SdfValueTypeNames->Float),
+              lower_limit);
+          SetAttributeDefault(
+              data_,
+              CreateAttributeSpec(data_, joint_path,
+                                  pxr::UsdPhysicsTokens->physicsUpperLimit,
+                                  pxr::SdfValueTypeNames->Float),
+              upper_limit);
+        } else if (type == mjJNT_SLIDE) {
+          SetAttributeDefault(
+              data_,
+              CreateAttributeSpec(data_, joint_path,
+                                  pxr::UsdPhysicsTokens->physicsLowerLimit,
+                                  pxr::SdfValueTypeNames->Float),
+              lower_limit);
+          SetAttributeDefault(
+              data_,
+              CreateAttributeSpec(data_, joint_path,
+                                  pxr::UsdPhysicsTokens->physicsUpperLimit,
+                                  pxr::SdfValueTypeNames->Float),
+              upper_limit);
+        }
+      }
+    }
+  }
+
  void WriteCamera(mjsCamera *spec_cam, const mjsBody *body) {
    const auto &body_path = body_paths_[mjs_getId(body->element)];
    auto name = GetAvailablePrimName(*spec_cam->name,
@@ -1635,17 +1854,6 @@ class ModelWriter {

      ApplyApiSchema(data_, body_path,
                     pxr::UsdPhysicsTokens->PhysicsRigidBodyAPI);
-
-      // If the parent is not the world body, but is child of the world body
-      // then we need to apply the articulation root API.
-      if (parent_id != kWorldIndex) {
-        int parent_parent_id =
-            mjs_getId(mjs_getParent(parent->element)->element);
-        if (parent_parent_id == kWorldIndex) {
-          ApplyApiSchema(data_, parent_path,
-                         pxr::UsdPhysicsTokens->PhysicsArticulationRootAPI);
-        }
-      }
    }

    // Create classes if necessary
@@ -1692,6 +1900,7 @@ class ModelWriter {
      }
      WriteSites(body);
      WriteGeoms(body);
+      WriteJoints(body);
      WriteCameras(body);
      WriteLights(body);
      body = mjs_asBody(mjs_nextElement(spec_, body->element));
@@ -1707,6 +1916,14 @@ class ModelWriter {
        CreatePrimSpec(data_, pxr::SdfPath::AbsoluteRootPath(), name,
                       pxr::UsdGeomTokens->Xform);
    SetPrimKind(data_, world_group_path, pxr::KindTokens->group);
+
+    if (write_physics_) {
+      // Apply the PhysicsArticulationRootAPI to the world body so that
+      // everything under it is automatically considered an articulation.
+      ApplyApiSchema(data_, world_group_path,
+                     pxr::UsdPhysicsTokens->PhysicsArticulationRootAPI);
+    }
+
    return world_group_path;
  }
 };
@@ -25,6 +25,8 @@
 #include <mujoco/experimental/usd/mjcPhysics/tokens.h>
 #include "test/experimental/usd/test_utils.h"
 #include "test/fixture.h"
+#include <pxr/base/gf/quatf.h>
+#include <pxr/base/gf/rotation.h>
 #include <pxr/base/gf/vec2f.h>
 #include <pxr/base/gf/vec3d.h>
 #include <pxr/base/gf/vec3f.h>
@@ -56,8 +58,12 @@
 #include <pxr/usd/usdGeom/tokens.h>
 #include <pxr/usd/usdPhysics/articulationRootAPI.h>
 #include <pxr/usd/usdPhysics/collisionAPI.h>
+#include <pxr/usd/usdPhysics/fixedJoint.h>
+#include <pxr/usd/usdPhysics/joint.h>
 #include <pxr/usd/usdPhysics/massAPI.h>
 #include <pxr/usd/usdPhysics/meshCollisionAPI.h>
+#include <pxr/usd/usdPhysics/prismaticJoint.h>
+#include <pxr/usd/usdPhysics/revoluteJoint.h>
 #include <pxr/usd/usdPhysics/rigidBodyAPI.h>
 #include <pxr/usd/usdPhysics/scene.h>
 #include <pxr/usd/usdPhysics/tokens.h>
@@ -144,13 +150,11 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestMaterials) {
      "/mesh_test/Materials/material_red/PreviewSurface.inputs:diffuseColor",
      pxr::GfVec3f(0.8, 0, 0));
  ExpectAttributeHasConnection(
-    stage,
-    "/mesh_test/Materials/material_red.outputs:surface",
-    "/mesh_test/Materials/material_red/PreviewSurface.outputs:surface");
+      stage, "/mesh_test/Materials/material_red.outputs:surface",
+      "/mesh_test/Materials/material_red/PreviewSurface.outputs:surface");
  ExpectAttributeHasConnection(
-    stage,
-    "/mesh_test/Materials/material_red.outputs:displacement",
-    "/mesh_test/Materials/material_red/PreviewSurface.outputs:displacement");
+      stage, "/mesh_test/Materials/material_red.outputs:displacement",
+      "/mesh_test/Materials/material_red/PreviewSurface.outputs:displacement");

  EXPECT_PRIM_VALID(stage, "/mesh_test/Materials/material_texture");
  EXPECT_PRIM_VALID(stage,
@@ -1149,14 +1153,16 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsRigidBody) {
  EXPECT_PRIM_VALID(stage, "/physics_test/test_body");
  EXPECT_PRIM_VALID(stage, "/physics_test/test_body/test_body_2");

+  // Articulation root is applied to the root of the physics scene (worldbody).
+  EXPECT_PRIM_API_APPLIED(stage, "/physics_test",
+                          pxr::UsdPhysicsArticulationRootAPI);
+
  EXPECT_PRIM_API_APPLIED(stage, "/physics_test/test_body",
                          pxr::UsdPhysicsRigidBodyAPI);
-  EXPECT_PRIM_API_APPLIED(stage, "/physics_test/test_body",
-                          pxr::UsdPhysicsArticulationRootAPI);
+  EXPECT_PRIM_API_NOT_APPLIED(stage, "/physics_test/test_body",
+                              pxr::UsdPhysicsArticulationRootAPI);
  EXPECT_PRIM_API_APPLIED(stage, "/physics_test/test_body/test_body_2",
                          pxr::UsdPhysicsRigidBodyAPI);
-
-  // Only the root body should have the articulation API applied.
  EXPECT_PRIM_API_NOT_APPLIED(stage, "/physics_test/test_body/test_body_2",
                              pxr::UsdPhysicsArticulationRootAPI);

@@ -1547,6 +1553,346 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsSliderCrankActuator) {
  ExpectAttributeEqual(stage, "/test/body/crank.mjc:crankLength", 1.23);
 }

+TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsFloatingAndFixedBaseBody) {
+  static constexpr char kXml[] = R"(
+    <mujoco model="test">
+      <worldbody>
+        <body name="fixed_base">
+          <geom type="sphere" size="1"/>
+        </body>
+        <body name="floating_base">
+          <joint type="free"/>
+          <geom type="sphere" size="1"/>
+        </body>
+      </worldbody>
+    </mujoco>
+  )";
+
+  auto stage = OpenStageWithPhysics(kXml);
+  EXPECT_THAT(stage, testing::NotNull());
+
+  // Test that the fixed_base body has a UsdPhysicsJoint child connected to the
+  // worldbody.
+  EXPECT_PRIM_VALID(stage, "/test/fixed_base/FixedJoint");
+  auto joint = pxr::UsdPhysicsFixedJoint::Get(
+      stage, SdfPath("/test/fixed_base/FixedJoint"));
+  ASSERT_TRUE(joint);
+
+  // Initial joint to the worldbody does't set a body0 rel.
+  EXPECT_REL_TARGET_COUNT(stage, "/test/fixed_base/FixedJoint.physics:body0",
+                          0);
+  EXPECT_REL_HAS_TARGET(stage, "/test/fixed_base/FixedJoint.physics:body1",
+                        "/test/fixed_base");
+
+  // Test that the floating_base body has no UsdPhysicsJoint children.
+  auto floating_base = stage->GetPrimAtPath(SdfPath("/test/floating_base"));
+  ASSERT_TRUE(floating_base);
+  for (const auto& child : floating_base.GetChildren()) {
+    EXPECT_FALSE(child.IsA<pxr::UsdPhysicsJoint>());
+  }
+}
+
+TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsFixedJoint) {
+  static constexpr char kXml[] = R"(
+    <mujoco model="test">
+      <worldbody>
+        <body name="parent">
+          <geom type="sphere" size="1"/>
+          <body name="child" pos="1 0 0">
+            <geom type="sphere" size="1"/>
+            <body name="grandchild" pos="1 0 0">
+              <geom type="sphere" size="1"/>
+            </body>
+          </body>
+        </body>
+      </worldbody>
+    </mujoco>
+  )";
+
+  auto stage = OpenStageWithPhysics(kXml);
+  EXPECT_THAT(stage, testing::NotNull());
+
+  EXPECT_PRIM_IS_A(stage, "/test/parent/FixedJoint", pxr::UsdPhysicsFixedJoint);
+  // Initial joint to the worldbody does't set a body0 rel.
+  EXPECT_REL_TARGET_COUNT(stage, "/test/parent/FixedJoint.physics:body0", 0);
+  EXPECT_REL_HAS_TARGET(stage, "/test/parent/FixedJoint.physics:body1",
+                        "/test/parent");
+
+  EXPECT_PRIM_IS_A(stage, "/test/parent/child/FixedJoint",
+                   pxr::UsdPhysicsFixedJoint);
+  EXPECT_REL_HAS_TARGET(stage, "/test/parent/child/FixedJoint.physics:body0",
+                        "/test/parent");
+  EXPECT_REL_HAS_TARGET(stage, "/test/parent/child/FixedJoint.physics:body1",
+                        "/test/parent/child");
+
+  EXPECT_PRIM_IS_A(stage, "/test/parent/child/grandchild/FixedJoint",
+                   pxr::UsdPhysicsFixedJoint);
+  EXPECT_REL_HAS_TARGET(
+      stage, "/test/parent/child/grandchild/FixedJoint.physics:body0",
+      "/test/parent/child");
+  EXPECT_REL_HAS_TARGET(
+      stage, "/test/parent/child/grandchild/FixedJoint.physics:body1",
+      "/test/parent/child/grandchild");
+}
+
+TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsRevoluteJoint) {
+  static constexpr char kXml[] = R"(
+    <mujoco model="test">
+      <worldbody>
+        <body name="parent">
+          <joint name="hinge_root"/>
+          <geom type="sphere" size="1"/>
+          <body name="child0" pos="1 0 0">
+            <joint name="hinge_normal" type="hinge" axis="0 0 1"/>
+            <geom type="sphere" size="1"/>
+          </body>
+          <body name="child1" pos="1 0 0">
+            <joint name="hinge_limited" type="hinge" axis="0 0 1" limited="true" range="-30 45"/>
+            <geom type="sphere" size="1"/>
+          </body>
+        </body>
+      </worldbody>
+    </mujoco>
+  )";
+
+  auto stage = OpenStageWithPhysics(kXml);
+  EXPECT_THAT(stage, testing::NotNull());
+
+  // hinge_root doesn't set a type so it's the default: a revolute joint.
+  EXPECT_PRIM_IS_A(stage, "/test/parent/hinge_root",
+                   pxr::UsdPhysicsRevoluteJoint);
+  // Initial joint to the worldbody does't set a body0 rel.
+  EXPECT_REL_TARGET_COUNT(stage, "/test/parent/hinge_root.physics:body0", 0);
+  EXPECT_REL_HAS_TARGET(stage, "/test/parent/hinge_root.physics:body1",
+                        "/test/parent");
+
+  EXPECT_PRIM_IS_A(stage, "/test/parent/child0/hinge_normal",
+                   pxr::UsdPhysicsRevoluteJoint);
+  EXPECT_REL_HAS_TARGET(stage, "/test/parent/child0/hinge_normal.physics:body0",
+                        "/test/parent");
+  EXPECT_REL_HAS_TARGET(stage, "/test/parent/child0/hinge_normal.physics:body1",
+                        "/test/parent/child0");
+  ExpectAttributeEqual(stage, "/test/parent/child0/hinge_normal.physics:axis",
+                       pxr::UsdPhysicsTokens->z);
+  EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE(
+      stage, "/test/parent/child0/hinge_normal.physics:lowerLimit");
+  EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE(
+      stage, "/test/parent/child0/hinge_normal.physics:upperLimit");
+
+  EXPECT_PRIM_IS_A(stage, "/test/parent/child1/hinge_limited",
+                   pxr::UsdPhysicsRevoluteJoint);
+  EXPECT_REL_HAS_TARGET(
+      stage, "/test/parent/child1/hinge_limited.physics:body0", "/test/parent");
+  EXPECT_REL_HAS_TARGET(stage,
+                        "/test/parent/child1/hinge_limited.physics:body1",
+                        "/test/parent/child1");
+  ExpectAttributeEqual(stage, "/test/parent/child1/hinge_limited.physics:axis",
+                       pxr::UsdPhysicsTokens->z);
+  ExpectAttributeEqual(
+      stage, "/test/parent/child1/hinge_limited.physics:lowerLimit", -30.0f);
+  ExpectAttributeEqual(
+      stage, "/test/parent/child1/hinge_limited.physics:upperLimit", 45.0f);
+}
+
+TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsPrismaticJoint) {
+  static constexpr char kXml[] = R"(
+    <mujoco model="test">
+      <worldbody>
+        <body name="parent">
+          <joint name="slide_root" type="slide"/>
+          <geom type="sphere" size="1"/>
+          <body name="child0" pos="1 0 0">
+            <joint name="slide_normal" type="slide" axis="1 0 0"/>
+            <geom type="sphere" size="1"/>
+          </body>
+          <body name="child1" pos="1 0 0">
+            <joint name="slide_limited" type="slide" axis="1 0 0" limited="true" range="-2.5 2.5"/>
+            <geom type="sphere" size="1"/>
+          </body>
+        </body>
+      </worldbody>
+    </mujoco>
+  )";
+
+  auto stage = OpenStageWithPhysics(kXml);
+  EXPECT_THAT(stage, testing::NotNull());
+
+  EXPECT_PRIM_IS_A(stage, "/test/parent/slide_root",
+                   pxr::UsdPhysicsPrismaticJoint);
+  // Initial joint to the worldbody does't set a body0 rel.
+  EXPECT_REL_TARGET_COUNT(stage, "/test/parent/slide_root.physics:body0", 0);
+  EXPECT_REL_HAS_TARGET(stage, "/test/parent/slide_root.physics:body1",
+                        "/test/parent");
+
+  EXPECT_PRIM_IS_A(stage, "/test/parent/child0/slide_normal",
+                   pxr::UsdPhysicsPrismaticJoint);
+  EXPECT_REL_HAS_TARGET(stage, "/test/parent/child0/slide_normal.physics:body0",
+                        "/test/parent");
+  EXPECT_REL_HAS_TARGET(stage, "/test/parent/child0/slide_normal.physics:body1",
+                        "/test/parent/child0");
+  ExpectAttributeEqual(stage, "/test/parent/child0/slide_normal.physics:axis",
+                       pxr::UsdPhysicsTokens->z);
+  EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE(
+      stage, "/test/parent/child0/slide_normal.physics:lowerLimit");
+  EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE(
+      stage, "/test/parent/child0/slide_normal.physics:upperLimit");
+
+  EXPECT_PRIM_IS_A(stage, "/test/parent/child1/slide_limited",
+                   pxr::UsdPhysicsPrismaticJoint);
+  EXPECT_REL_HAS_TARGET(
+      stage, "/test/parent/child1/slide_limited.physics:body0", "/test/parent");
+  EXPECT_REL_HAS_TARGET(stage,
+                        "/test/parent/child1/slide_limited.physics:body1",
+                        "/test/parent/child1");
+  ExpectAttributeEqual(stage, "/test/parent/child1/slide_limited.physics:axis",
+                       pxr::UsdPhysicsTokens->z);
+  ExpectAttributeEqual(
+      stage, "/test/parent/child1/slide_limited.physics:lowerLimit", -2.5f);
+  ExpectAttributeEqual(
+      stage, "/test/parent/child1/slide_limited.physics:upperLimit", 2.5f);
+}
+
+TEST_F(MjcfSdfFileFormatPluginTest, TestRadianAnglesAreConvertedToDegrees) {
+  static constexpr char kXml[] = R"(
+    <mujoco model="test">
+      <compiler angle="radian"/>
+
+      <worldbody>
+        <body name="parent">
+          <joint name="hinge" type="hinge" axis="0 0 1" limited="true" range="-3.14159265359 0.78539816339"/>
+          <geom type="sphere" size="1"/>
+        </body>
+      </worldbody>
+    </mujoco>
+  )";
+
+  auto stage = OpenStageWithPhysics(kXml);
+  EXPECT_THAT(stage, testing::NotNull());
+
+  EXPECT_PRIM_VALID(stage, "/test/parent/hinge");
+  ExpectAttributeEqual(stage, "/test/parent/hinge.physics:lowerLimit", -180.0f);
+  ExpectAttributeEqual(stage, "/test/parent/hinge.physics:upperLimit", 45.0f);
+}
+
+TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsJointFrames) {
+  static constexpr char kXml[] = R"(
+    <mujoco model="test">
+      <worldbody>
+        <body name="parent" pos="0 1 0">
+          <body name="child0" pos="1 0 0">
+            <joint name="hinge" type="hinge" pos="0.1 0.2 0.3" axis="0 1 0"/>
+            <geom type="sphere" size="0.1"/>
+          </body>
+          <body name="child1" pos="2 3 4">
+            <joint name="slide" type="slide" pos="0.4 0.5 0.6" axis="-1 0 0"/>
+            <geom type="sphere" size="0.1"/>
+          </body>
+          <body name="child2" pos="5 6 7">
+            <joint name="slide_nonaxis" type="slide" pos="0.7 0.8 0.9" axis="1 1 1"/>
+            <geom type="sphere" size="0.1"/>
+          </body>
+        </body>
+      </worldbody>
+    </mujoco>
+  )";
+
+  auto stage = OpenStageWithPhysics(kXml);
+  EXPECT_THAT(stage, testing::NotNull());
+
+  // Test the hinge joint.
+  EXPECT_PRIM_VALID(stage, "/test/parent/child0/hinge");
+  auto hinge_joint = pxr::UsdPhysicsRevoluteJoint::Get(
+      stage, SdfPath("/test/parent/child0/hinge"));
+  ASSERT_TRUE(hinge_joint);
+
+  ExpectAttributeEqual(stage, "/test/parent/child0/hinge.physics:localPos0",
+                       pxr::GfVec3f(1.1, 0.2, 0.3));
+
+  pxr::GfRotation hinge_rot;
+  hinge_rot.SetRotateInto({0, 0, 1}, {0, 1, 0});
+  pxr::GfQuatf expected_hinge_rot(hinge_rot.GetQuat());
+
+  pxr::GfQuatf hinge_local_rot0;
+  hinge_joint.GetLocalRot0Attr().Get(&hinge_local_rot0);
+  EXPECT_TRUE(AreQuatsSameRotation(expected_hinge_rot, hinge_local_rot0));
+
+  ExpectAttributeEqual(stage, "/test/parent/child0/hinge.physics:localPos1",
+                       pxr::GfVec3f(0.1, 0.2, 0.3));
+
+  pxr::GfQuatf hinge_local_rot1;
+  hinge_joint.GetLocalRot1Attr().Get(&hinge_local_rot1);
+  EXPECT_TRUE(AreQuatsSameRotation(expected_hinge_rot, hinge_local_rot1));
+
+  // Test the slide joint.
+  EXPECT_PRIM_VALID(stage, "/test/parent/child1/slide");
+  auto slide_joint = pxr::UsdPhysicsPrismaticJoint::Get(
+      stage, SdfPath("/test/parent/child1/slide"));
+  ASSERT_TRUE(slide_joint);
+
+  ExpectAttributeEqual(stage, "/test/parent/child1/slide.physics:localPos0",
+                       pxr::GfVec3f(2.4, 3.5, 4.6));
+
+  pxr::GfRotation slide_rot;
+  slide_rot.SetRotateInto({0, 0, 1}, {-1, 0, 0});
+  pxr::GfQuatf expected_slide_rot(slide_rot.GetQuat());
+
+  pxr::GfQuatf slide_local_rot0;
+  slide_joint.GetLocalRot0Attr().Get(&slide_local_rot0);
+  EXPECT_TRUE(AreQuatsSameRotation(expected_slide_rot, slide_local_rot0));
+
+  ExpectAttributeEqual(stage, "/test/parent/child1/slide.physics:localPos1",
+                       pxr::GfVec3f(0.4, 0.5, 0.6));
+
+  pxr::GfQuatf slide_local_rot1;
+  slide_joint.GetLocalRot1Attr().Get(&slide_local_rot1);
+  EXPECT_TRUE(AreQuatsSameRotation(expected_slide_rot, slide_local_rot1));
+
+  // Test the slide_nonaxis joint.
+  EXPECT_PRIM_VALID(stage, "/test/parent/child2/slide_nonaxis");
+  auto slide_nonaxis_joint = pxr::UsdPhysicsPrismaticJoint::Get(
+      stage, SdfPath("/test/parent/child2/slide_nonaxis"));
+  ASSERT_TRUE(slide_nonaxis_joint);
+
+  ExpectAttributeEqual(stage,
+                       "/test/parent/child2/slide_nonaxis.physics:localPos0",
+                       pxr::GfVec3f(5.7, 6.8, 7.9));
+
+  pxr::GfRotation slide_nonaxis_rot;
+  slide_nonaxis_rot.SetRotateInto({0, 0, 1}, {1, 1, 1});
+  pxr::GfQuatf expected_slide_nonaxis_rot(slide_nonaxis_rot.GetQuat());
+
+  pxr::GfQuatf slide_nonaxis_local_rot0;
+  slide_nonaxis_joint.GetLocalRot0Attr().Get(&slide_nonaxis_local_rot0);
+  EXPECT_TRUE(AreQuatsSameRotation(expected_slide_nonaxis_rot,
+                                   slide_nonaxis_local_rot0));
+
+  ExpectAttributeEqual(stage,
+                       "/test/parent/child2/slide_nonaxis.physics:localPos1",
+                       pxr::GfVec3f(0.7, 0.8, 0.9));
+
+  pxr::GfQuatf slide_nonaxis_local_rot1;
+  slide_nonaxis_joint.GetLocalRot1Attr().Get(&slide_nonaxis_local_rot1);
+  EXPECT_TRUE(AreQuatsSameRotation(expected_slide_nonaxis_rot,
+                                   slide_nonaxis_local_rot1));
+}
+
+TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsUnsupportedJoint) {
+  static constexpr char kXml[] = R"(
+    <mujoco model="test">
+      <worldbody>
+        <body name="parent">
+          <joint type="ball" name="ball_joint"/>
+          <geom type="sphere" size="1"/>
+        </body>
+      </worldbody>
+    </mujoco>
+  )";
+
+  auto stage = OpenStageWithPhysics(kXml);
+  EXPECT_THAT(stage, testing::NotNull());
+
+  EXPECT_PRIM_INVALID(stage, "/test/parent/ball_joint");
+}
 }  // namespace
 }  // namespace usd
 }  // namespace mujoco
@@ -18,6 +18,8 @@

 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
+#include <pxr/base/gf/math.h>
+#include <pxr/base/gf/quatf.h>
 #include <pxr/base/tf/token.h>
 #include <pxr/usd/sdf/assetPath.h>
 #include <pxr/usd/sdf/childrenPolicies.h>
@@ -120,5 +122,20 @@ void ExpectAllAuthoredAttributesMatchSchemaTypes(const pxr::UsdPrim& prim) {
    }
  }
 }
+
+bool AreQuatsSameRotation(const pxr::GfQuatf& q1, const pxr::GfQuatf& q2,
+                          float tolerance) {
+  // The dot product of two unit quaternions (q1 and q2) is cos(theta), where
+  // theta is the angle between them on the 4D hypersphere.
+  //
+  // If q1 is close to q2, dot(q1, q2) is close to 1.
+  // If q1 is close to -q2, dot(q1, q2) is close to -1.
+  //
+  // By taking the absolute value of the dot product, we can check for
+  // closeness to 1 to see if the quaternions are collinear, which is what
+  // we want. This works for both cases.
+  const float dot = pxr::GfDot(q1, q2);
+  return pxr::GfIsClose(pxr::GfAbs(dot), 1.0f, tolerance);
+}
 }  // namespace usd
 }  // namespace mujoco
@@ -18,6 +18,7 @@
 #include <string>

 #include <gtest/gtest.h>
+#include <pxr/base/gf/quatf.h>
 #include <pxr/usd/sdf/assetPath.h>
 #include <pxr/usd/sdf/declareHandles.h>
 #include <pxr/usd/sdf/fileFormat.h>
@@ -29,6 +30,9 @@
 #define EXPECT_PRIM_VALID(stage, path) \
  EXPECT_TRUE((stage)->GetPrimAtPath(SdfPath(path)).IsValid());

+#define EXPECT_PRIM_INVALID(stage, path) \
+  EXPECT_FALSE((stage)->GetPrimAtPath(SdfPath(path)).IsValid());
+
 #define EXPECT_PRIM_IS_A(stage, path, type)                         \
  {                                                                 \
    EXPECT_TRUE((stage)->GetPrimAtPath(SdfPath(path)).IsA<type>()); \
@@ -66,6 +70,19 @@
 #define EXPECT_ATTRIBUTE_HAS_NO_VALUE(stage, path) \
  EXPECT_FALSE((stage)->GetAttributeAtPath(SdfPath(path)).HasValue());

+#define EXPECT_ATTRIBUTE_HAS_AUTHORED_VALUE(stage, path) \
+  EXPECT_TRUE((stage)->GetAttributeAtPath(SdfPath(path)).HasAuthoredValue());
+
+#define EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE(stage, path) \
+  EXPECT_FALSE((stage)->GetAttributeAtPath(SdfPath(path)).HasAuthoredValue());
+
+#define EXPECT_REL_TARGET_COUNT(stage, path, count)                     \
+  {                                                                     \
+    pxr::SdfPathVector targets;                                         \
+    (stage)->GetRelationshipAtPath(SdfPath(path)).GetTargets(&targets); \
+    EXPECT_TRUE(targets.size() == count);                               \
+  }
+
 #define EXPECT_REL_HAS_TARGET(stage, path, target_path)                 \
  {                                                                     \
    pxr::SdfPathVector targets;                                         \
@@ -115,6 +132,10 @@ void ExpectAttributeHasConnection(pxr::UsdStageRefPtr stage, const char* path,
 // Checks that all authored attributes on the given prim have types that match
 // the schema types.
 void ExpectAllAuthoredAttributesMatchSchemaTypes(const pxr::UsdPrim& prim);
+
+// Checks if two quaternions represent the same rotation.
+bool AreQuatsSameRotation(const pxr::GfQuatf& q1, const pxr::GfQuatf& q2,
+                          float tolerance = 1e-6);
 }  // namespace usd
 }  // namespace mujoco
 #endif  // MUJOCO_TEST_EXPERIMENTAL_USD_PLUGINS_MJCF_FIXTURE_H_