From 3441e0aa0379281a1cb20d7c55b3c3332b6310b6 Mon Sep 17 00:00:00 2001 From: Robin Alazard Date: Thu, 26 Jun 2025 03:01:31 -0700 Subject: [PATCH] mj_usd plugin: add joints export. Note that ball joint is missing, will come later. PiperOrigin-RevId: 776057523 Change-Id: I8d13c3ea9e7f3d894af73a4640eb93202f8a2a03 --- .../usd/plugins/mjcf/mujoco_to_usd.cc | 247 +++++++++++- .../usd/plugins/mjcf/mjcf_file_format_test.cc | 366 +++++++++++++++++- test/experimental/usd/test_utils.cc | 17 + test/experimental/usd/test_utils.h | 21 + 4 files changed, 626 insertions(+), 25 deletions(-) diff --git a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc index 0f50b62a..c17df766 100644 --- a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc +++ b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc @@ -21,9 +21,9 @@ #include #include +#include #include #include -#include #include "mjcf/utils.h" #include #include @@ -55,6 +55,11 @@ #include #include #include +#include +#include +#include +#include +#include #include #include #include @@ -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; } }; diff --git a/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc b/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc index a1cdbbda..ddb7a60c 100644 --- a/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc +++ b/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc @@ -25,6 +25,8 @@ #include #include "test/experimental/usd/test_utils.h" #include "test/fixture.h" +#include +#include #include #include #include @@ -56,8 +58,12 @@ #include #include #include +#include +#include #include #include +#include +#include #include #include #include @@ -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"( + + + + + + + + + + + + )"; + + 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()); + } +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsFixedJoint) { + static constexpr char kXml[] = R"( + + + + + + + + + + + + + + )"; + + 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"( + + + + + + + + + + + + + + + + + )"; + + 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"( + + + + + + + + + + + + + + + + + )"; + + 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"( + + + + + + + + + + + )"; + + 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"( + + + + + + + + + + + + + + + + + + + )"; + + 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"( + + + + + + + + + )"; + + auto stage = OpenStageWithPhysics(kXml); + EXPECT_THAT(stage, testing::NotNull()); + + EXPECT_PRIM_INVALID(stage, "/test/parent/ball_joint"); +} } // namespace } // namespace usd } // namespace mujoco diff --git a/test/experimental/usd/test_utils.cc b/test/experimental/usd/test_utils.cc index 165f82ce..eb008a68 100644 --- a/test/experimental/usd/test_utils.cc +++ b/test/experimental/usd/test_utils.cc @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include #include @@ -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 diff --git a/test/experimental/usd/test_utils.h b/test/experimental/usd/test_utils.h index 2aa62a6c..46abaec2 100644 --- a/test/experimental/usd/test_utils.h +++ b/test/experimental/usd/test_utils.h @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -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()); \ @@ -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_