From b25f8433fe765b54745d33f44ad8ebbd002c6e9a Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Wed, 4 Mar 2026 08:33:13 -0800 Subject: [PATCH] Refactor USD connect and weld equality decoding to align with MuJoCo's relpose semantics. The relpose in mujoco represents the anchor in body1 space and the relative orientation of body2 in body1 space. Previously we were computing it as the relative position of body2 from body1 space. Further, we now properly account for the setting of strictly the anchor and not the relpos by setting localPos0 *only*. PiperOrigin-RevId: 878506356 Change-Id: I83fc7e97cd1a891e17f0bb091f8950b581be0e01 --- plugin/usd_decoder/usd_decoder.cc | 145 +++++++++++++++--------------- 1 file changed, 72 insertions(+), 73 deletions(-) diff --git a/plugin/usd_decoder/usd_decoder.cc b/plugin/usd_decoder/usd_decoder.cc index ebf2bb5d..015e9bd7 100644 --- a/plugin/usd_decoder/usd_decoder.cc +++ b/plugin/usd_decoder/usd_decoder.cc @@ -985,8 +985,7 @@ void ParseMjcPhysicsTendon(mjSpec* spec, const pxr::MjcPhysicsTendon& tendon) { if (!segments.empty() && segments.size() != n_targets) { mju_warning( "Spatial tendon %s has %lu segments but %lu wrap targets, skipping.", - prim.GetPath().GetAsString().c_str(), segments.size(), - n_targets); + prim.GetPath().GetAsString().c_str(), segments.size(), n_targets); return; } // Check that if we have >1 segments that the user has specified how much @@ -1000,8 +999,7 @@ void ParseMjcPhysicsTendon(mjSpec* spec, const pxr::MjcPhysicsTendon& tendon) { return; } // Check that if we side site indices that we have N of them. - if (!side_site_indices.empty() && - side_site_indices.size() != n_targets) { + if (!side_site_indices.empty() && side_site_indices.size() != n_targets) { mju_warning( "Spatial tendon %s has %lu sideSite indices but %lu wrap targets, " "skipping.", @@ -1022,8 +1020,7 @@ void ParseMjcPhysicsTendon(mjSpec* spec, const pxr::MjcPhysicsTendon& tendon) { if (!coefs.empty() && coefs.size() != n_targets) { mju_warning( "Spatial tendon %s has %lu coefs but %lu wrap targets, skipping.", - prim.GetPath().GetAsString().c_str(), coefs.size(), - n_targets); + prim.GetPath().GetAsString().c_str(), coefs.size(), n_targets); } } @@ -1867,7 +1864,7 @@ void ParseMjcEqualityAPISolverParams( } void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, - pxr::UsdGeomXformCache& xform_cache) { + pxr::UsdGeomXformCache& xform_cache) { if (prim.HasAPI()) { // Handle MjcPhysicsEqualityJointAPI on revolute/prismatic joints. pxr::MjcPhysicsEqualityJointAPI eq_joint_api(prim); @@ -1953,19 +1950,15 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, // In USD, joints have a reference frame that is shared between the two // connecting bodies. This reference frame is defined relative to both - // bodies, in localPos/Rot 0 and 1. A fixed joint removes all degrees of - // freedom for the joint, ensuring the reference frame is fixed in place - // This means the bodies should also be fixed, but relative to the joint - // depending on their respective localPos/Rot. - // In MuJoCo fixed joint frames are not explicitly defined relative to their - // connecting bodies. Instead, we define a weld constraint providing the - // weld point (anchor) relative to body 2 and then we specify the position - // of body 2 relative to body 1. + // bodies, in localPos/Rot 0 and 1. + // In MuJoCo this is specified as an equality constraint, with the + // anchor point defined relative to body2 (USD body1) and + // the relpose attribute defines the position of the anchor relative to + // body1 (USD body0) and the orientation of body2 relative to body1. - // Here is the mapping of terms concretely: - // T(bodyX) = transform of bodyX relative to joint frame (localPos/Rot). - // anchor = localPos1 (position of the weld point relative to mjc body 2) - // relpose = T(body0)*T(body1)^-1 + // Concretely: + // anchor = localPos1 + // relpose = (localPos0, localRot0 * localRot1.GetConjugate()) // relpose is float[7], pos(3) + quat(4) // anchor is float[3], pos(3) @@ -1982,74 +1975,80 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, if (!body0_xform.Factor(&scale_orient, &body0_scale, &rot, &translation, &persp)) { // unable to decompose, emit warning and set scale to identity - mju_warning( - "Unable to decompose matrix for body 0: %s.", - body0_path.GetAsString().c_str()); + mju_warning("Unable to decompose matrix for body 0: %s.", + body0_path.GetAsString().c_str()); body0_scale = pxr::GfVec3f(1, 1, 1); } - if (!body1_xform.Factor(&scale_orient, &body1_scale, &rot, - &translation, &persp)) { + if (!body1_xform.Factor(&scale_orient, &body1_scale, &rot, &translation, + &persp)) { // unable to decompose, emit warning and set scale to identity - mju_warning( - "Unable to decompose matrix for body 1: %s.", - body1_path.GetAsString().c_str()); + mju_warning("Unable to decompose matrix for body 1: %s.", + body1_path.GetAsString().c_str()); body1_scale = pxr::GfVec3f(1, 1, 1); - } + } } - pxr::GfVec3f localPos1; - joint.GetLocalPos1Attr().Get(&localPos1); - localPos1[0] *= body1_scale[0]; - localPos1[1] *= body1_scale[1]; - localPos1[2] *= body1_scale[2]; - pxr::GfQuatf localRot1; - joint.GetLocalRot1Attr().Get(&localRot1); + pxr::GfVec3f localPos0; + joint.GetLocalPos0Attr().Get(&localPos0); + localPos0[0] *= body0_scale[0]; + localPos0[1] *= body0_scale[1]; + localPos0[2] *= body0_scale[2]; - pxr::GfVec3f localPos0; - joint.GetLocalPos0Attr().Get(&localPos0); - localPos0[0] *= body0_scale[0]; - localPos0[1] *= body0_scale[1]; - localPos0[2] *= body0_scale[2]; - pxr::GfQuatf localRot0; - joint.GetLocalRot0Attr().Get(&localRot0); + pxr::GfVec3f localPos1; + joint.GetLocalPos1Attr().Get(&localPos1); + localPos1[0] *= body1_scale[0]; + localPos1[1] *= body1_scale[1]; + localPos1[2] *= body1_scale[2]; - auto relpose_quat = localRot0 * localRot1.GetConjugate(); - relpose_quat.Normalize(); - auto relpose_pos = localPos0 - relpose_quat.Transform(localPos1); + pxr::GfQuatf localRot0, localRot1; + joint.GetLocalRot0Attr().Get(&localRot0); + joint.GetLocalRot1Attr().Get(&localRot1); - eq->data[0] = localPos1[0]; - eq->data[1] = localPos1[1]; - eq->data[2] = localPos1[2]; - eq->data[3] = relpose_pos[0]; - eq->data[4] = relpose_pos[1]; - eq->data[5] = relpose_pos[2]; - eq->data[6] = relpose_quat.GetReal(); - eq->data[7] = relpose_quat.GetImaginary()[0]; - eq->data[8] = relpose_quat.GetImaginary()[1]; - eq->data[9] = relpose_quat.GetImaginary()[2]; + auto relpose_quat = localRot0 * localRot1.GetConjugate(); + relpose_quat.Normalize(); - if (prim.HasAPI()) { - eq->type = mjEQ_CONNECT; - pxr::MjcPhysicsEqualityAPI equality_api(prim); - ParseMjcEqualityAPISolverParams(eq, equality_api, prim); - } else if (prim.HasAPI()) { - pxr::MjcPhysicsEqualityAPI equality_api(prim); - ParseMjcEqualityAPISolverParams(eq, equality_api, prim); + if (prim.HasAPI()) { + eq->type = mjEQ_CONNECT; + eq->data[0] = localPos0[0]; + eq->data[1] = localPos0[1]; + eq->data[2] = localPos0[2]; - pxr::MjcPhysicsEqualityWeldAPI weld_api(prim); - auto torque_scale_attr = weld_api.GetTorqueScaleAttr(); - if (torque_scale_attr.HasAuthoredValue()) { - float torque_scale; - torque_scale_attr.Get(&torque_scale); - eq->data[10] = torque_scale; + pxr::MjcPhysicsEqualityAPI equality_api(prim); + ParseMjcEqualityAPISolverParams(eq, equality_api, prim); + } else { + eq->data[0] = localPos1[0]; + eq->data[1] = localPos1[1]; + eq->data[2] = localPos1[2]; + + bool has_relpose = joint.GetLocalPos0Attr().HasAuthoredValue() || + joint.GetLocalRot0Attr().HasAuthoredValue() || + joint.GetLocalRot1Attr().HasAuthoredValue(); + + if (has_relpose) { + eq->data[3] = localPos0[0]; + eq->data[4] = localPos0[1]; + eq->data[5] = localPos0[2]; + eq->data[6] = relpose_quat.GetReal(); + eq->data[7] = relpose_quat.GetImaginary()[0]; + eq->data[8] = relpose_quat.GetImaginary()[1]; + eq->data[9] = relpose_quat.GetImaginary()[2]; + } + + if (prim.HasAPI()) { + pxr::MjcPhysicsEqualityAPI equality_api(prim); + ParseMjcEqualityAPISolverParams(eq, equality_api, prim); + + pxr::MjcPhysicsEqualityWeldAPI weld_api(prim); + float torque_scale; + if (weld_api.GetTorqueScaleAttr().Get(&torque_scale)) { + eq->data[10] = torque_scale; + } + } } } - } else { - mju_warning("Constraint %s is not a supported constraint type, skipping.", - prim.GetPath().GetAsString().c_str()); - } } + void ParseUsdPhysicsJoint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, pxr::UsdGeomXformCache& xform_cache) { // A fixed joint means the bodies are welded. @@ -2324,7 +2323,7 @@ void PopulateSpecFromTree(pxr::UsdStageRefPtr stage, mjSpec* spec, for (const auto& constraint_path : current_node->constraints) { ParseConstraint(spec, stage->GetPrimAtPath(constraint_path), - current_mj_body, caches.xform_cache); + current_mj_body, caches.xform_cache); } pxr::UsdPrim body_prim_for_xform =