chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactor USD connect and weld equality decoding to align with MuJoCo's relpose semantics.

Browse Source 
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
This commit is contained in:
Sam Haves
2026-03-04 08:33:13 -08:00
committed by Copybara-Service
parent 8ddb5ff97b
commit b25f8433fe
1 changed files with 72 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
plugin/usd_decoder/usd_decoder.cc
+72 -73
View File
@@ -985,8 +985,7 @@ void ParseMjcPhysicsTendon(mjSpec* spec, const pxr::MjcPhysicsTendon& tendon) {
if (!segments.empty() && segments.size() != n_targets) { if (!segments.empty() && segments.size() != n_targets) {
mju_warning( mju_warning(
"Spatial tendon %s has %lu segments but %lu wrap targets, skipping.", "Spatial tendon %s has %lu segments but %lu wrap targets, skipping.",
prim.GetPath().GetAsString().c_str(), segments.size(), prim.GetPath().GetAsString().c_str(), segments.size(), n_targets);
n_targets);
return; return;
} }
// Check that if we have >1 segments that the user has specified how much // 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; return;
} }
// Check that if we side site indices that we have N of them. // Check that if we side site indices that we have N of them.
if (!side_site_indices.empty() && if (!side_site_indices.empty() && side_site_indices.size() != n_targets) {
side_site_indices.size() != n_targets) {
mju_warning( mju_warning(
"Spatial tendon %s has %lu sideSite indices but %lu wrap targets, " "Spatial tendon %s has %lu sideSite indices but %lu wrap targets, "
"skipping.", "skipping.",
@@ -1022,8 +1020,7 @@ void ParseMjcPhysicsTendon(mjSpec* spec, const pxr::MjcPhysicsTendon& tendon) {
if (!coefs.empty() && coefs.size() != n_targets) { if (!coefs.empty() && coefs.size() != n_targets) {
mju_warning( mju_warning(
"Spatial tendon %s has %lu coefs but %lu wrap targets, skipping.", "Spatial tendon %s has %lu coefs but %lu wrap targets, skipping.",
prim.GetPath().GetAsString().c_str(), coefs.size(), prim.GetPath().GetAsString().c_str(), coefs.size(), n_targets);
n_targets);
} }
} }
@@ -1867,7 +1864,7 @@ void ParseMjcEqualityAPISolverParams(
} }
void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body,
pxr::UsdGeomXformCache& xform_cache) { pxr::UsdGeomXformCache& xform_cache) {
if (prim.HasAPI<pxr::MjcPhysicsEqualityJointAPI>()) { if (prim.HasAPI<pxr::MjcPhysicsEqualityJointAPI>()) {
// Handle MjcPhysicsEqualityJointAPI on revolute/prismatic joints. // Handle MjcPhysicsEqualityJointAPI on revolute/prismatic joints.
pxr::MjcPhysicsEqualityJointAPI eq_joint_api(prim); 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 // In USD, joints have a reference frame that is shared between the two
// connecting bodies. This reference frame is defined relative to both // connecting bodies. This reference frame is defined relative to both
// bodies, in localPos/Rot 0 and 1. A fixed joint removes all degrees of // bodies, in localPos/Rot 0 and 1.
// freedom for the joint, ensuring the reference frame is fixed in place // In MuJoCo this is specified as an equality constraint, with the
// This means the bodies should also be fixed, but relative to the joint // anchor point defined relative to body2 (USD body1) and
// depending on their respective localPos/Rot. // the relpose attribute defines the position of the anchor relative to
// In MuJoCo fixed joint frames are not explicitly defined relative to their // body1 (USD body0) and the orientation of body2 relative to body1.
// 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.
// Here is the mapping of terms concretely: // Concretely:
// T(bodyX) = transform of bodyX relative to joint frame (localPos/Rot). // anchor = localPos1
// anchor = localPos1 (position of the weld point relative to mjc body 2) // relpose = (localPos0, localRot0 * localRot1.GetConjugate())
// relpose = T(body0)*T(body1)^-1
// relpose is float[7], pos(3) + quat(4) // relpose is float[7], pos(3) + quat(4)
// anchor is float[3], pos(3) // 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, if (!body0_xform.Factor(&scale_orient, &body0_scale, &rot, &translation,
&persp)) { &persp)) {
// unable to decompose, emit warning and set scale to identity // unable to decompose, emit warning and set scale to identity
mju_warning( mju_warning("Unable to decompose matrix for body 0: %s.",
"Unable to decompose matrix for body 0: %s.", body0_path.GetAsString().c_str());
body0_path.GetAsString().c_str());
body0_scale = pxr::GfVec3f(1, 1, 1); body0_scale = pxr::GfVec3f(1, 1, 1);
} }
if (!body1_xform.Factor(&scale_orient, &body1_scale, &rot, if (!body1_xform.Factor(&scale_orient, &body1_scale, &rot, &translation,
&translation, &persp)) { &persp)) {
// unable to decompose, emit warning and set scale to identity // unable to decompose, emit warning and set scale to identity
mju_warning( mju_warning("Unable to decompose matrix for body 1: %s.",
"Unable to decompose matrix for body 1: %s.", body1_path.GetAsString().c_str());
body1_path.GetAsString().c_str());
body1_scale = pxr::GfVec3f(1, 1, 1); body1_scale = pxr::GfVec3f(1, 1, 1);
} }
} }
pxr::GfVec3f localPos1; pxr::GfVec3f localPos0;
joint.GetLocalPos1Attr().Get(&localPos1); joint.GetLocalPos0Attr().Get(&localPos0);
localPos1[0] *= body1_scale[0]; localPos0[0] *= body0_scale[0];
localPos1[1] *= body1_scale[1]; localPos0[1] *= body0_scale[1];
localPos1[2] *= body1_scale[2]; localPos0[2] *= body0_scale[2];
pxr::GfQuatf localRot1;
joint.GetLocalRot1Attr().Get(&localRot1);
pxr::GfVec3f localPos0; pxr::GfVec3f localPos1;
joint.GetLocalPos0Attr().Get(&localPos0); joint.GetLocalPos1Attr().Get(&localPos1);
localPos0[0] *= body0_scale[0]; localPos1[0] *= body1_scale[0];
localPos0[1] *= body0_scale[1]; localPos1[1] *= body1_scale[1];
localPos0[2] *= body0_scale[2]; localPos1[2] *= body1_scale[2];
pxr::GfQuatf localRot0;
joint.GetLocalRot0Attr().Get(&localRot0);
auto relpose_quat = localRot0 * localRot1.GetConjugate(); pxr::GfQuatf localRot0, localRot1;
relpose_quat.Normalize(); joint.GetLocalRot0Attr().Get(&localRot0);
auto relpose_pos = localPos0 - relpose_quat.Transform(localPos1); joint.GetLocalRot1Attr().Get(&localRot1);
eq->data[0] = localPos1[0]; auto relpose_quat = localRot0 * localRot1.GetConjugate();
eq->data[1] = localPos1[1]; relpose_quat.Normalize();
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];
if (prim.HasAPI<pxr::MjcPhysicsEqualityConnectAPI>()) { if (prim.HasAPI<pxr::MjcPhysicsEqualityConnectAPI>()) {
eq->type = mjEQ_CONNECT; eq->type = mjEQ_CONNECT;
pxr::MjcPhysicsEqualityAPI equality_api(prim); eq->data[0] = localPos0[0];
ParseMjcEqualityAPISolverParams(eq, equality_api, prim); eq->data[1] = localPos0[1];
} else if (prim.HasAPI<pxr::MjcPhysicsEqualityWeldAPI>()) { eq->data[2] = localPos0[2];
pxr::MjcPhysicsEqualityAPI equality_api(prim);
ParseMjcEqualityAPISolverParams(eq, equality_api, prim);
pxr::MjcPhysicsEqualityWeldAPI weld_api(prim); pxr::MjcPhysicsEqualityAPI equality_api(prim);
auto torque_scale_attr = weld_api.GetTorqueScaleAttr(); ParseMjcEqualityAPISolverParams(eq, equality_api, prim);
if (torque_scale_attr.HasAuthoredValue()) { } else {
float torque_scale; eq->data[0] = localPos1[0];
torque_scale_attr.Get(&torque_scale); eq->data[1] = localPos1[1];
eq->data[10] = torque_scale; 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::MjcPhysicsEqualityWeldAPI>()) {
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, void ParseUsdPhysicsJoint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body,
pxr::UsdGeomXformCache& xform_cache) { pxr::UsdGeomXformCache& xform_cache) {
// A fixed joint means the bodies are welded. // 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) { for (const auto& constraint_path : current_node->constraints) {
ParseConstraint(spec, stage->GetPrimAtPath(constraint_path), ParseConstraint(spec, stage->GetPrimAtPath(constraint_path),
current_mj_body, caches.xform_cache); current_mj_body, caches.xform_cache);
} }
pxr::UsdPrim body_prim_for_xform = pxr::UsdPrim body_prim_for_xform =