chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 10 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) {
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<pxr::MjcPhysicsEqualityJointAPI>()) {
// 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<pxr::MjcPhysicsEqualityConnectAPI>()) {
eq->type = mjEQ_CONNECT;
pxr::MjcPhysicsEqualityAPI equality_api(prim);
ParseMjcEqualityAPISolverParams(eq, equality_api, prim);
} else if (prim.HasAPI<pxr::MjcPhysicsEqualityWeldAPI>()) {
pxr::MjcPhysicsEqualityAPI equality_api(prim);
ParseMjcEqualityAPISolverParams(eq, equality_api, prim);
if (prim.HasAPI<pxr::MjcPhysicsEqualityConnectAPI>()) {
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::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,
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 =