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 =