mj_usd plugin: add joints export.

Note that ball joint is missing, will come later.

PiperOrigin-RevId: 776057523
Change-Id: I8d13c3ea9e7f3d894af73a4640eb93202f8a2a03

test/experimental/usd/test_utils.cc

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